diff options
Diffstat (limited to 'src/state.cpp')
| -rw-r--r-- | src/state.cpp | 7 |
1 files changed, 4 insertions, 3 deletions
diff --git a/src/state.cpp b/src/state.cpp index 5253425..dfe41fa 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -553,9 +553,6 @@ State::F ( const Vector3d &w, double dt ) State::F ( const Quaterniond &q, const Vector3d &w, double dt ) #endif { -#if STATESIZE==13 - Quaterniond q = body->qhat(); -#endif Vector3d v; v = body->vel(); // Allocate matrix F @@ -564,7 +561,11 @@ State::F ( const Quaterniond &q, const Vector3d &w, double dt ) f = MatrixXd::Zero(rows,rows); // Set body F +#if STATESIZE==13 + f.topLeftCorner<STATESIZE,STATESIZE>() = body->F(w,dt); +#else f.topLeftCorner<STATESIZE,STATESIZE>() = body->F(w,q,dt); +#endif // Set Fxi Fyi { // limit i's scope auto i = features.begin(); |