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(); }