From 05f9fcfd1079db97155321fad35a80fd36703e5f Mon Sep 17 00:00:00 2001
From: Henning Stahlke <github@henningstahlke.de>
Date: Wed, 8 Feb 2017 23:51:38 +0100
Subject: [PATCH] comments

---
 src/FDM/YASim/Airplane.cpp  |  2 ++
 src/FDM/YASim/RigidBody.cpp | 38 ++++++++++++++++++++-----------------
 2 files changed, 23 insertions(+), 17 deletions(-)

diff --git a/src/FDM/YASim/Airplane.cpp b/src/FDM/YASim/Airplane.cpp
index 876528ec8..3d67328a2 100644
--- a/src/FDM/YASim/Airplane.cpp
+++ b/src/FDM/YASim/Airplane.cpp
@@ -448,6 +448,7 @@ void Airplane::setupState(const float aoa, const float speed, const float gla, S
     s->orient[3] =       0; s->orient[4] = 1; s->orient[5] =      0;
     s->orient[6] = -sinAoA; s->orient[7] = 0; s->orient[8] = cosAoA;
 
+    //? what is gla? v[1]=y, v[2]=z? should sin go to v2 instead v1?
     s->v[0] = speed*Math::cos(gla); s->v[1] = -speed*Math::sin(gla); s->v[2] = 0;
 
     int i;
@@ -976,6 +977,7 @@ void Airplane::runApproach()
     _model.calcForces(&_approachState);
 }
 
+// used in the solve methods
 void Airplane::applyDragFactor(float factor)
 {
     float applied = Math::pow(factor, SOLVE_TWEAK);
diff --git a/src/FDM/YASim/RigidBody.cpp b/src/FDM/YASim/RigidBody.cpp
index 61652e131..b4956cc42 100644
--- a/src/FDM/YASim/RigidBody.cpp
+++ b/src/FDM/YASim/RigidBody.cpp
@@ -249,6 +249,7 @@ void RigidBody::addForce(float* pos, float* force)
     addTorque(t);
 }
 
+/// not used (?)
 void RigidBody::setBodySpin(float* rotation)
 {
     Math::set3(rotation, _spin);
@@ -259,33 +260,36 @@ void RigidBody::getCG(float* cgOut)
     Math::set3(_cg, cgOut);
 }
 
+/// return acceleration at c.g.
 void RigidBody::getAccel(float* accelOut)
 {
     Math::mul3(1/_totalMass, _force, accelOut);
 }
 
+/// return acceleration at pos (unused?)
 void RigidBody::getAccel(float* pos, float* accelOut)
 {
     getAccel(accelOut);
-
-    // Turn the "spin" vector into a normalized spin axis "a" and a
-    // radians/sec scalar "rate".
-    float a[3];
+    // centripetal accelerations
+    // Calculate normalized spin axis "a" and rate (radians/sec) from spin vector.
     float rate = Math::mag3(_spin);
-    Math::set3(_spin, 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)
-    Math::add3(v, a, v);                 // v = v + a
+    if (rate != 0) {
+      float a[3], v[3];
+      Math::sub3(_cg, pos, v); // distance = cg - pos; 
+      if (v[0] == 0 && v[1] == 0 && v[2] == 0) // nothing to do
+        return;
+      Math::mul3(1/rate, _spin, a);
+      // d_a  = a * (distance dot a); (projection of distance vector on a, |a|=1)
+      Math::mul3(Math::dot3(v, a), a, a);  
+      Math::add3(v, a, v);                 // v = distance + projection;
 
-    // Now v contains the vector from pos to the rotation axis.
-    // Multiply by the square of the rotation rate to get the linear
-    // acceleration.
-    Math::mul3(rate*rate, v, v);
-    Math::add3(v, accelOut, accelOut);
+      // Now v contains the vector from pos to the rotation axis.
+      // Multiply by the square of the rotation rate to get the linear
+      // acceleration.
+      // nothing to do in the next two lines, if rate == 0
+      Math::mul3(rate*rate, v, v);		
+      Math::add3(v, accelOut, accelOut);
+    }
 }
 
 void RigidBody::getAngularAccel(float* accelOut)