void pfxSetupSwingTwistJoint( PfxJoint &joint, const PfxRigidState &stateA, const PfxRigidState &stateB, PfxSolverBody &solverBodyA, PfxSolverBody &solverBodyB, PfxFloat timeStep ) { PfxVector3 rA = rotate(solverBodyA.m_orientation,joint.m_anchorA); PfxVector3 rB = rotate(solverBodyB.m_orientation,joint.m_anchorB); PfxVector3 vA = stateA.getLinearVelocity() + cross(stateA.getAngularVelocity(),rA); PfxVector3 vB = stateB.getLinearVelocity() + cross(stateB.getAngularVelocity(),rB); PfxVector3 vAB = vA-vB; PfxVector3 angAB = stateA.getAngularVelocity() - stateB.getAngularVelocity(); PfxVector3 distance = (stateA.getPosition() + rA) - (stateB.getPosition() + rB); PfxMatrix3 worldFrameA,worldFrameB; worldFrameA = PfxMatrix3(solverBodyA.m_orientation) * joint.m_frameA; worldFrameB = PfxMatrix3(solverBodyB.m_orientation) * joint.m_frameB; // Linear Constraint PfxMatrix3 K = PfxMatrix3::scale(PfxVector3(solverBodyA.m_massInv + solverBodyB.m_massInv)) - crossMatrix(rA) * solverBodyA.m_inertiaInv * crossMatrix(rA) - crossMatrix(rB) * solverBodyB.m_inertiaInv * crossMatrix(rB); for(int c=0;c<3;c++) { PfxJointConstraint &jointConstraint = joint.m_constraints[c]; PfxConstraintRow &constraint = jointConstraint.m_constraintRow; PfxVector3 normal = worldFrameA[c]; PfxFloat posErr = dot(distance,-normal); PfxFloat lowerLimit = -jointConstraint.m_maxImpulse; PfxFloat upperLimit = jointConstraint.m_maxImpulse; PfxFloat velocityAmp = 1.0f; pfxCalcLinearLimit(jointConstraint,posErr,velocityAmp,lowerLimit,upperLimit); PfxFloat denom = dot(K*normal,normal); constraint.m_rhs = -velocityAmp*dot(vAB,normal); constraint.m_rhs -= (jointConstraint.m_bias * (-posErr)) / timeStep; constraint.m_rhs *= jointConstraint.m_weight/denom; constraint.m_jacDiagInv = jointConstraint.m_weight*velocityAmp/denom; constraint.m_lowerLimit = lowerLimit; constraint.m_upperLimit = upperLimit; pfxStoreVector3(normal,constraint.m_normal); } PfxVector3 axis[3]; PfxFloat angle[3]; pfxCalcJointAngleSwingTwist(worldFrameA,worldFrameB,angle,axis); // Angular Constraint for(int c=3;c<6;c++) { PfxJointConstraint &jointConstraint = joint.m_constraints[c]; PfxConstraintRow &constraint = jointConstraint.m_constraintRow; PfxVector3 normal = axis[c-3]; PfxFloat posErr = angle[c-3]; PfxFloat lowerLimit = -jointConstraint.m_maxImpulse; PfxFloat upperLimit = jointConstraint.m_maxImpulse; PfxFloat velocityAmp = 1.0f; pfxCalcAngularLimit(jointConstraint,posErr,velocityAmp,lowerLimit,upperLimit); PfxFloat denom = dot((solverBodyA.m_inertiaInv+solverBodyB.m_inertiaInv)*normal,normal); constraint.m_rhs = -velocityAmp*dot(angAB,normal); // velocity error constraint.m_rhs -= (jointConstraint.m_bias * (-posErr)) / timeStep; // position error constraint.m_rhs *= jointConstraint.m_weight/denom; constraint.m_jacDiagInv = jointConstraint.m_weight*velocityAmp/denom; constraint.m_lowerLimit = lowerLimit; constraint.m_upperLimit = upperLimit; pfxStoreVector3(normal,constraint.m_normal); } }