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