diff options
Diffstat (limited to 'src/body.cpp')
| -rw-r--r-- | src/body.cpp | 23 |
1 files changed, 15 insertions, 8 deletions
diff --git a/src/body.cpp b/src/body.cpp index 1e1f57d..1b07361 100644 --- a/src/body.cpp +++ b/src/body.cpp @@ -17,6 +17,19 @@ */ #include "body.h" +void +Body::clamp() +{ +#ifdef DOCLAMP + // Constrain the height + if (X[2]<MAXHEIGHT) { + X[2]=MAXHEIGHT; + } else if (X[2]>MINHEIGHT) { + X[2]=MINHEIGHT; + } +#endif /* ----- DOCLAMP ----- */ +} + /* *-------------------------------------------------------------------------------------- * Class: Body @@ -28,12 +41,7 @@ void Body::dx ( const Matrix<double,9,1> &del ) { X += del; - // Constrain the height - if (X[2]<-1.5) { - X[2]=-1.; - } else if (X[2]>-0.3) { - X[2]=-0.3; - } + clamp(); return ; } /* ----- end of method Body::dx ----- */ @@ -195,8 +203,7 @@ Body::motionModel ( const Vector3d &acc, const Vector3d &ang, const Quaterniond b.segment<3>(3) = acc-bias+Rbw.transpose()*gravity_world; X.segment<6>(0) += (A*X.segment<3>(3)+b)*dt; - if (X[2]>-0.3) X[2]=-0.3; - if (X[2]<-1.3) X[2]=-1.3; + clamp(); return ; } /* ----- end of method Body::motionModel ----- */ |