//! apply the correction impulses for two bodies
//double solveAngularLimits(double timeStep,ref btVector3 axis, double jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
internal double solveAngularLimits(
double timeStep, ref btVector3 axis, double jacDiagABInv,
btRigidBody body0, btRigidBody body1 )
{
if( needApplyTorques() == false ) return 0.0f;
double target_velocity = m_targetVelocity;
double 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
btVector3 angVelA = body0.getAngularVelocity();
btVector3 angVelB = body1.getAngularVelocity();
btVector3 vel_diff;
vel_diff = angVelA - angVelB;
double rel_vel = axis.dot( vel_diff );
// correction velocity
double motor_relvel = m_limitSoftness * ( target_velocity - m_damping * rel_vel );
if( motor_relvel < btScalar.SIMD_EPSILON && motor_relvel > -btScalar.SIMD_EPSILON )
{
return 0.0f;//no need for applying force
}
// correction impulse
double unclippedMotorImpulse = ( 1 + m_bounce ) * motor_relvel * jacDiagABInv;
// clip correction impulse
double 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
double lo = (double)( -btScalar.BT_LARGE_FLOAT );
double hi = (double)( btScalar.BT_LARGE_FLOAT );
double oldaccumImpulse = m_accumulatedImpulse;
double sum = oldaccumImpulse + clippedMotorImpulse;
m_accumulatedImpulse = sum > hi ? btScalar.BT_ZERO : sum < lo ? btScalar.BT_ZERO : sum;
clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
btVector3 motorImp; axis.Mult( clippedMotorImpulse, out motorImp );
body0.applyTorqueImpulse( ref motorImp );
btVector3 tmp;
motorImp.Invert( out tmp );
body1.applyTorqueImpulse( ref tmp );
return clippedMotorImpulse;
}