2 #include "RigidBody.hpp"
7 // Allocate space for 16 masses initially. More space will be
8 // allocated dynamically.
11 _masses = new Mass[_massesAlloced];
12 _gyro[0] = _gyro[1] = _gyro[2] = 0;
13 _spin[0] = _spin[1] = _spin[2] = 0;
16 RigidBody::~RigidBody()
21 int RigidBody::addMass(float mass, float* pos)
23 // If out of space, reallocate twice as much
24 if(_nMasses == _massesAlloced) {
26 Mass *m2 = new Mass[_massesAlloced];
28 for(i=0; i<_nMasses; i++)
34 _masses[_nMasses].m = mass;
35 Math::set3(pos, _masses[_nMasses].p);
39 void RigidBody::setMass(int handle, float mass)
41 _masses[handle].m = mass;
44 void RigidBody::setMass(int handle, float mass, float* pos)
46 _masses[handle].m = mass;
47 Math::set3(pos, _masses[handle].p);
50 int RigidBody::numMasses()
55 float RigidBody::getMass(int handle)
57 return _masses[handle].m;
60 void RigidBody::getMassPosition(int handle, float* out)
62 out[0] = _masses[handle].p[0];
63 out[1] = _masses[handle].p[1];
64 out[2] = _masses[handle].p[2];
67 float RigidBody::getTotalMass()
72 // Calcualtes the rotational velocity of a particular point. All
73 // coordinates are local!
74 void RigidBody::pointVelocity(float* pos, float* rot, float* out)
76 Math::sub3(pos, _cg, out); // out = pos-cg
77 Math::cross3(rot, out, out); // = rot cross (pos-cg)
80 void RigidBody::setGyro(float* angularMomentum)
82 Math::set3(angularMomentum, _gyro);
85 void RigidBody::recalc()
87 // Calculate the c.g and total mass:
89 _cg[0] = _cg[1] = _cg[2] = 0;
91 for(i=0; i<_nMasses; i++) {
92 float m = _masses[i].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];
98 Math::mul3(1/_totalMass, _cg, _cg);
100 // Now the inertia tensor:
104 for(i=0; i<_nMasses; i++) {
105 float m = _masses[i].m;
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];
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;
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;
120 Math::invert33(_tI, _invI);
123 void RigidBody::reset()
125 _torque[0] = _torque[1] = _torque[2] = 0;
126 _force[0] = _force[1] = _force[2] = 0;
129 void RigidBody::addForce(float* force)
131 Math::add3(_force, force, _force);
134 void RigidBody::addTorque(float* torque)
136 Math::add3(_torque, torque, _torque);
139 void RigidBody::addForce(float* pos, float* force)
143 // For a force F at position X, the torque about the c.g C is:
144 // torque = F cross (C - X)
146 Math::sub3(_cg, pos, v);
147 Math::cross3(force, v, t);
151 void RigidBody::setBodySpin(float* rotation)
153 Math::set3(rotation, _spin);
156 void RigidBody::getCG(float* cgOut)
158 Math::set3(_cg, cgOut);
161 void RigidBody::getAccel(float* accelOut)
163 Math::mul3(1/_totalMass, _force, accelOut);
166 void RigidBody::getAccel(float* pos, float* accelOut)
170 // Turn the "spin" vector into a normalized spin axis "a" and a
171 // radians/sec scalar "rate".
173 float rate = Math::mag3(_spin);
174 Math::set3(_spin, a);
176 Math::mul3(1/rate, a, a);
177 //an else branch is not neccesary. a, which is a=(0,0,0) in the else case, is only used in a dot product
179 Math::sub3(_cg, pos, v); // v = cg - pos
180 Math::mul3(Math::dot3(v, a), a, a); // a = a * (v dot a)
181 Math::add3(v, a, v); // v = v + a
183 // Now v contains the vector from pos to the rotation axis.
184 // Multiply by the square of the rotation rate to get the linear
186 Math::mul3(rate*rate, v, v);
187 Math::add3(v, accelOut, accelOut);
190 void RigidBody::getAngularAccel(float* accelOut)
192 // Compute "tau" as the externally applied torque, plus the
193 // counter-torque due to the internal gyro.
194 float tau[3]; // torque
195 Math::cross3(_gyro, _spin, tau);
196 Math::add3(_torque, tau, tau);
198 // Now work the equation of motion. Use "v" as a notational
199 // shorthand, as the value isn't an acceleration until the end.
201 Math::vmul33(_tI, _spin, v); // v = I*omega
202 Math::cross3(_spin, v, v); // v = omega X I*omega
203 Math::add3(tau, v, v); // v = tau + (omega X I*omega)
204 Math::vmul33(_invI, v, v); // v = invI*(tau + (omega X I*omega))
207 void RigidBody::getInertiaMatrix(float* inertiaOut)
209 // valid only after a call to RigidBody::recalc()
210 // See comment at top of RigidBody.hpp on units.
213 inertiaOut[i] = _tI[i];
217 }; // namespace yasim