void createSceneJoints() { const int n = 10; int startId = numRigidBodies; PfxVector3 boxSize(1.0f); PfxFloat boxMass = 1.0f; for(int i=0;i<n;i++) { createBrick(numRigidBodies++,PfxVector3(0,3.0f+i*2.5f*boxSize[1],0),PfxQuat::identity(),boxSize,boxMass); } for(int i=startId;i<startId+n;i++) { PfxRigidState &stateA = states[i]; PfxRigidState &stateB = states[(i+1)%numRigidBodies]; PfxVector3 anchor; if(i == numRigidBodies-1) { anchor = stateA.getPosition() + PfxVector3(0,boxSize[1],0); } else { anchor = ( stateA.getPosition() + stateB.getPosition() ) * 0.5f; } PfxSwingTwistJointInitParam jparam; jparam.anchorPoint = anchor; jparam.twistAxis = PfxVector3(0,1,0); pfxInitializeSwingTwistJoint(joints[numJoints],stateA,stateB,jparam); joints[numJoints].m_constraints[4].m_damping = 0.1f; joints[numJoints].m_constraints[5].m_damping = 0.1f; pfxUpdateJointPairs(jointPairs[numJoints],numJoints,joints[numJoints],stateA,stateB); SCE_PFX_ASSERT(numJoints<NUM_JOINTS); numJoints++; } states[startId].setLinearVelocity(PfxVector3(0,0,5)); states[startId].setLinearDamping(0.95f); states[startId].setAngularDamping(0.95f); }
void constraintSolver() { PfxPerfCounter pc; pc.countBegin("setup solver bodies"); { PfxSetupSolverBodiesParam param; param.states = states; param.bodies = bodies; param.solverBodies = solverBodies; param.numRigidBodies = numRigidBodies; int ret = pfxSetupSolverBodies(param); if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxSetupSolverBodies failed %d\n",ret); } pc.countEnd(); pc.countBegin("setup contact constraints"); { PfxSetupContactConstraintsParam param; param.contactPairs = pairs; param.numContactPairs = numPairs; param.offsetContactManifolds = contacts; param.offsetRigidStates = states; param.offsetRigidBodies = bodies; param.offsetSolverBodies = solverBodies; param.numRigidBodies = numRigidBodies; param.timeStep = timeStep; param.separateBias = separateBias; int ret = pfxSetupContactConstraints(param); if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxSetupJointConstraints failed %d\n",ret); } pc.countEnd(); pc.countBegin("setup joint constraints"); { PfxSetupJointConstraintsParam param; param.jointPairs = jointPairs; param.numJointPairs = numJoints; param.offsetJoints = joints; param.offsetRigidStates = states; param.offsetRigidBodies = bodies; param.offsetSolverBodies = solverBodies; param.numRigidBodies = numRigidBodies; param.timeStep = timeStep; for(int i=0;i<numJoints;i++) { pfxUpdateJointPairs(jointPairs[i],i,joints[i],states[joints[i].m_rigidBodyIdA],states[joints[i].m_rigidBodyIdB]); } int ret = pfxSetupJointConstraints(param); if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxSetupJointConstraints failed %d\n",ret); } pc.countEnd(); pc.countBegin("solve constraints"); { PfxSolveConstraintsParam param; param.workBytes = pfxGetWorkBytesOfSolveConstraints(numRigidBodies,numPairs,numJoints); param.workBuff = pool.allocate(param.workBytes); param.contactPairs = pairs; param.numContactPairs = numPairs; param.offsetContactManifolds = contacts; param.jointPairs = jointPairs; param.numJointPairs = numJoints; param.offsetJoints = joints; param.offsetRigidStates = states; param.offsetSolverBodies = solverBodies; param.numRigidBodies = numRigidBodies; param.iteration = iteration; int ret = pfxSolveConstraints(param); if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxSolveConstraints failed %d\n",ret); pool.deallocate(param.workBuff); } pc.countEnd(); //pc.printCount(); }