Port over remaining Point3D usage to the more type and unit safe SG* classes.

[flightgear.git] / src / FDM / YASim / RigidBody.cpp

diff --git a/src/FDM/YASim/RigidBody.cpp b/src/FDM/YASim/RigidBody.cpp
index 145cfc1055d820096672d4982d8630e5d49eedf4..7f530305b2c42bd4723e56c798ca8289696805f7 100644 (file)
--- a/src/FDM/YASim/RigidBody.cpp
+++ b/src/FDM/YASim/RigidBody.cpp
@@ -172,8 +172,9 @@ void RigidBody::getAccel(float* pos, float* accelOut)
     float a[3];
     float rate = Math::mag3(_spin);
     Math::set3(_spin, a);
-    Math::mul3(1/rate, a, a);
-
+    if (rate !=0 )
+        Math::mul3(1/rate, a, a);
+    //an else branch is not neccesary. a, which is a=(0,0,0) in the else case, is only used in a dot product
     float v[3];
     Math::sub3(_cg, pos, v);             // v = cg - pos
     Math::mul3(Math::dot3(v, a), a, a);  // a = a * (v dot a)
@@ -203,4 +204,14 @@ void RigidBody::getAngularAccel(float* accelOut)
     Math::vmul33(_invI, v, v);   // v = invI*(tau + (omega X I*omega))
 }
 
+void RigidBody::getInertiaMatrix(float* inertiaOut)
+{
+    // valid only after a call to RigidBody::recalc()
+    // See comment at top of RigidBody.hpp on units.
+    for(int i=0;i<9;i++)
+    {
+       inertiaOut[i] = _tI[i];
+    }
+}
+
 }; // namespace yasim