PfxInt32 pfxInitializeBallJoint(PfxJoint &joint, const PfxRigidState &stateA,const PfxRigidState &stateB, const PfxBallJointInitParam ¶m) { joint.m_active = 1; joint.m_numConstraints = 3; joint.m_userData = NULL; joint.m_type = kPfxJointBall; joint.m_rigidBodyIdA = stateA.getRigidBodyId(); joint.m_rigidBodyIdB = stateB.getRigidBodyId(); for(int i=0;i<3;i++) { joint.m_constraints[i].reset(); } joint.m_constraints[0].m_lock = SCE_PFX_JOINT_LOCK_FIX; joint.m_constraints[1].m_lock = SCE_PFX_JOINT_LOCK_FIX; joint.m_constraints[2].m_lock = SCE_PFX_JOINT_LOCK_FIX; PfxMatrix3 rotA = transpose(PfxMatrix3(stateA.getOrientation())); PfxMatrix3 rotB = transpose(PfxMatrix3(stateB.getOrientation())); joint.m_anchorA = rotA * (param.anchorPoint - stateA.getPosition()); joint.m_anchorB = rotB * (param.anchorPoint - stateB.getPosition()); joint.m_frameA = PfxMatrix3::identity(); joint.m_frameB = rotB * PfxMatrix3(stateA.getOrientation()) * joint.m_frameA; return SCE_PFX_OK; }
PfxInt32 pfxInitializeSwingTwistJoint(PfxJoint &joint, const PfxRigidState &stateA,const PfxRigidState &stateB, const PfxSwingTwistJointInitParam ¶m) { joint.m_active = 1; joint.m_numConstraints = 6; joint.m_userData = NULL; joint.m_type = kPfxJointSwingtwist; joint.m_rigidBodyIdA = stateA.getRigidBodyId(); joint.m_rigidBodyIdB = stateB.getRigidBodyId(); for(int i=0;i<6;i++) { joint.m_constraints[i].reset(); } joint.m_constraints[0].m_lock = SCE_PFX_JOINT_LOCK_FIX; joint.m_constraints[1].m_lock = SCE_PFX_JOINT_LOCK_FIX; joint.m_constraints[2].m_lock = SCE_PFX_JOINT_LOCK_FIX; joint.m_constraints[3].m_lock = SCE_PFX_JOINT_LOCK_LIMIT; joint.m_constraints[4].m_lock = SCE_PFX_JOINT_LOCK_LIMIT; joint.m_constraints[5].m_lock = SCE_PFX_JOINT_LOCK_FREE; // Set twist angle limit if(param.twistLowerAngle > param.twistUpperAngle || !SCE_PFX_RANGE_CHECK(param.twistLowerAngle,-SCE_PFX_PI-0.01f,SCE_PFX_PI+0.01f) || !SCE_PFX_RANGE_CHECK(param.twistUpperAngle,-SCE_PFX_PI-0.01f,SCE_PFX_PI+0.01f)) { return SCE_PFX_ERR_OUT_OF_RANGE; } joint.m_constraints[3].m_movableLowerLimit = param.twistLowerAngle; joint.m_constraints[3].m_movableUpperLimit = param.twistUpperAngle; // Set swing angle limit if(param.swingLowerAngle > param.swingUpperAngle || !SCE_PFX_RANGE_CHECK(param.swingLowerAngle,0.0f,SCE_PFX_PI+0.01f) || !SCE_PFX_RANGE_CHECK(param.swingUpperAngle,0.0f,SCE_PFX_PI+0.01f)) { return SCE_PFX_ERR_OUT_OF_RANGE; } joint.m_constraints[4].m_movableLowerLimit = param.swingLowerAngle; joint.m_constraints[4].m_movableUpperLimit = param.swingUpperAngle; // Calc joint frame PfxMatrix3 rotA = transpose(PfxMatrix3(stateA.getOrientation())); PfxMatrix3 rotB = transpose(PfxMatrix3(stateB.getOrientation())); PfxVector3 axisInA = rotA * normalize(param.twistAxis); joint.m_anchorA = rotA * (param.anchorPoint - stateA.getPosition()); joint.m_anchorB = rotB * (param.anchorPoint - stateB.getPosition()); PfxVector3 axis1, axis2; pfxGetPlaneSpace(axisInA, axis1, axis2 ); joint.m_frameA = PfxMatrix3(axisInA, axis1, axis2); joint.m_frameB = rotB * PfxMatrix3(stateA.getOrientation()) * joint.m_frameA; return SCE_PFX_OK; }
PfxInt32 pfxInitializeSliderJoint(PfxJoint &joint, const PfxRigidState &stateA,const PfxRigidState &stateB, const PfxSliderJointInitParam ¶m) { joint.m_active = 1; joint.m_numConstraints = 6; joint.m_userData = NULL; joint.m_type = kPfxJointSlider; joint.m_rigidBodyIdA = stateA.getRigidBodyId(); joint.m_rigidBodyIdB = stateB.getRigidBodyId(); for(int i=0;i<6;i++) { joint.m_constraints[i].reset(); } if(param.lowerDistance == 0.0f && param.upperDistance == 0.0f) { joint.m_constraints[0].m_lock = SCE_PFX_JOINT_LOCK_FREE; } else { joint.m_constraints[0].m_lock = SCE_PFX_JOINT_LOCK_LIMIT; } joint.m_constraints[1].m_lock = SCE_PFX_JOINT_LOCK_FIX; joint.m_constraints[2].m_lock = SCE_PFX_JOINT_LOCK_FIX; joint.m_constraints[3].m_lock = SCE_PFX_JOINT_LOCK_FIX; joint.m_constraints[4].m_lock = SCE_PFX_JOINT_LOCK_FIX; joint.m_constraints[5].m_lock = SCE_PFX_JOINT_LOCK_FIX; if(param.lowerDistance > param.upperDistance ) { return SCE_PFX_ERR_OUT_OF_RANGE; } joint.m_constraints[0].m_movableLowerLimit = param.lowerDistance; joint.m_constraints[0].m_movableUpperLimit = param.upperDistance; // Calc joint frame PfxMatrix3 rotA = transpose(PfxMatrix3(stateA.getOrientation())); PfxMatrix3 rotB = transpose(PfxMatrix3(stateB.getOrientation())); PfxVector3 axisInA = rotA * normalize(param.direction); joint.m_anchorA = rotA * (param.anchorPoint - stateA.getPosition()); joint.m_anchorB = rotB * (param.anchorPoint - stateB.getPosition()); PfxVector3 axis1, axis2; pfxGetPlaneSpace(axisInA, axis1, axis2 ); joint.m_frameA = PfxMatrix3(axisInA, axis1, axis2); joint.m_frameB = rotB * PfxMatrix3(stateA.getOrientation()) * joint.m_frameA; return SCE_PFX_OK; }