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;
}