YASim optimization: pre-calculate data for masses that do not change in flight to save CPU (RigidBody::recalc() runs at FDM rate).

export mass data to property tree.

src/FDM/YASim/Airplane.cpp

@@ -358,12 +358,14 @@ void Airplane::addThruster(Thruster* thruster, float mass, float* cg)
     _thrusters.add(t);
 }
 
+/// Use ballast to redistribute mass, this is NOT added to empty weight.
 void Airplane::addBallast(float* pos, float mass)
 {
-    _model.getBody()->addMass(mass, pos);
+    _model.getBody()->addMass(mass, pos, true);
     _ballast += mass;
 }
 
+/// Masses configurable at runtime, e.g. cargo, pax
 int Airplane::addWeight(float* pos, float size)
 {
     WeightRec* wr = new WeightRec();
@@ -378,6 +380,7 @@ int Airplane::addWeight(float* pos, float size)
     return _weights.add(wr);
 }
 
+/// Change weight of a previously added mass point
 void Airplane::setWeight(int handle, float mass)
 {
     WeightRec* wr = (WeightRec*)_weights.get(handle);
@@ -500,7 +503,7 @@ float Airplane::compileWing(Wing* w)
         mass = mass * Math::sqrt(mass);
         float pos[3];
         s->getPosition(pos);
-        int mid = _model.getBody()->addMass(mass, pos);
+        int mid = _model.getBody()->addMass(mass, pos, true);
         if (_wingsN != 0) {
           SGPropertyNode_ptr n = _wingsN->getNode("surfaces", true)->getChild("surface", sid, true);
           n->getNode("drag", true)->setFloatValue(td);
@@ -562,7 +565,7 @@ float Airplane::compileFuselage(Fuselage* f)
 
         // _Mass_ weighting goes as surface area^(3/2)
         float mass = scale*segWgt * Math::sqrt(scale*segWgt);
-        _model.getBody()->addMass(mass, pos);
+        _model.getBody()->addMass(mass, pos, true);
         wgt += mass;
 
         // Make a Surface too
@@ -732,7 +735,7 @@ void Airplane::compile()
     // Add the thruster masses
     for(i=0; i<_thrusters.size(); i++) {
         ThrustRec* t = (ThrustRec*)_thrusters.get(i);
-        body->addMass(t->mass, t->cg);
+        body->addMass(t->mass, t->cg, true);
     }
 
     // Add the tanks, empty for now.

src/FDM/YASim/RigidBody.cpp

@@ -1,5 +1,7 @@
+#include <Main/fg_props.hxx>
 #include "Math.hpp"
 #include "RigidBody.hpp"
+
 namespace yasim {
 
 RigidBody::RigidBody()
@@ -11,6 +13,7 @@ RigidBody::RigidBody()
     _masses = new Mass[_massesAlloced];
     _gyro[0] = _gyro[1] = _gyro[2] = 0;
     _spin[0] = _spin[1] = _spin[2] = 0;
+    _bodyN = fgGetNode("/fdm/yasim/model/masses", true);
 }
 
 RigidBody::~RigidBody()
@@ -18,7 +21,9 @@ RigidBody::~RigidBody()
     delete[] _masses;
 }
 
-int RigidBody::addMass(float mass, float* pos)
+/// add new point mass to body
+/// isStatic: set to true for masses that do not change per iteration (everything but fuel?)
+int RigidBody::addMass(float mass, float* pos, bool isStatic)
 {
     // If out of space, reallocate twice as much
     if(_nMasses == _massesAlloced) {
@@ -30,21 +35,38 @@ int RigidBody::addMass(float mass, float* pos)
         delete[] _masses;
         _masses = m2;
     }
-
+    setMass(_nMasses, mass, pos, isStatic);
-    _masses[_nMasses].m = mass;
-    Math::set3(pos, _masses[_nMasses].p);
     return _nMasses++;
 }
 
+/// change mass
+/// handle: returned by addMass
 void RigidBody::setMass(int handle, float mass)
 {
+    if (_masses[handle].m  == mass) 
+      return;
     _masses[handle].m = mass;
+    // if static mass is changed, reset pre-calculated mass
+    // may apply to weights like cargo, pax, that usually do not change with FDM rate 
+    if (_masses[handle].isStatic)
+      _staticMass.m = 0;
+    if (_bodyN != 0)
+      _bodyN->getChild("mass", handle, true)->getNode("mass", true)->setFloatValue(mass);
 }
 
-void RigidBody::setMass(int handle, float mass, float* pos)
+void RigidBody::setMass(int handle, float mass, float* pos, bool isStatic)
 {
     _masses[handle].m = mass;
+    _masses[handle].isStatic = isStatic;
     Math::set3(pos, _masses[handle].p);
+    if (_bodyN != 0) {
+      SGPropertyNode_ptr n = _bodyN->getChild("mass", handle, true);
+      n->getNode("isStatic", true)->setValue(isStatic);
+      n->getNode("mass", true)->setFloatValue(mass);
+      n->getNode("pos-x", true)->setFloatValue(pos[0]);
+      n->getNode("pos-y", true)->setFloatValue(pos[1]);
+      n->getNode("pos-z", true)->setFloatValue(pos[2]);	
+    }
 }
 
 int RigidBody::numMasses()
@@ -82,47 +104,113 @@ void RigidBody::setGyro(float* angularMomentum)
     Math::set3(angularMomentum, _gyro);
 }
 
