Add compile time flags.

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  ----- */