///This is the scalar reference implementation of solving a single constraint row, the innerloop of the Projected Gauss Seidel/Sequential Impulse constraint solver
///Below are optional SSE2 and SSE4/FMA3 versions. We assume most hardware has SSE2. For SSE4/FMA3 we perform a CPU feature check.
public static double gResolveSingleConstraintRowGeneric_scalar_reference( btSolverBody body1, btSolverBody body2, btSolverConstraint c )
{
double deltaImpulse = c.m_rhs - (double)( c.m_appliedImpulse ) * c.m_cfm;
double deltaVel1Dotn = c.m_contactNormal1.dot( ref body1.m_deltaLinearVelocity )
+ c.m_relpos1CrossNormal.dot( ref body1.m_deltaAngularVelocity );
double deltaVel2Dotn = c.m_contactNormal2.dot( ref body2.m_deltaLinearVelocity )
+ c.m_relpos2CrossNormal.dot( ref body2.m_deltaAngularVelocity );
// double delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn;
deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
double sum = (double)( c.m_appliedImpulse ) + deltaImpulse;
if( sum < c.m_lowerLimit )
{
deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
c.m_appliedImpulse = c.m_lowerLimit;
}
else if( sum > c.m_upperLimit )
{
deltaImpulse = c.m_upperLimit - c.m_appliedImpulse;
c.m_appliedImpulse = c.m_upperLimit;
}
else
{
c.m_appliedImpulse = sum;
}
btScalar.Dbg( "Constraint applied impulse is " + c.m_appliedImpulse.ToString( "g17" ) );
btVector3 mass, val;
body1.internalGetInvMass( out mass );
mass.Mult( ref c.m_contactNormal1, out val );
body1.applyImpulse( ref val, ref c.m_angularComponentA, deltaImpulse );
body2.internalGetInvMass( out mass );
mass.Mult( ref c.m_contactNormal2, out val );
body2.applyImpulse( ref val, ref c.m_angularComponentB, deltaImpulse );
return deltaImpulse;
}
protected void resolveSplitPenetrationImpulseCacheFriendly(
btSolverBody body1,
btSolverBody body2,
btSolverConstraint c )
{
if( c.m_rhsPenetration != 0 )
{
gNumSplitImpulseRecoveries++;
double deltaImpulse = c.m_rhsPenetration - (double)( c.m_appliedPushImpulse ) * c.m_cfm;
btVector3 tmplin, tmpang;
body1.internalGetPushVelocity( out tmplin );
body1.internalGetTurnVelocity( out tmpang );
double deltaVel1Dotn = c.m_contactNormal1.dot( ref tmplin ) + c.m_relpos1CrossNormal.dot( ref tmpang );
body2.internalGetPushVelocity( out tmplin );
body2.internalGetTurnVelocity( out tmpang );
double deltaVel2Dotn = c.m_contactNormal2.dot( ref tmplin ) + c.m_relpos2CrossNormal.dot( ref tmpang );
deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
double sum = (double)( c.m_appliedPushImpulse ) + deltaImpulse;
if( sum < c.m_lowerLimit )
{
deltaImpulse = c.m_lowerLimit - c.m_appliedPushImpulse;
c.m_appliedPushImpulse = c.m_lowerLimit;
}
else
{
c.m_appliedPushImpulse = sum;
}
btVector3 mass, val;
if( !c.m_contactNormal1.isZero() && !c.m_angularComponentA.isZero() )
{
btScalar.Dbg( "push impulse setup from " + deltaImpulse.ToString( "g17" ) + " * " + c.m_contactNormal1.ToString() + " and " + c.m_angularComponentA.ToString() );
body1.internalGetInvMass( out mass );
mass.Mult( ref c.m_contactNormal1, out val );
body1.applyPushImpulse( ref val, ref c.m_angularComponentA, deltaImpulse );
}
if( !c.m_contactNormal2.isZero() && !c.m_angularComponentB.isZero() )
{
btScalar.Dbg( "push impulse setup from " + deltaImpulse.ToString( "g17" ) + " * " + c.m_contactNormal2.ToString() + " and " + c.m_angularComponentB.ToString() );
body2.internalGetInvMass( out mass );
mass.Mult( ref c.m_contactNormal2, out val );
body2.applyPushImpulse( ref val, ref c.m_angularComponentB, deltaImpulse );
}
}
}