//! apply the correction impulses for two bodies
public float SolveAngularLimits(float timeStep,ref Vector3 axis, float jacDiagABInv,RigidBody body0, RigidBody body1)
{
if (NeedApplyTorques() == false)
{
return 0.0f;
}
float target_velocity = m_targetVelocity;
float maxMotorForce = m_maxMotorForce;
//current error correction
if (m_currentLimit!=0)
{
target_velocity = -m_stopERP*m_currentLimitError/(timeStep);
maxMotorForce = m_maxLimitForce;
}
maxMotorForce *= timeStep;
// current velocity difference
Vector3 angVelA = Vector3.Zero;
body0.InternalGetAngularVelocity(ref angVelA);
Vector3 angVelB = Vector3.Zero;
body1.InternalGetAngularVelocity(ref angVelB);
Vector3 vel_diff = angVelA-angVelB;
float rel_vel = Vector3.Dot(axis,vel_diff);
// correction velocity
float motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel);
if ( motor_relvel < MathUtil.SIMD_EPSILON && motor_relvel > -MathUtil.SIMD_EPSILON )
{
return 0.0f;//no need for applying force
}
// correction impulse
float unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
// clip correction impulse
float clippedMotorImpulse;
///@todo: should clip against accumulated impulse
if (unclippedMotorImpulse>0.0f)
{
clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
}
else
{
clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
}
// sort with accumulated impulses
float lo = float.MinValue;
float hi = float.MaxValue;
float oldaccumImpulse = m_accumulatedImpulse;
float sum = oldaccumImpulse + clippedMotorImpulse;
m_accumulatedImpulse = sum > hi ? 0f : sum < lo ? 0f : sum;
clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
Vector3 motorImp = clippedMotorImpulse * axis;
//body0.applyTorqueImpulse(motorImp);
//body1.applyTorqueImpulse(-motorImp);
body0.InternalApplyImpulse(Vector3.Zero, Vector3.TransformNormal(axis,body0.GetInvInertiaTensorWorld()),clippedMotorImpulse);
body1.InternalApplyImpulse(Vector3.Zero, Vector3.TransformNormal(axis,body1.GetInvInertiaTensorWorld()),-clippedMotorImpulse);
return clippedMotorImpulse;
}