1
0
Fork 0

YASim: user Math functions in RigidBody

This commit is contained in:
Henning Stahlke 2017-12-11 19:27:59 +01:00
parent abb451256d
commit a199ca817c
2 changed files with 74 additions and 78 deletions

View file

@ -50,6 +50,8 @@ public:
out[2] = v[2];
}
static void zero3(float* out) { out[0] = out[1] = out[2] = 0; }
static inline float dot3(const float* a, const float* b) {
return a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
}

View file

@ -69,9 +69,7 @@ void RigidBody::setMass(int handle, float mass, const float* pos, bool isStatic)
void RigidBody::getMassPosition(int handle, float* out) const
{
out[0] = _masses[handle].p[0];
out[1] = _masses[handle].p[1];
out[2] = _masses[handle].p[2];
Math::set3(_masses[handle].p, out);
}
// Calcualtes the rotational velocity of a particular point. All
@ -84,56 +82,54 @@ void RigidBody::pointVelocity(const float* pos, const float* rot, float* out)
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;
// aggregate all masses that do not change (e.g. fuselage, wings) into one point mass
_staticMass.m = 0;
Math::zero3(_staticMass.p);
int i;
int s = 0;
for(i=0; i<_nMasses; i++) {
if (_masses[i].isStatic) {
s++;
float mass = _masses[i].m;
_staticMass.m += mass;
float momentum[3];
Math::mul3(mass, _masses[i].p, momentum);
Math::add3(momentum, _staticMass.p, _staticMass.p);
}
}
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;
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 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;
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];
// 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
@ -151,44 +147,42 @@ void RigidBody::recalc()
// 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];
Math::mul3(_staticMass.m, _staticMass.p, _cg);
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];
}
// only masses we did not aggregate
if (!_masses[i].isStatic) {
float mass = _masses[i].m;
_totalMass += mass;
float momentum[3];
Math::mul3(mass, _masses[i].p, momentum);
Math::add3(momentum, _cg, _cg);
}
}
Math::mul3(1/_totalMass, _cg, _cg);
// Now the inertia tensor:
for(i=0; i<9; i++)
_tI[i] = _tI_static[i];
_tI[i] = _tI_static[i];
for(i=0; i<_nMasses; i++) {
if (!_masses[i].isStatic) {
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];
@ -201,8 +195,8 @@ void RigidBody::recalc()
void RigidBody::reset()
{
_torque[0] = _torque[1] = _torque[2] = 0;
_force[0] = _force[1] = _force[2] = 0;
Math::zero3(_torque);
Math::zero3(_force);
}
void RigidBody::addForce(const float* pos, const float* force)