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