YASim: user Math functions in RigidBody
This commit is contained in:
parent
abb451256d
commit
a199ca817c
2 changed files with 74 additions and 78 deletions
|
@ -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];
|
||||
}
|
||||
|
|
|
@ -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)
|
||||
|
|
Loading…
Add table
Reference in a new issue