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