| 232 | } |
| 233 | |
| 234 | F32 Rigid::getKineticEnergy() |
| 235 | { |
| 236 | Point3F w; |
| 237 | QuatF qmat = angPosition; |
| 238 | qmat.inverse(); |
| 239 | qmat.mulP(angVelocity,&w); |
| 240 | const F32* f = invObjectInertia; |
| 241 | return 0.5f * ((mass * mDot(linVelocity,linVelocity)) + |
| 242 | w.x * w.x / f[0] + |
| 243 | w.y * w.y / f[5] + |
| 244 | w.z * w.z / f[10]); |
| 245 | } |
| 246 | |
| 247 | void Rigid::getOriginVector(const Point3F &p,Point3F* r) |
| 248 | { |