Fix gravity in Body::F

1 files changed, 2 insertions, 4 deletions

diff --git a/src/body.cpp b/src/body.cpp
index 5579e4d..1aafd54 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -262,7 +262,7 @@ Body::F ( const Vector3d &ang, const Quaterniond &q, double dt )
     v1 = X(3);
     v2 = X(4);
     v3 = X(5);
-    double gw = gravity_world[2];
+    double gw = -gravity_world[2];
     F.block<1,4>(0,6) << 2*qbw1*v1 + 2*qbw2*v2 + 2*qbw3*v3, 
                          2*qbw1*v2 - 2*qbw2*v1 + 2*qbw4*v3, 
                          2*qbw1*v3 - 2*qbw3*v1 - 2*qbw4*v2, 
@@ -279,10 +279,8 @@ Body::F ( const Vector3d &ang, const Quaterniond &q, double dt )
                          -2*gw*qbw4, -2*gw*qbw3, -2*gw*qbw2, -2*gw*qbw1,
                           2*gw*qbw1,  2*gw*qbw2, -2*gw*qbw3, -2*gw*qbw4;
     F.block<4,4>(6,6) =  0.5*omega(ang);
-    F.block<3,3>(3,10) = -Matrix<double,3,3>::Identity();
-#else
-    F.block<3,3>(3,6) = -Matrix<double,3,3>::Identity();
 #endif
+    F.block<3,3>(3,STATESIZE-3) = -Matrix<double,3,3>::Identity();
     F *= dt;
     F += Matrix<double,STATESIZE,STATESIZE>::Identity();
     return F;