fly/flightgear
1
0
Fork 0
Code Activity

Optimize YASim/RigidBody.cpp utilizing symmetry of tensor.

Browse source
This commit is contained in:
Henning Stahlke 2017-02-01 23:31:43 +01:00
parent 6257c29fd8
commit b447bbdc5e
2 changed files with 39 additions and 28 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

30
src/FDM/YASim/Math.hpp
View file

@ -130,31 +130,29 @@ public:
out[2] = x*m[2] + y*m[5] + z*m[8]; out[2] = x*m[2] + y*m[5] + z*m[8];
} }
// Invert matrix /// Invert symmetric matrix; ~1/3 less calculations due to symmetry
static void invert33(float* m, float* out) { static void invert33_sym(float* m, float* out) {
// Compute the inverse as the adjoint matrix times 1/(det M). // Compute the inverse as the adjoint matrix times 1/(det M).
// A, B ... I are the cofactors of a b c // A, B ... I are the cofactors of a b c
// d e f // d e f
// g h i // g h i
// symetric: d=b, g=c, h=f
float a=m[0], b=m[1], c=m[2]; float a=m[0], b=m[1], c=m[2];
float d=m[3], e=m[4], f=m[5]; float e=m[4], f=m[5];
float g=m[6], h=m[7], i=m[8]; float i=m[8];
float A = (e*i - h*f); float A = (e*i - f*f);
float B = -(d*i - g*f); float B = -(b*i - c*f);
float C = (d*h - g*e); float C = (b*f - c*e);
float D = -(b*i - h*c); float E = (a*i - c*c);
float E = (a*i - g*c); float F = -(a*f - c*b);
float F = -(a*h - g*b); float I = (a*e - b*b);
float G = (b*f - e*c);
float H = -(a*f - d*c);
float I = (a*e - d*b);
float id = 1/(a*A + b*B + c*C); float id = 1/(a*A + b*B + c*C);
out[0] = id*A; out[1] = id*D; out[2] = id*G; out[0] = id*A; out[1] = id*B; out[2] = id*C;
out[3] = id*B; out[4] = id*E; out[5] = id*H; out[3] = out[1]; out[4] = id*E; out[5] = id*F;
out[6] = id*C; out[7] = id*F; out[8] = id*I; out[6] = out[2]; out[7] = out[5]; out[8] = id*I;
} }
// Transpose matrix (for an orthonormal orientation matrix, this // Transpose matrix (for an orthonormal orientation matrix, this

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

@ -82,6 +82,12 @@ void RigidBody::setGyro(float* angularMomentum)
Math::set3(angularMomentum, _gyro); Math::set3(angularMomentum, _gyro);
} }
/// 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.
*/
void RigidBody::recalc() void RigidBody::recalc()
{ {
// Calculate the c.g and total mass: // Calculate the c.g and total mass:
@ -102,22 +108,29 @@ void RigidBody::recalc()
_tI[i] = 0; _tI[i] = 0;
for(i=0; i<_nMasses; i++) { for(i=0; i<_nMasses; i++) {
float m = _masses[i].m; 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 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 xy = m*x*y; float yz = m*y*z; float zx = m*z*x; _tI[0] += y2+z2; _tI[1] -= xy; _tI[2] -= zx;
float x2 = m*x*x; float y2 = m*y*y; float z2 = m*z*z; _tI[4] += x2+z2; _tI[5] -= yz;
_tI[8] += x2+y2;
_tI[0] += y2+z2; _tI[1] -= xy; _tI[2] -= zx;
_tI[3] -= xy; _tI[4] += x2+z2; _tI[5] -= yz;
_tI[6] -= zx; _tI[7] -= yz; _tI[8] += x2+y2;
} }
// copy symmetric elements
_tI[3] = _tI[1];
_tI[6] = _tI[2];
_tI[7] = _tI[5];
// And its inverse //calculate inverse
Math::invert33(_tI, _invI); Math::invert33_sym(_tI, _invI);
} }
void RigidBody::reset() void RigidBody::reset()