//! 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; }