From b447bbdc5e95ee5e763ba89e88247a3b3c584232 Mon Sep 17 00:00:00 2001 From: Henning Stahlke Date: Wed, 1 Feb 2017 23:31:43 +0100 Subject: [PATCH] Optimize YASim/RigidBody.cpp utilizing symmetry of tensor. --- src/FDM/YASim/Math.hpp | 30 ++++++++++++++---------------- src/FDM/YASim/RigidBody.cpp | 37 +++++++++++++++++++++++++------------ 2 files changed, 39 insertions(+), 28 deletions(-) diff --git a/src/FDM/YASim/Math.hpp b/src/FDM/YASim/Math.hpp index 82bad4c5c..19b814b81 100644 --- a/src/FDM/YASim/Math.hpp +++ b/src/FDM/YASim/Math.hpp @@ -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 diff --git a/src/FDM/YASim/RigidBody.cpp b/src/FDM/YASim/RigidBody.cpp index 7f530305b..80c813b95 100644 --- a/src/FDM/YASim/RigidBody.cpp +++ b/src/FDM/YASim/RigidBody.cpp @@ -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: @@ -102,22 +108,29 @@ void RigidBody::recalc() _tI[i] = 0; 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 y = _masses[i].p[1] - _cg[1]; - float z = _masses[i].p[2] - _cg[2]; + 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 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; - - _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[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]; + _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()