void pfxSolveSwingTwistJoint(
PfxJoint &joint,
PfxSolverBody &solverBodyA,
PfxSolverBody &solverBodyB
)
{
PfxVector3 rA = rotate(solverBodyA.m_orientation,joint.m_anchorA);
PfxVector3 rB = rotate(solverBodyB.m_orientation,joint.m_anchorB);
// Linear Constraint
pfxSolveLinearConstraintRow(joint.m_constraints[0].m_constraintRow,
solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,solverBodyA.m_massInv,solverBodyA.m_inertiaInv,rA,
solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,solverBodyB.m_massInv,solverBodyB.m_inertiaInv,rB);
pfxSolveLinearConstraintRow(joint.m_constraints[1].m_constraintRow,
solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,solverBodyA.m_massInv,solverBodyA.m_inertiaInv,rA,
solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,solverBodyB.m_massInv,solverBodyB.m_inertiaInv,rB);
pfxSolveLinearConstraintRow(joint.m_constraints[2].m_constraintRow,
solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,solverBodyA.m_massInv,solverBodyA.m_inertiaInv,rA,
solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,solverBodyB.m_massInv,solverBodyB.m_inertiaInv,rB);
// Angular Constraint
pfxSolveAngularConstraintRow(joint.m_constraints[3].m_constraintRow,
solverBodyA.m_deltaAngularVelocity,solverBodyA.m_inertiaInv,rA,
solverBodyB.m_deltaAngularVelocity,solverBodyB.m_inertiaInv,rB);
pfxSolveAngularConstraintRow(joint.m_constraints[4].m_constraintRow,
solverBodyA.m_deltaAngularVelocity,solverBodyA.m_inertiaInv,rA,
solverBodyB.m_deltaAngularVelocity,solverBodyB.m_inertiaInv,rB);
pfxSolveAngularConstraintRow(joint.m_constraints[5].m_constraintRow,
solverBodyA.m_deltaAngularVelocity,solverBodyA.m_inertiaInv,rA,
solverBodyB.m_deltaAngularVelocity,solverBodyB.m_inertiaInv,rB);
}
void pfxSolveContactConstraint(
PfxConstraintRow &constraintResponse,
PfxConstraintRow &constraintFriction1,
PfxConstraintRow &constraintFriction2,
const PfxVector3 &contactPointA,
const PfxVector3 &contactPointB,
PfxSolverBody &solverBodyA,
PfxSolverBody &solverBodyB,
PfxFloat friction
)
{
PfxVector3 rA = rotate(solverBodyA.m_orientation,contactPointA);
PfxVector3 rB = rotate(solverBodyB.m_orientation,contactPointB);
PfxFloat massInvA = solverBodyA.m_massInv;
PfxFloat massInvB = solverBodyB.m_massInv;
PfxMatrix3 inertiaInvA = solverBodyA.m_inertiaInv;
PfxMatrix3 inertiaInvB = solverBodyB.m_inertiaInv;
if(solverBodyA.m_motionType == kPfxMotionTypeOneWay) {
massInvB = 0.0f;
inertiaInvB = PfxMatrix3(0.0f);
}
if(solverBodyB.m_motionType == kPfxMotionTypeOneWay) {
massInvA = 0.0f;
inertiaInvA = PfxMatrix3(0.0f);
}
pfxSolveLinearConstraintRow(constraintResponse,
solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,massInvA,inertiaInvA,rA,
solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,massInvB,inertiaInvB,rB);
PfxFloat mf = friction*fabsf(constraintResponse.m_accumImpulse);
constraintFriction1.m_lowerLimit = -mf;
constraintFriction1.m_upperLimit = mf;
constraintFriction2.m_lowerLimit = -mf;
constraintFriction2.m_upperLimit = mf;
pfxSolveLinearConstraintRow(constraintFriction1,
solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,massInvA,inertiaInvA,rA,
solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,massInvB,inertiaInvB,rB);
pfxSolveLinearConstraintRow(constraintFriction2,
solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,massInvA,inertiaInvA,rA,
solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,massInvB,inertiaInvB,rB);
}
void pfxSolveBallJoint(
PfxJoint &joint,
PfxSolverBody &solverBodyA,
PfxSolverBody &solverBodyB
)
{
PfxVector3 rA = rotate(solverBodyA.m_orientation,joint.m_anchorA);
PfxVector3 rB = rotate(solverBodyB.m_orientation,joint.m_anchorB);
// Linear Constraint
// ARA begin insert new code
#ifdef __ARM_NEON__
pfxSolveLinearConstraintRowNEON(joint.m_constraints[0].m_constraintRow,
solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,solverBodyA.m_massInv,solverBodyA.m_inertiaInv,rA,
solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,solverBodyB.m_massInv,solverBodyB.m_inertiaInv,rB);
pfxSolveLinearConstraintRowNEON(joint.m_constraints[1].m_constraintRow,
solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,solverBodyA.m_massInv,solverBodyA.m_inertiaInv,rA,
solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,solverBodyB.m_massInv,solverBodyB.m_inertiaInv,rB);
pfxSolveLinearConstraintRowNEON(joint.m_constraints[2].m_constraintRow,
solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,solverBodyA.m_massInv,solverBodyA.m_inertiaInv,rA,
solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,solverBodyB.m_massInv,solverBodyB.m_inertiaInv,rB);
#else // __ARM_NEON__
// ARA end
pfxSolveLinearConstraintRow(joint.m_constraints[0].m_constraintRow,
solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,solverBodyA.m_massInv,solverBodyA.m_inertiaInv,rA,
solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,solverBodyB.m_massInv,solverBodyB.m_inertiaInv,rB);
pfxSolveLinearConstraintRow(joint.m_constraints[1].m_constraintRow,
solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,solverBodyA.m_massInv,solverBodyA.m_inertiaInv,rA,
solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,solverBodyB.m_massInv,solverBodyB.m_inertiaInv,rB);
pfxSolveLinearConstraintRow(joint.m_constraints[2].m_constraintRow,
solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,solverBodyA.m_massInv,solverBodyA.m_inertiaInv,rA,
solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,solverBodyB.m_massInv,solverBodyB.m_inertiaInv,rB);
// ARA begin insert new code
#endif // __ARM_NEON__
// ARA end
}