+void RigidBody::_recalcStatic()
+{
+	// aggregate all masses that do not change (e.g. fuselage, wings) into one point mass
+	_staticMass.m = 0;
+	_staticMass.p[0] = 0;
+	_staticMass.p[1] = 0;
+	_staticMass.p[2] = 0;
+	int i;
+	int s = 0;
+	for(i=0; i<_nMasses; i++) {
+		if (_masses[i].isStatic) {
+			s++;
+			float m = _masses[i].m;
+			_staticMass.m += m;
+			_staticMass.p[0] += m * _masses[i].p[0];
+			_staticMass.p[1] += m * _masses[i].p[1];
+			_staticMass.p[2] += m * _masses[i].p[2];
+		}
+	}
+	Math::mul3(1/_staticMass.m, _staticMass.p, _staticMass.p);
+  if (_bodyN != 0) {
+    _bodyN->getNode("aggregated-mass", true)->setFloatValue(_staticMass.m);
+    _bodyN->getNode("aggregated-count", true)->setIntValue(s);
+    _bodyN->getNode("aggregated-pos-x", true)->setFloatValue(_staticMass.p[0]);
+    _bodyN->getNode("aggregated-pos-y", true)->setFloatValue(_staticMass.p[1]);
+    _bodyN->getNode("aggregated-pos-z", true)->setFloatValue(_staticMass.p[2]);	
+  }
+	// Now the inertia tensor:
+	for(i=0; i<9; i++)
+		_tI_static[i] = 0;
+
+	for(i=0; i<_nMasses; i++) {
+		if (_masses[i].isStatic) {
+			float m = _masses[i].m;
+
+			float x = _masses[i].p[0] - _staticMass.p[0];
+			float y = _masses[i].p[1] - _staticMass.p[1];
+			float z = _masses[i].p[2] - _staticMass.p[2];
+
+			float xy = m*x*y; float yz = m*y*z; float zx = m*z*x;
+			float x2 = m*x*x; float y2 = m*y*y; float z2 = m*z*z;
+
+			// tensor is symmetric, so we can save some calculations in the loop
+			_tI_static[0] += y2+z2;  _tI_static[1] -=    xy;  _tI_static[2] -=    zx;
+			                         _tI_static[4] += x2+z2;  _tI_static[5] -=    yz;
+			                                                  _tI_static[8] += x2+y2;
+		}
+	}
+	// copy symmetric elements
+	_tI_static[3] = _tI_static[1];
+	_tI_static[6] = _tI_static[2];
+	_tI_static[7] = _tI_static[5];
+}
+
 /// calculate the total mass, centre of gravity and inertia tensor
 /**
   recalc is used when compiling the model but more important it is called in
   Model::iterate() e.g. at FDM rate (120 Hz)
-  We can save some CPU due to the symmetry of the tensor.
+  We can save some CPU due to the symmetry of the tensor and by aggregating
+  masses that do not change during flight.
  */
 void RigidBody::recalc()
 {
-    // Calculate the c.g and total mass:
+    //aggregate static masses into one mass
-    _totalMass = 0;
+    if (_staticMass.m == 0) _recalcStatic();
-    _cg[0] = _cg[1] = _cg[2] = 0;
+
+    // Calculate the c.g and total mass
+    // init with pre-calculated static mass
+    _totalMass = _staticMass.m;
+    _cg[0] = _staticMass.m * _staticMass.p[0];
+    _cg[1] = _staticMass.m * _staticMass.p[1];
+    _cg[2] = _staticMass.m * _staticMass.p[2];
     int i;
     for(i=0; i<_nMasses; i++) {
+      // only masses we did not aggregate
+      if (!_masses[i].isStatic) { 
         float m = _masses[i].m;
         _totalMass += m;
         _cg[0] += m * _masses[i].p[0];
         _cg[1] += m * _masses[i].p[1];
         _cg[2] += m * _masses[i].p[2];
+      }
     }
     Math::mul3(1/_totalMass, _cg, _cg);
 
     // Now the inertia tensor:
     for(i=0; i<9; i++)
-	_tI[i] = 0;
+      _tI[i] = _tI_static[i];
 
     for(i=0; i<_nMasses; i++) {
-      float m = _masses[i].m;
+      if (!_masses[i].isStatic) {
+        float m = _masses[i].m;
 
-      float x = _masses[i].p[0] - _cg[0];
+        float x = _masses[i].p[0] - _cg[0];
-      float y = _masses[i].p[1] - _cg[1];
+        float y = _masses[i].p[1] - _cg[1];
-      float z = _masses[i].p[2] - _cg[2];
+        float z = _masses[i].p[2] - _cg[2];
-      float mx = m*x;
+        float mx = m*x;
-      float my = m*y;
+        float my = m*y;
-      float mz = m*z;
+        float mz = m*z;
-      
+        
-      float xy = mx*y; float yz = my*z; float zx = mz*x;
+        float xy = mx*y; float yz = my*z; float zx = mz*x;
-      float x2 = mx*x; float y2 = my*y; float z2 = mz*z;
+        float x2 = mx*x; float y2 = my*y; float z2 = mz*z;
 
-      _tI[0] += y2+z2;  _tI[1] -=    xy;  _tI[2] -=    zx;
+        _tI[0] += y2+z2;  _tI[1] -=    xy;  _tI[2] -=    zx;
-                        _tI[4] += x2+z2;  _tI[5] -=    yz;
+                          _tI[4] += x2+z2;  _tI[5] -=    yz;
-                                          _tI[8] += x2+y2;
+                                            _tI[8] += x2+y2;
+      }
     }
     // copy symmetric elements 
     _tI[3] = _tI[1];

src/FDM/YASim/RigidBody.hpp

@@ -1,5 +1,7 @@
 #ifndef _RIGIDBODY_HPP
 #define _RIGIDBODY_HPP
+#include <simgear/props/props.hxx>
+#include "Vector.hpp"
 
 namespace yasim {
 
@@ -20,18 +22,19 @@ namespace yasim {
 //
 class RigidBody
 {
+    SGPropertyNode_ptr _bodyN;
 public:
     RigidBody();
     ~RigidBody();
 
     // Adds a point mass to the system.  Returns a handle so the gyro
     // can be later modified via setMass().
-    int addMass(float mass, float* pos);
+    int addMass(float mass, float* pos, bool isStatic = false);
 
     // Modifies a previously-added point mass (fuel tank running dry,
     // gear going up, swing wing swinging, pilot bailing out, etc...)
     void setMass(int handle, float mass);
-    void setMass(int handle, float mass, float* pos);
+    void setMass(int handle, float mass, float* pos, bool isStatic = false);
 
     int numMasses();
     float getMass(int handle);
@@ -54,7 +57,7 @@ public:
     // When masses are moved or changed, this object needs to
     // regenerate its internal tables.  This step is expensive, so
     // it's exposed to the client who can amortize the call across
-    // multiple changes.
+    // multiple changes. see also _recalcStatic() 
     void recalc();
 
     // Resets the current force/torque parameters to zero.
@@ -102,17 +105,24 @@ public:
     void getInertiaMatrix(float* inertiaOut);
 
 private:
-    struct Mass { float m; float p[3]; };
+    /** 
+    Most of the mass points do not change after compilation of the aircraft so
+    they can be replaced by one aggregated mass at the c.g. of the static masses.
+    The isStatic flag is used to mark those masses.
+    */
+    struct Mass { float m; float p[3]; bool isStatic; };
+    void _recalcStatic(); /// aggregate static masses
+    Mass  _staticMass;		/// aggregated static masses, calculated once
+    Mass* _masses;        /// mass elements
+    int   _nMasses;       /// number of masses
+    int   _massesAlloced; /// counter for memory allocation
 
-    // Internal "rotational structure"
-    Mass* _masses;
-    int   _nMasses;
-    int   _massesAlloced;
     float _totalMass;
     float _cg[3];
     float _gyro[3];
 
     // Inertia tensor, and its inverse.  Computed from the above.
+    float _tI_static[9];
     float _tI[9];
     float _invI[9];