From 623f94f85a49f1b720a025c680769ba2ba028ba0 Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Tue, 4 Apr 2017 13:59:03 -0500
Subject: Update Body to use STATESIZE

---
 src/state.cpp | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

(limited to 'src/state.cpp')

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();
-- 
cgit v1.1