diff --git a/src/FDM/YASim/Airplane.cpp b/src/FDM/YASim/Airplane.cpp index 4375b6a4b..6fc987002 100644 --- a/src/FDM/YASim/Airplane.cpp +++ b/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. diff --git a/src/FDM/YASim/RigidBody.cpp b/src/FDM/YASim/RigidBody.cpp index 80c813b95..61652e131 100644 --- a/src/FDM/YASim/RigidBody.cpp +++ b/src/FDM/YASim/RigidBody.cpp @@ -1,5 +1,7 @@ +#include
#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; } - - _masses[_nMasses].m = mass; - Math::set3(pos, _masses[_nMasses].p); + setMass(_nMasses, mass, pos, isStatic); 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: - _totalMass = 0; - _cg[0] = _cg[1] = _cg[2] = 0; + //aggregate static masses into one mass + if (_staticMass.m == 0) _recalcStatic(); + + // 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 y = _masses[i].p[1] - _cg[1]; - float z = _masses[i].p[2] - _cg[2]; - float mx = m*x; - float my = m*y; - float mz = m*z; - - 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 x = _masses[i].p[0] - _cg[0]; + float y = _masses[i].p[1] - _cg[1]; + float z = _masses[i].p[2] - _cg[2]; + float mx = m*x; + float my = m*y; + float mz = m*z; + + float xy = mx*y; float yz = my*z; float zx = mz*x; + float x2 = mx*x; float y2 = my*y; float z2 = mz*z; - _tI[0] += y2+z2; _tI[1] -= xy; _tI[2] -= zx; - _tI[4] += x2+z2; _tI[5] -= yz; - _tI[8] += x2+y2; + _tI[0] += y2+z2; _tI[1] -= xy; _tI[2] -= zx; + _tI[4] += x2+z2; _tI[5] -= yz; + _tI[8] += x2+y2; + } } // copy symmetric elements _tI[3] = _tI[1]; diff --git a/src/FDM/YASim/RigidBody.hpp b/src/FDM/YASim/RigidBody.hpp index 6e4a4ad30..160a24ecb 100644 --- a/src/FDM/YASim/RigidBody.hpp +++ b/src/FDM/YASim/RigidBody.hpp @@ -1,5 +1,7 @@ #ifndef _RIGIDBODY_HPP #define _RIGIDBODY_HPP +#include +#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];