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

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

@ -82,6 +82,12 @@ void RigidBody::setGyro(float* angularMomentum)
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()
{
// Calculate the c.g and total mass:
@ -107,17 +113,24 @@ void RigidBody::recalc()
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 = 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;
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[3] -= xy; _tI[4] += x2+z2; _tI[5] -= yz;
_tI[6] -= zx; _tI[7] -= yz; _tI[8] += x2+y2;
_tI[4] += x2+z2; _tI[5] -= yz;
_tI[8] += x2+y2;
}
// copy symmetric elements
_tI[3] = _tI[1];
_tI[6] = _tI[2];
_tI[7] = _tI[5];
// And its inverse
Math::invert33(_tI, _invI);
//calculate inverse
Math::invert33_sym(_tI, _invI);
}
void RigidBody::reset()