fly/flightgear
1
0
Fork 0
Code Activity

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

Browse source 
export mass data to property tree.
This commit is contained in:
Henning Stahlke 2017-02-08 08:19:09 +01:00
parent 4e1be43f15
commit 559dcf4e32
3 changed files with 136 additions and 35 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

11
src/FDM/YASim/Airplane.cpp
View file

@ -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.

134
src/FDM/YASim/RigidBody.cpp
View file

@ -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;
}
_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];

26
src/FDM/YASim/RigidBody.hpp
View file

@ -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];