-----------------------------------------------------------------------------
| 56 | |
| 57 | //----------------------------------------------------------------------------- |
| 58 | void Rigid::integrate(F32 delta) |
| 59 | { |
| 60 | // Update Angular position |
| 61 | F32 angle = angVelocity.len(); |
| 62 | if (angle != 0.0f) { |
| 63 | QuatF dq; |
| 64 | F32 sinHalfAngle; |
| 65 | mSinCos(angle * delta * -0.5f, sinHalfAngle, dq.w); |
| 66 | sinHalfAngle *= 1.0f / angle; |
| 67 | dq.x = angVelocity.x * sinHalfAngle; |
| 68 | dq.y = angVelocity.y * sinHalfAngle; |
| 69 | dq.z = angVelocity.z * sinHalfAngle; |
| 70 | QuatF tmp = angPosition; |
| 71 | angPosition.mul(tmp, dq); |
| 72 | angPosition.normalize(); |
| 73 | |
| 74 | // Rotate the position around the center of mass |
| 75 | Point3F lp = linPosition - worldCenterOfMass; |
| 76 | dq.mulP(lp,&linPosition); |
| 77 | linPosition += worldCenterOfMass; |
| 78 | } |
| 79 | |
| 80 | // Update angular momentum |
| 81 | angMomentum = angMomentum + torque * delta; |
| 82 | |
| 83 | // Update linear position, momentum |
| 84 | linPosition = linPosition + linVelocity * delta; |
| 85 | linMomentum = linMomentum + force * delta; |
| 86 | linVelocity = linMomentum * oneOverMass; |
| 87 | |
| 88 | // Update dependent state variables |
| 89 | updateInertialTensor(); |
| 90 | updateVelocity(); |
| 91 | updateCenterOfMass(); |
| 92 | } |
| 93 | |
| 94 | void Rigid::updateVelocity() |
| 95 | { |