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 }
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); } // ARA begin insert new code #ifdef __ARM_NEON__ pfxSolveLinearConstraintRowNEON(constraintResponse, solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,massInvA,inertiaInvA,rA, solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,massInvB,inertiaInvB,rB); #else // __ARM_NEON__ // ARA end pfxSolveLinearConstraintRow(constraintResponse, solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,massInvA,inertiaInvA,rA, solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,massInvB,inertiaInvB,rB); // ARA begin insert new code #endif // __ARM_NEON__ // ARA end PfxFloat mf = friction*fabsf(constraintResponse.m_accumImpulse); constraintFriction1.m_lowerLimit = -mf; constraintFriction1.m_upperLimit = mf; constraintFriction2.m_lowerLimit = -mf; constraintFriction2.m_upperLimit = mf; // ARA begin insert new code #ifdef __ARM_NEON__ pfxSolveLinearConstraintRowNEON(constraintFriction1, solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,massInvA,inertiaInvA,rA, solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,massInvB,inertiaInvB,rB); pfxSolveLinearConstraintRowNEON(constraintFriction2, solverBodyA.m_deltaLinearVelocity,solverBodyA.m_deltaAngularVelocity,massInvA,inertiaInvA,rA, solverBodyB.m_deltaLinearVelocity,solverBodyB.m_deltaAngularVelocity,massInvB,inertiaInvB,rB); #else // __ARM_NEON__ // ARA end 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); // ARA begin insert new code #endif // __ARM_NEON__ // ARA end }