src/FDM/YASim/RigidBody.cpp

   1 #include "Math.hpp"
   2 #include "RigidBody.hpp"
   3 namespace yasim {
   4
   5 RigidBody::RigidBody()
   6 {
   7     // Allocate space for 16 masses initially.  More space will be
   8     // allocated dynamically.
   9     _nMasses = 0;
  10     _massesAlloced = 16;
  11     _masses = new Mass[_massesAlloced];
  12     _gyro[0] = _gyro[1] = _gyro[2] = 0;
  13     _spin[0] = _spin[1] = _spin[2] = 0;
  14 }
  15
  16 RigidBody::~RigidBody()
  17 {
  18     delete[] _masses;
  19 }
  20
  21 int RigidBody::addMass(float mass, float* pos)
  22 {
  23     // If out of space, reallocate twice as much
  24     if(_nMasses == _massesAlloced) {
  25         _massesAlloced *= 2;
  26         Mass *m2 = new Mass[_massesAlloced];
  27         int i;
  28         for(i=0; i<_nMasses; i++)
  29             m2[i] = _masses[i];
  30         delete[] _masses;
  31         _masses = m2;
  32     }
  33
  34     _masses[_nMasses].m = mass;
  35     Math::set3(pos, _masses[_nMasses].p);
  36     return _nMasses++;
  37 }
  38
  39 void RigidBody::setMass(int handle, float mass)
  40 {
  41     _masses[handle].m = mass;
  42 }
  43
  44 void RigidBody::setMass(int handle, float mass, float* pos)
  45 {
  46     _masses[handle].m = mass;
  47     Math::set3(pos, _masses[handle].p);
  48 }
  49
  50 int RigidBody::numMasses()
  51 {
  52     return _nMasses;
  53 }
  54
  55 float RigidBody::getMass(int handle)
  56 {
  57     return _masses[handle].m;
  58 }
  59
  60 void RigidBody::getMassPosition(int handle, float* out)
  61 {
  62     out[0] = _masses[handle].p[0];
  63     out[1] = _masses[handle].p[1];
  64     out[2] = _masses[handle].p[2];
  65 }
  66
  67 float RigidBody::getTotalMass()
  68 {
  69     return _totalMass;
  70 }
  71
  72 // Calcualtes the rotational velocity of a particular point.  All
  73 // coordinates are local!
  74 void RigidBody::pointVelocity(float* pos, float* rot, float* out)
  75 {
  76     Math::sub3(pos, _cg, out);   //  out = pos-cg
  77     Math::cross3(rot, out, out); //      = rot cross (pos-cg)
  78 }
  79
  80 void RigidBody::setGyro(float* angularMomentum)
  81 {
  82     Math::set3(angularMomentum, _gyro);
  83 }
  84
  85 void RigidBody::recalc()
  86 {
  87     // Calculate the c.g and total mass:
  88     _totalMass = 0;
  89     _cg[0] = _cg[1] = _cg[2] = 0;
  90     int i;
  91     for(i=0; i<_nMasses; i++) {
  92         float m = _masses[i].m;
  93         _totalMass += m;
  94         _cg[0] += m * _masses[i].p[0];
  95         _cg[1] += m * _masses[i].p[1];
  96         _cg[2] += m * _masses[i].p[2];
  97     }
  98     Math::mul3(1/_totalMass, _cg, _cg);
  99
 100     // Now the inertia tensor:
 101     for(i=0; i<9; i++)
 102         _tI[i] = 0;
 103
 104     for(i=0; i<_nMasses; i++) {
 105         float m = _masses[i].m;
 106
 107         float x = _masses[i].p[0] - _cg[0];
 108         float y = _masses[i].p[1] - _cg[1];
 109         float z = _masses[i].p[2] - _cg[2];
 110
 111         float xy = m*x*y; float yz = m*y*z; float zx = m*z*x;
 112         float x2 = m*x*x; float y2 = m*y*y; float z2 = m*z*z;
 113
 114         _tI[0] += y2+z2;  _tI[1] -=    xy;  _tI[2] -=    zx;
 115         _tI[3] -=    xy;  _tI[4] += x2+z2;  _tI[5] -=    yz;
 116         _tI[6] -=    zx;   _tI[7] -=   yz;  _tI[8] += x2+y2;
 117     }
 118
 119     // And its inverse
 120     Math::invert33(_tI, _invI);
 121 }
 122
 123 void RigidBody::reset()
 124 {
 125     _torque[0] = _torque[1] = _torque[2] = 0;
 126     _force[0] = _force[1] = _force[2] = 0;
 127 }
 128
 129 void RigidBody::addForce(float* force)
 130 {
 131     Math::add3(_force, force, _force);
 132 }
 133
 134 void RigidBody::addTorque(float* torque)
 135 {
 136     Math::add3(_torque, torque, _torque);
 137 }
 138
 139 void RigidBody::addForce(float* pos, float* force)
 140 {
 141     addForce(force);
 142
 143     // For a force F at position X, the torque about the c.g C is:
 144     // torque = F cross (C - X)
 145     float v[3], t[3];
 146     Math::sub3(_cg, pos, v);
 147     Math::cross3(force, v, t);
 148     addTorque(t);
 149 }
 150
 151 void RigidBody::setBodySpin(float* rotation)
 152 {
 153     Math::set3(rotation, _spin);
 154 }
 155
 156 void RigidBody::getCG(float* cgOut)
 157 {
 158     Math::set3(_cg, cgOut);
 159 }
 160
 161 void RigidBody::getAccel(float* accelOut)
 162 {
 163     Math::mul3(1/_totalMass, _force, accelOut);
 164 }
 165
 166 void RigidBody::getAccel(float* pos, float* accelOut)
 167 {
 168     getAccel(accelOut);
 169
 170     // Turn the "spin" vector into a normalized spin axis "a" and a
 171     // radians/sec scalar "rate".
 172     float a[3];
 173     float rate = Math::mag3(_spin);
 174     Math::set3(_spin, a);
 175     Math::mul3(1/rate, a, a);
 176
 177     float v[3];
 178     Math::sub3(_cg, pos, v);             // v = cg - pos
 179     Math::mul3(Math::dot3(v, a), a, a);  // a = a * (v dot a)
 180     Math::add3(v, a, v);                 // v = v + a
 181
 182     // Now v contains the vector from pos to the rotation axis.
 183     // Multiply by the square of the rotation rate to get the linear
 184     // acceleration.
 185     Math::mul3(rate*rate, v, v);
 186     Math::add3(v, accelOut, accelOut);
 187 }
 188
 189 void RigidBody::getAngularAccel(float* accelOut)
 190 {
 191     // Compute "tau" as the externally applied torque, plus the
 192     // counter-torque due to the internal gyro.
 193     float tau[3]; // torque
 194     Math::cross3(_gyro, _spin, tau);
 195     Math::add3(_torque, tau, tau);
 196
 197     // Now work the equation of motion.  Use "v" as a notational
 198     // shorthand, as the value isn't an acceleration until the end.
 199     float *v = accelOut;
 200     Math::vmul33(_tI, _spin, v);  // v = I*omega
 201     Math::cross3(_spin, v, v);   // v = omega X I*omega
 202     Math::add3(tau, v, v);       // v = tau + (omega X I*omega)
 203     Math::vmul33(_invI, v, v);   // v = invI*(tau + (omega X I*omega))
 204 }
 205
 206 }; // namespace yasim