diff options
| author | Martin Miller | 2017-04-04 13:59:03 -0500 |
|---|---|---|
| committer | Martin Miller | 2017-04-04 13:59:03 -0500 |
| commit | 623f94f85a49f1b720a025c680769ba2ba028ba0 (patch) | |
| tree | 4d5c45ec1d67fde8fbdc4c4ad579a96528b8aa5d /src/state.cpp | |
| parent | ac813cce202542e05f24c4e73af4381157aaf3c2 (diff) | |
| download | refslam-623f94f85a49f1b720a025c680769ba2ba028ba0.zip refslam-623f94f85a49f1b720a025c680769ba2ba028ba0.tar.gz | |
Update Body to use STATESIZE
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(); |