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];
27 for(int i=0; i<_nMasses; i++)
33 _masses[_nMasses].m = mass;
34 Math::set3(pos, _masses[_nMasses].p);
38 void RigidBody::setMass(int handle, float mass)
40 _masses[handle].m = mass;
43 void RigidBody::setMass(int handle, float mass, float* pos)
45 _masses[handle].m = mass;
46 Math::set3(pos, _masses[handle].p);
49 int RigidBody::numMasses()
54 float RigidBody::getMass(int handle)
56 return _masses[handle].m;
59 void RigidBody::getMassPosition(int handle, float* out)
61 out[0] = _masses[handle].p[0];
62 out[1] = _masses[handle].p[1];
63 out[2] = _masses[handle].p[2];
66 float RigidBody::getTotalMass()
71 // Calcualtes the rotational velocity of a particular point. All
72 // coordinates are local!
73 void RigidBody::pointVelocity(float* pos, float* rot, float* out)
75 Math::sub3(pos, _cg, out); // out = pos-cg
76 Math::cross3(rot, out, out); // = rot cross (pos-cg)
79 void RigidBody::setGyro(float* angularMomentum)
81 Math::set3(angularMomentum, _gyro);
84 void RigidBody::recalc()
86 // Calculate the c.g and total mass:
88 _cg[0] = _cg[1] = _cg[2] = 0;
89 for(int i=0; i<_nMasses; i++) {
90 float m = _masses[i].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];
96 Math::mul3(1/_totalMass, _cg, _cg);
98 // Now the inertia tensor:
99 for(int i=0; i<9; i++)
102 for(int i=0; i<_nMasses; i++) {
103 float m = _masses[i].m;
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];
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;
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;
118 Math::invert33(_I, _invI);
121 void RigidBody::reset()
123 _torque[0] = _torque[1] = _torque[2] = 0;
124 _force[0] = _force[1] = _force[2] = 0;
127 void RigidBody::addForce(float* force)
129 Math::add3(_force, force, _force);
132 void RigidBody::addTorque(float* torque)
134 Math::add3(_torque, torque, _torque);
137 void RigidBody::addForce(float* pos, float* force)
141 // For a force F at position X, the torque about the c.g C is:
142 // torque = F cross (C - X)
144 Math::sub3(_cg, pos, v);
145 Math::cross3(force, v, t);
149 void RigidBody::setBodySpin(float* rotation)
151 Math::set3(rotation, _spin);
154 void RigidBody::getCG(float* cgOut)
156 Math::set3(_cg, cgOut);
159 void RigidBody::getAccel(float* accelOut)
161 Math::mul3(1/_totalMass, _force, accelOut);
164 void RigidBody::getAccel(float* pos, float* accelOut)
168 // Turn the "spin" vector into a normalized spin axis "a" and a
169 // radians/sec scalar "rate".
171 float rate = Math::mag3(_spin);
172 Math::set3(_spin, a);
173 Math::mul3(1/rate, a, a);
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
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
183 Math::mul3(rate*rate, v, v);
184 Math::add3(v, accelOut, accelOut);
187 void RigidBody::getAngularAccel(float* accelOut)
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);
195 // Now work the equation of motion. Use "v" as a notational
196 // shorthand, as the value isn't an acceleration until the end.
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))
204 }; // namespace yasim