void jfRigidBody_x86::integrate(jfReal timeStep)
{
//Linear Accel
if(!m_IsAwake)
{
return;
}
jfVector3_x86 angularAccel;
(*m_LastFrameAccel) = (*m_Accel);
(*m_LastFrameAccel).addScaledVector((*m_ForceAccum), m_InverseMass);
m_InverseInertiaTensorWorld->transform(*m_TorqueAccum, &angularAccel);
//Update velocity and rotation
m_Velocity->addScaledVector((*m_LastFrameAccel), timeStep);
m_Rotation->addScaledVector(angularAccel, timeStep);
//Drag
(*m_Velocity) *= jfRealPow(m_LinearDamping, timeStep);
(*m_Rotation) *= jfRealPow(m_AngularDamping, timeStep);
//Adjust position and orientation
m_Pos->addScaledVector((*m_Velocity), timeStep);
m_Orientation->addScaledVector((*m_Rotation), timeStep);
//Drag
(*m_Velocity) *= jfRealPow(m_LinearDamping, timeStep);
(*m_Rotation) *= jfRealPow(m_AngularDamping, timeStep);
calculateDerivedData();
clearAccumulators();
// Update the kinetic energy store, and possibly put the body to
// sleep.
if (m_CanSleep) {
jfReal currentMotion = m_Velocity->dotProduct(*m_Velocity) +
m_Rotation->dotProduct(*m_Rotation);
jfReal bias = jfRealPow(0.05, timeStep);
m_Motion = bias*m_Motion + (1-bias)*currentMotion;
if (m_Motion < SleepEpsilon)
{
setAwake(false);
}
else if (m_Motion > (10 * SleepEpsilon))
{
//Limit Motion
m_Motion = (10 * SleepEpsilon);
}
}
}
void RigidBody::integrate(Real duration)
{
Common::Vector2 lastFrameAcceleration = forceAccum * inverseMass;
Real angularAcceleration = torqueAccum * inverseInertiaTensor;
velocity += lastFrameAcceleration * duration;
rotation += angularAcceleration * duration;
velocity *= pow(damping, duration);
rotation *= pow(angularDamping, duration);
position += velocity * duration;
orientation = Common::Math::rotate2D(orientation, rotation * duration);
calculateDerivedData();
clearAccumulators();
}
