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