diff options
Diffstat (limited to 'src/body.cpp')
| -rw-r--r-- | src/body.cpp | 6 |
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; |