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;
}
void pfxSetupBallJoint(
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 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);
}
}
void pfxGetShapeAabbLargeTriMesh(const PfxShape &shape,PfxVector3 &aabbMin,PfxVector3 &aabbMax)
{
const PfxLargeTriMesh *largemesh = shape.getLargeTriMesh();
PfxVector3 half = absPerElem(PfxMatrix3(shape.getOffsetOrientation())) * largemesh->m_half;
aabbMin = shape.getOffsetPosition() - half;
aabbMax = shape.getOffsetPosition() + half;
}
void pfxGetShapeAabbConvexMesh(const PfxShape &shape,PfxVector3 &aabbMin,PfxVector3 &aabbMax)
{
const PfxConvexMesh *convex = shape.getConvexMesh();
PfxVector3 half = absPerElem(PfxMatrix3(shape.getOffsetOrientation())) * convex->m_half;
aabbMin = shape.getOffsetPosition() - half;
aabbMax = shape.getOffsetPosition() + half;
}
void pfxGetShapeAabbCylinder(const PfxShape &shape,PfxVector3 &aabbMin,PfxVector3 &aabbMax)
{
PfxVector3 capSize = absPerElem(PfxMatrix3(shape.getOffsetOrientation())) *
PfxVector3(shape.getCylinder().m_halfLen,shape.getCylinder().m_radius,shape.getCylinder().m_radius);
aabbMin = shape.getOffsetPosition() - capSize;
aabbMax = shape.getOffsetPosition() + capSize;
}
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 pfxGetShapeAabbBox(const PfxShape &shape,PfxVector3 &aabbMin,PfxVector3 &aabbMax)
{
PfxVector3 boxSize = absPerElem(PfxMatrix3(shape.getOffsetOrientation())) * shape.getBox().m_half;
aabbMin = shape.getOffsetPosition() - boxSize;
aabbMax = shape.getOffsetPosition() + boxSize;
}
void render(void)
{
render_begin();
const PfxVector3 colorWhite(1.0f);
const PfxVector3 colorGray(0.7f);
for(int i=0;i<physics_get_num_rigidbodies();i++) {
const PfxRigidState &state = physics_get_state(i);
const PfxCollidable &coll = physics_get_collidable(i);
PfxVector3 color = state.isAsleep()?colorGray:colorWhite;
PfxTransform3 rbT(state.getOrientation(), state.getPosition());
PfxShapeIterator itrShape(coll);
for(PfxUInt32 j=0;j<coll.getNumShapes();j++,++itrShape) {
const PfxShape &shape = *itrShape;
PfxTransform3 offsetT = shape.getOffsetTransform();
PfxTransform3 worldT = rbT * offsetT;
switch(shape.getType()) {
case kPfxShapeSphere:
render_sphere(
worldT,
color,
PfxFloatInVec(shape.getSphere().m_radius));
break;
case kPfxShapeBox:
render_box(
worldT,
color,
shape.getBox().m_half);
break;
case kPfxShapeCapsule:
render_capsule(
worldT,
color,
PfxFloatInVec(shape.getCapsule().m_radius),
PfxFloatInVec(shape.getCapsule().m_halfLen));
break;
case kPfxShapeCylinder:
render_cylinder(
worldT,
color,
PfxFloatInVec(shape.getCylinder().m_radius),
PfxFloatInVec(shape.getCylinder().m_halfLen));
break;
case kPfxShapeConvexMesh:
render_mesh(
worldT,
color,
convexMeshId);
break;
case kPfxShapeLargeTriMesh:
render_mesh(
worldT,
color,
landscapeMeshId);
break;
default:
break;
}
}
}
render_debug_begin();
#ifdef ENABLE_DEBUG_DRAW_CONTACT
for(int i=0;i<physics_get_num_contacts();i++) {
const PfxContactManifold &contact = physics_get_contact(i);
const PfxRigidState &stateA = physics_get_state(contact.getRigidBodyIdA());
const PfxRigidState &stateB = physics_get_state(contact.getRigidBodyIdB());
for(int j=0;j<contact.getNumContacts();j++) {
const PfxContactPoint &cp = contact.getContactPoint(j);
PfxVector3 pA = stateA.getPosition()+rotate(stateA.getOrientation(),pfxReadVector3(cp.m_localPointA));
const float w = 0.05f;
render_debug_line(pA+PfxVector3(-w,0.0f,0.0f),pA+PfxVector3(w,0.0f,0.0f),PfxVector3(0,0,1));
render_debug_line(pA+PfxVector3(0.0f,-w,0.0f),pA+PfxVector3(0.0f,w,0.0f),PfxVector3(0,0,1));
render_debug_line(pA+PfxVector3(0.0f,0.0f,-w),pA+PfxVector3(0.0f,0.0f,w),PfxVector3(0,0,1));
}
}
#endif
#ifdef ENABLE_DEBUG_DRAW_AABB
for(int i=0;i<physics_get_num_rigidbodies();i++) {
const PfxRigidState &state = physics_get_state(i);
const PfxCollidable &coll = physics_get_collidable(i);
PfxVector3 center = state.getPosition() + coll.getCenter();
PfxVector3 half = absPerElem(PfxMatrix3(state.getOrientation())) * coll.getHalf();
render_debug_box(center,half,PfxVector3(1,0,0));
}
#endif
#ifdef ENABLE_DEBUG_DRAW_ISLAND
const PfxIsland *island = physics_get_islands();
if(island) {
for(PfxUInt32 i=0;i<pfxGetNumIslands(island);i++) {
PfxIslandUnit *islandUnit = pfxGetFirstUnitInIsland(island,i);
PfxVector3 aabbMin(SCE_PFX_FLT_MAX);
PfxVector3 aabbMax(-SCE_PFX_FLT_MAX);
for(;islandUnit!=NULL;islandUnit=pfxGetNextUnitInIsland(islandUnit)) {
const PfxRigidState &state = physics_get_state(pfxGetUnitId(islandUnit));
const PfxCollidable &coll = physics_get_collidable(pfxGetUnitId(islandUnit));
PfxVector3 center = state.getPosition() + coll.getCenter();
PfxVector3 half = absPerElem(PfxMatrix3(state.getOrientation())) * coll.getHalf();
aabbMin = minPerElem(aabbMin,center-half);
aabbMax = maxPerElem(aabbMax,center+half);
}
render_debug_box((aabbMax+aabbMin)*0.5f,(aabbMax-aabbMin)*0.5f,PfxVector3(0,1,0));
}
}
#endif
for(int i=0;i<physics_get_num_rays();i++) {
const PfxRayInput& rayInput = physics_get_rayinput(i);
const PfxRayOutput& rayOutput = physics_get_rayoutput(i);
if(rayOutput.m_contactFlag) {
render_debug_line(
rayInput.m_startPosition,
rayOutput.m_contactPoint,
PfxVector3(1.0f,0.0f,1.0f));
render_debug_line(
rayOutput.m_contactPoint,
rayOutput.m_contactPoint+rayOutput.m_contactNormal,
PfxVector3(1.0f,0.0f,1.0f));
}
else {
render_debug_line(rayInput.m_startPosition,
rayInput.m_startPosition+rayInput.m_direction,
PfxVector3(0.5f,0.0f,0.5f));
}
}
extern bool doAreaRaycast;
extern PfxVector3 areaCenter;
extern PfxVector3 areaExtent;
if(doAreaRaycast) {
render_debug_box(areaCenter,areaExtent,PfxVector3(0,0,1));
}
render_debug_end();
render_end();
}
void pfxSetupContactConstraint(
PfxConstraintRow &constraintResponse,
PfxConstraintRow &constraintFriction1,
PfxConstraintRow &constraintFriction2,
PfxFloat penetrationDepth,
PfxFloat restitution,
PfxFloat friction,
const PfxVector3 &contactNormal,
const PfxVector3 &contactPointA,
const PfxVector3 &contactPointB,
const PfxRigidState &stateA,
const PfxRigidState &stateB,
PfxSolverBody &solverBodyA,
PfxSolverBody &solverBodyB,
PfxFloat separateBias,
PfxFloat timeStep
)
{
(void)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);
}
PfxMatrix3 K = PfxMatrix3::scale(PfxVector3(massInvA + massInvB)) -
crossMatrix(rA) * inertiaInvA * crossMatrix(rA) -
crossMatrix(rB) * inertiaInvB * crossMatrix(rB);
PfxVector3 vA = stateA.getLinearVelocity() + cross(stateA.getAngularVelocity(),rA);
PfxVector3 vB = stateB.getLinearVelocity() + cross(stateB.getAngularVelocity(),rB);
PfxVector3 vAB = vA-vB;
PfxVector3 tangent1,tangent2;
pfxGetPlaneSpace(contactNormal,tangent1,tangent2);
// Contact Constraint
{
PfxVector3 normal = contactNormal;
PfxFloat denom = dot(K*normal,normal);
constraintResponse.m_rhs = -(1.0f+restitution)*dot(vAB,normal); // velocity error
constraintResponse.m_rhs -= (separateBias * SCE_PFX_MIN(0.0f,penetrationDepth+SCE_PFX_CONTACT_SLOP)) / timeStep; // position error
constraintResponse.m_rhs /= denom;
constraintResponse.m_jacDiagInv = 1.0f/denom;
constraintResponse.m_lowerLimit = 0.0f;
constraintResponse.m_upperLimit = SCE_PFX_FLT_MAX;
pfxStoreVector3(normal,constraintResponse.m_normal);
}
// Friction Constraint 1
{
PfxVector3 normal = tangent1;
PfxFloat denom = dot(K*normal,normal);
constraintFriction1.m_jacDiagInv = 1.0f/denom;
constraintFriction1.m_rhs = -dot(vAB,normal);
constraintFriction1.m_rhs *= constraintFriction1.m_jacDiagInv;
constraintFriction1.m_lowerLimit = 0.0f;
constraintFriction1.m_upperLimit = SCE_PFX_FLT_MAX;
pfxStoreVector3(normal,constraintFriction1.m_normal);
}
// Friction Constraint 2
{
PfxVector3 normal = tangent2;
PfxFloat denom = dot(K*normal,normal);
constraintFriction2.m_jacDiagInv = 1.0f/denom;
constraintFriction2.m_rhs = -dot(vAB,normal);
constraintFriction2.m_rhs *= constraintFriction2.m_jacDiagInv;
constraintFriction2.m_lowerLimit = 0.0f;
constraintFriction2.m_upperLimit = SCE_PFX_FLT_MAX;
pfxStoreVector3(normal,constraintFriction2.m_normal);
}
}
