summaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/body.cpp6
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;