//! 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;
}
protected void initSolverBody( btSolverBody solverBody, btRigidBody rb, double timeStep )
{
//btRigidBody rb = collisionObject != null ? btRigidBody.upcast( collisionObject ) : null;
solverBody.m_deltaLinearVelocity = btVector3.Zero;
solverBody.m_deltaAngularVelocity = btVector3.Zero;
solverBody.m_pushVelocity = btVector3.Zero;
solverBody.m_turnVelocity = btVector3.Zero;
solverBody.modified = false;
solverBody.pushed = false;
if( rb != null )
{
solverBody.m_worldTransform = rb.m_worldTransform;
btVector3 tmp = new btVector3( rb.getInvMass() );
btVector3 tmp2;
btVector3 tmp3;
rb.getLinearFactor( out tmp2 );
tmp.Mult( ref tmp2, out tmp3 );
solverBody.internalSetInvMass( ref tmp3 );
solverBody.m_originalBody = rb;
rb.getAngularFactor( out solverBody.m_angularFactor );
rb.getLinearFactor( out solverBody.m_linearFactor );
rb.getLinearVelocity( out solverBody.m_linearVelocity );
rb.getAngularVelocity( out solverBody.m_angularVelocity );
rb.m_totalForce.Mult( rb.m_inverseMass * timeStep, out solverBody.m_externalForceImpulse );
//= rb.getTotalForce() * rb.getInvMass() * timeStep;
rb.m_invInertiaTensorWorld.Apply( ref rb.m_totalTorque, out tmp );
tmp.Mult( timeStep, out solverBody.m_externalTorqueImpulse );
btScalar.Dbg( "Setup external impulses " + solverBody.m_externalForceImpulse + " " + solverBody.m_externalTorqueImpulse );
///solverBody.m_externalTorqueImpulse = rb.getTotalTorque() * rb.getInvInertiaTensorWorld() * timeStep;
}
else
{
solverBody.modified = false;
solverBody.m_worldTransform = btTransform.Identity;
solverBody.internalSetInvMass( ref btVector3.Zero );
solverBody.m_originalBody = null;
solverBody.m_angularFactor = btVector3.One;
solverBody.m_linearFactor = btVector3.One;
solverBody.m_linearVelocity = btVector3.Zero;
solverBody.m_angularVelocity = btVector3.Zero;
solverBody.m_externalForceImpulse = btVector3.Zero;
solverBody.m_externalTorqueImpulse = btVector3.Zero;
}
}