internal double solveLinearAxis(
double timeStep,
double jacDiagABInv,
btRigidBody body1, btVector3 pointInA,
btRigidBody body2, btVector3 pointInB,
int limit_index,
btVector3 axis_normal_on_a,
btVector3 anchorPos )
{
///find relative velocity
// btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
// btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
btVector3 rel_pos1 = anchorPos - body1.m_worldTransform.m_origin;
btVector3 rel_pos2 = anchorPos - body2.m_worldTransform.m_origin;
btVector3 vel1 = body1.getVelocityInLocalPoint( ref rel_pos1 );
btVector3 vel2 = body2.getVelocityInLocalPoint( ref rel_pos2 );
btVector3 vel = vel1 - vel2;
double rel_vel = axis_normal_on_a.dot( vel );
/// apply displacement correction
//positional error (zeroth order error)
double depth = -( pointInA - pointInB ).dot( axis_normal_on_a );
double lo = (double)( -btScalar.BT_LARGE_FLOAT );
double hi = (double)( btScalar.BT_LARGE_FLOAT );
double minLimit = m_lowerLimit[limit_index];
double maxLimit = m_upperLimit[limit_index];
//handle the limits
if( minLimit < maxLimit )
{
{
if( depth > maxLimit )
{
depth -= maxLimit;
lo = btScalar.BT_ZERO;
}
else
{
if( depth < minLimit )
{
depth -= minLimit;
hi = btScalar.BT_ZERO;
}
else
{
return 0.0f;
}
}
}
}
double normalImpulse = m_limitSoftness * ( m_restitution * depth / timeStep - m_damping * rel_vel ) * jacDiagABInv;
double oldNormalImpulse = m_accumulatedImpulse[limit_index];
double sum = oldNormalImpulse + normalImpulse;
m_accumulatedImpulse[limit_index] = sum > hi ? btScalar.BT_ZERO : sum < lo ? btScalar.BT_ZERO : sum;
normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
body1.applyImpulse( ref impulse_vector, ref rel_pos1 );
btVector3 tmp;
impulse_vector.Invert( out tmp );
body2.applyImpulse( ref tmp, ref rel_pos2 );
return normalImpulse;
}