2 Physics Effects Copyright(C) 2010 Sony Computer Entertainment Inc.
\r
5 Physics Effects is open software; you can redistribute it and/or
\r
6 modify it under the terms of the BSD License.
\r
8 Physics Effects is distributed in the hope that it will be useful,
\r
9 but WITHOUT ANY WARRANTY; without even the implied warranty of
\r
10 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
\r
11 See the BSD License for more details.
\r
13 A copy of the BSD License is distributed with
\r
14 Physics Effects under the filename: physics_effects_license.txt
\r
17 #include "physics_func.h"
\r
18 #include "../common/perf_func.h"
\r
20 ///////////////////////////////////////////////////////////////////////////////
\r
23 #define NUM_RIGIDBODIES 5000
\r
24 #define NUM_JOINTS 5000
\r
25 #define NUM_CONTACTS 4000
\r
27 const float timeStep = 0.016f;
\r
28 const float separateBias = 0.1f;
\r
33 PfxVector3 worldCenter(0.0f);
\r
34 PfxVector3 worldExtent(500.0f);
\r
38 PfxRigidState states[NUM_RIGIDBODIES];
\r
39 PfxRigidBody bodies[NUM_RIGIDBODIES];
\r
40 PfxCollidable collidables[NUM_RIGIDBODIES];
\r
41 PfxSolverBody solverBodies[NUM_RIGIDBODIES];
\r
42 int numRigidBodies = 0;
\r
44 //J ?????????????????
\r
45 //E Large mesh for representing a landscape
\r
46 #include "landscape.h"
\r
47 PfxLargeTriMesh gLargeMesh;
\r
52 PfxConvexMesh gConvex;
\r
56 PfxBroadphaseProxy proxies[NUM_RIGIDBODIES];
\r
60 PfxConstraintPair jointPairs[NUM_JOINTS];
\r
61 PfxJoint joints[NUM_JOINTS];
\r
66 unsigned int pairSwap;
\r
67 unsigned int numPairs[2];
\r
68 PfxBroadphasePair pairsBuff[2][NUM_CONTACTS];
\r
72 PfxContactManifold contacts[NUM_CONTACTS];
\r
75 PfxUInt32 contactIdPool[NUM_CONTACTS];
\r
76 int numContactIdPool;
\r
79 //E Temporary buffers
\r
80 #define POOL_BYTES (5*1024*1024)
\r
81 unsigned char SCE_PFX_ALIGNED(128) poolBuff[POOL_BYTES];
\r
83 //J ????????????????
\r
84 //E Stack allocator for temporary buffers
\r
85 PfxHeapManager pool(poolBuff,POOL_BYTES);
\r
87 // ARA begin insert new code
\r
88 //E task manager for parallel demo
\r
89 #define NUM_THREADS 1
\r
90 PfxTaskManager *taskManager = NULL;
\r
91 //E need enough bytes for NUM_THREADS PfxTaskArg objects, with the space rounded up to fill a 16-bit aligned space
\r
92 #define TASK_MANAGER_POOL_BYTES 1024
\r
93 unsigned char SCE_PFX_ALIGNED(16) taskPoolBuff[TASK_MANAGER_POOL_BYTES];
\r
96 ///////////////////////////////////////////////////////////////////////////////
\r
97 // Simulation Function
\r
103 pairSwap = 1-pairSwap;
\r
105 unsigned int &numPreviousPairs = numPairs[1-pairSwap];
\r
106 unsigned int &numCurrentPairs = numPairs[pairSwap];
\r
107 PfxBroadphasePair *previousPairs = pairsBuff[1-pairSwap];
\r
108 PfxBroadphasePair *currentPairs = pairsBuff[pairSwap];
\r
110 //J ?????????????????
\r
111 //E Find the axis along which all rigid bodies are most widely positioned
\r
114 PfxVector3 s(0.0f),s2(0.0f);
\r
115 for(int i=0;i<numRigidBodies;i++) {
\r
116 PfxVector3 c = states[i].getPosition();
\r
118 s2 += mulPerElem(c,c);
\r
120 PfxVector3 v = s2 - mulPerElem(s,s) / (float)numRigidBodies;
\r
121 if(v[1] > v[0]) axis = 1;
\r
122 if(v[2] > v[axis]) axis = 2;
\r
125 //J ???????????????
\r
126 //E Create broadpahse proxies
\r
128 for(int i=0;i<numRigidBodies;i++) {
\r
129 pfxUpdateBroadphaseProxy(proxies[i],states[i],collidables[i],worldCenter,worldExtent,axis);
\r
132 int workBytes = sizeof(PfxBroadphaseProxy) * numRigidBodies;
\r
133 void *workBuff = pool.allocate(workBytes);
\r
135 pfxParallelSort(proxies,numRigidBodies,workBuff,workBytes);
\r
137 pool.deallocate(workBuff);
\r
141 //E Find overlapped pairs
\r
143 PfxFindPairsParam findPairsParam;
\r
144 findPairsParam.pairBytes = pfxGetPairBytesOfFindPairs(NUM_CONTACTS);
\r
145 findPairsParam.pairBuff = pool.allocate(findPairsParam.pairBytes);
\r
146 findPairsParam.workBytes = pfxGetWorkBytesOfFindPairs(NUM_CONTACTS);
\r
147 findPairsParam.workBuff = pool.allocate(findPairsParam.workBytes);
\r
148 findPairsParam.proxies = proxies;
\r
149 findPairsParam.numProxies = numRigidBodies;
\r
150 findPairsParam.maxPairs = NUM_CONTACTS;
\r
151 findPairsParam.axis = axis;
\r
153 PfxFindPairsResult findPairsResult;
\r
155 int ret = pfxFindPairs(findPairsParam,findPairsResult);
\r
156 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxFindPairs failed %d\n",ret);
\r
158 pool.deallocate(findPairsParam.workBuff);
\r
161 //E Decompose overlapped pairs into 3 arrays
\r
162 PfxDecomposePairsParam decomposePairsParam;
\r
163 decomposePairsParam.pairBytes = pfxGetPairBytesOfDecomposePairs(numPreviousPairs,findPairsResult.numPairs);
\r
164 decomposePairsParam.pairBuff = pool.allocate(decomposePairsParam.pairBytes);
\r
165 decomposePairsParam.workBytes = pfxGetWorkBytesOfDecomposePairs(numPreviousPairs,findPairsResult.numPairs);
\r
166 decomposePairsParam.workBuff = pool.allocate(decomposePairsParam.workBytes);
\r
167 decomposePairsParam.previousPairs = previousPairs;
\r
168 decomposePairsParam.numPreviousPairs = numPreviousPairs;
\r
169 decomposePairsParam.currentPairs = findPairsResult.pairs; // Set pairs from pfxFindPairs()
\r
170 decomposePairsParam.numCurrentPairs = findPairsResult.numPairs; // Set the number of pairs from pfxFindPairs()
\r
172 PfxDecomposePairsResult decomposePairsResult;
\r
174 ret = pfxDecomposePairs(decomposePairsParam,decomposePairsResult);
\r
175 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxDecomposePairs failed %d\n",ret);
\r
177 pool.deallocate(decomposePairsParam.workBuff);
\r
179 PfxBroadphasePair *outNewPairs = decomposePairsResult.outNewPairs;
\r
180 PfxBroadphasePair *outKeepPairs = decomposePairsResult.outKeepPairs;
\r
181 PfxBroadphasePair *outRemovePairs = decomposePairsResult.outRemovePairs;
\r
182 PfxUInt32 numOutNewPairs = decomposePairsResult.numOutNewPairs;
\r
183 PfxUInt32 numOutKeepPairs = decomposePairsResult.numOutKeepPairs;
\r
184 PfxUInt32 numOutRemovePairs = decomposePairsResult.numOutRemovePairs;
\r
186 //J ?????????????????
\r
187 //E Put removed contacts into the contact pool
\r
188 for(PfxUInt32 i=0;i<numOutRemovePairs;i++) {
\r
189 contactIdPool[numContactIdPool++] = pfxGetContactId(outRemovePairs[i]);
\r
192 //J ??????????????????
\r
193 //E Add new contacts and initialize
\r
194 for(PfxUInt32 i=0;i<numOutNewPairs;i++) {
\r
196 if(numContactIdPool > 0) {
\r
197 cId = contactIdPool[--numContactIdPool];
\r
200 cId = numContacts++;
\r
202 if(cId >= NUM_CONTACTS) {
\r
205 SCE_PFX_ASSERT(cId < NUM_CONTACTS);
\r
206 pfxSetContactId(outNewPairs[i],cId);
\r
207 PfxContactManifold &contact = contacts[cId];
\r
208 contact.reset(pfxGetObjectIdA(outNewPairs[i]),pfxGetObjectIdB(outNewPairs[i]));
\r
212 //E Merge 'new' and 'keep' pairs
\r
213 numCurrentPairs = 0;
\r
214 for(PfxUInt32 i=0;i<numOutKeepPairs;i++) {
\r
215 currentPairs[numCurrentPairs++] = outKeepPairs[i];
\r
217 for(PfxUInt32 i=0;i<numOutNewPairs;i++) {
\r
218 currentPairs[numCurrentPairs++] = outNewPairs[i];
\r
221 pool.deallocate(decomposePairsParam.pairBuff);
\r
222 pool.deallocate(findPairsParam.pairBuff);
\r
226 int workBytes = sizeof(PfxBroadphasePair) * numCurrentPairs;
\r
227 void *workBuff = pool.allocate(workBytes);
\r
229 pfxParallelSort(currentPairs,numCurrentPairs,workBuff,workBytes);
\r
231 pool.deallocate(workBuff);
\r
237 unsigned int numCurrentPairs = numPairs[pairSwap];
\r
238 PfxBroadphasePair *currentPairs = pairsBuff[pairSwap];
\r
241 //E Detect collisions
\r
243 PfxDetectCollisionParam param;
\r
244 param.contactPairs = currentPairs;
\r
245 param.numContactPairs = numCurrentPairs;
\r
246 param.offsetContactManifolds = contacts;
\r
247 param.offsetRigidStates = states;
\r
248 param.offsetCollidables = collidables;
\r
249 param.numRigidBodies = numRigidBodies;
\r
251 // ARA begin insert new code
\r
252 #ifdef USE_PTHREADS
\r
253 int ret = pfxDetectCollision(param, taskManager);
\r
257 int ret = pfxDetectCollision(param);
\r
259 // ARA begin insert new code
\r
263 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxDetectCollision failed %d\n",ret);
\r
267 //E Refresh contacts
\r
269 PfxRefreshContactsParam param;
\r
270 param.contactPairs = currentPairs;
\r
271 param.numContactPairs = numCurrentPairs;
\r
272 param.offsetContactManifolds = contacts;
\r
273 param.offsetRigidStates = states;
\r
274 param.numRigidBodies = numRigidBodies;
\r
276 // ARA begin insert new code
\r
277 #ifdef USE_PTHREADS
\r
278 int ret = pfxRefreshContacts(param, taskManager);
\r
282 int ret = pfxRefreshContacts(param);
\r
284 // ARA begin insert new code
\r
288 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxRefreshContacts failed %d\n",ret);
\r
292 void constraintSolver()
\r
296 unsigned int numCurrentPairs = numPairs[pairSwap];
\r
297 PfxBroadphasePair *currentPairs = pairsBuff[pairSwap];
\r
299 pc.countBegin("setup solver bodies");
\r
301 PfxSetupSolverBodiesParam param;
\r
302 param.states = states;
\r
303 param.bodies = bodies;
\r
304 param.solverBodies = solverBodies;
\r
305 param.numRigidBodies = numRigidBodies;
\r
307 // ARA begin insert new code
\r
308 #ifdef USE_PTHREADS
\r
309 int ret = pfxSetupSolverBodies(param, taskManager);
\r
313 int ret = pfxSetupSolverBodies(param);
\r
315 // ARA begin insert new code
\r
319 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxSetupSolverBodies failed %d\n",ret);
\r
323 pc.countBegin("setup contact constraints");
\r
325 PfxSetupContactConstraintsParam param;
\r
326 param.contactPairs = currentPairs;
\r
327 param.numContactPairs = numCurrentPairs;
\r
328 param.offsetContactManifolds = contacts;
\r
329 param.offsetRigidStates = states;
\r
330 param.offsetRigidBodies = bodies;
\r
331 param.offsetSolverBodies = solverBodies;
\r
332 param.numRigidBodies = numRigidBodies;
\r
333 param.timeStep = timeStep;
\r
334 param.separateBias = separateBias;
\r
336 // ARA begin insert new code
\r
337 #ifdef USE_PTHREADS
\r
338 int ret = pfxSetupContactConstraints(param, taskManager);
\r
342 int ret = pfxSetupContactConstraints(param);
\r
344 // ARA begin insert new code
\r
348 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxSetupJointConstraints failed %d\n",ret);
\r
352 pc.countBegin("setup joint constraints");
\r
354 PfxSetupJointConstraintsParam param;
\r
355 param.jointPairs = jointPairs;
\r
356 param.numJointPairs = numJoints;
\r
357 param.offsetJoints = joints;
\r
358 param.offsetRigidStates = states;
\r
359 param.offsetRigidBodies = bodies;
\r
360 param.offsetSolverBodies = solverBodies;
\r
361 param.numRigidBodies = numRigidBodies;
\r
362 param.timeStep = timeStep;
\r
364 for(int i=0;i<numJoints;i++) {
\r
365 pfxUpdateJointPairs(jointPairs[i],i,joints[i],states[joints[i].m_rigidBodyIdA],states[joints[i].m_rigidBodyIdB]);
\r
368 // ARA begin insert new code
\r
369 #ifdef USE_PTHREADS
\r
370 int ret = pfxSetupJointConstraints(param, taskManager);
\r
374 int ret = pfxSetupJointConstraints(param);
\r
376 // ARA begin insert new code
\r
380 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxSetupJointConstraints failed %d\n",ret);
\r
384 pc.countBegin("solve constraints");
\r
386 PfxSolveConstraintsParam param;
\r
387 param.workBytes = pfxGetWorkBytesOfSolveConstraints(numRigidBodies,numCurrentPairs,numJoints);
\r
388 param.workBuff = pool.allocate(param.workBytes);
\r
389 param.contactPairs = currentPairs;
\r
390 param.numContactPairs = numCurrentPairs;
\r
391 param.offsetContactManifolds = contacts;
\r
392 param.jointPairs = jointPairs;
\r
393 param.numJointPairs = numJoints;
\r
394 param.offsetJoints = joints;
\r
395 param.offsetRigidStates = states;
\r
396 param.offsetSolverBodies = solverBodies;
\r
397 param.numRigidBodies = numRigidBodies;
\r
398 param.iteration = iteration;
\r
400 // ARA begin insert new code
\r
401 #ifdef USE_PTHREADS
\r
402 int ret = pfxSolveConstraints(param, taskManager);
\r
406 int ret = pfxSolveConstraints(param);
\r
408 // ARA begin insert new code
\r
412 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxSolveConstraints failed %d\n",ret);
\r
414 pool.deallocate(param.workBuff);
\r
423 PfxUpdateRigidStatesParam param;
\r
424 param.states = states;
\r
425 param.bodies = bodies;
\r
426 param.numRigidBodies = numRigidBodies;
\r
427 param.timeStep = timeStep;
\r
429 // ARA begin insert new code
\r
430 #ifdef USE_PTHREADS
\r
431 pfxUpdateRigidStates(param, taskManager);
\r
435 pfxUpdateRigidStates(param);
\r
437 // ARA begin insert new code
\r
442 void physics_simulate()
\r
446 for(int i=1;i<numRigidBodies;i++) {
\r
447 pfxApplyExternalForce(states[i],bodies[i],bodies[i].getMass()*PfxVector3(0.0f,-9.8f,0.0f),PfxVector3(0.0f),timeStep);
\r
450 perf_push_marker("broadphase");
\r
451 pc.countBegin("broadphase");
\r
456 perf_push_marker("collision");
\r
457 pc.countBegin("collision");
\r
462 perf_push_marker("solver");
\r
463 pc.countBegin("solver");
\r
464 constraintSolver();
\r
468 perf_push_marker("integrate");
\r
469 pc.countBegin("integrate");
\r
476 if(frame%100 == 0) {
\r
477 float broadphaseTime = pc.getCountTime(0);
\r
478 float collisionTime = pc.getCountTime(2);
\r
479 float solverTime = pc.getCountTime(4);
\r
480 float integrateTime = pc.getCountTime(6);
\r
481 SCE_PFX_PRINTF("frame %3d broadphase %.2f collision %.2f solver %.2f integrate %.2f | total %.2f\n",frame,
\r
482 broadphaseTime,collisionTime,solverTime,integrateTime,
\r
483 broadphaseTime+collisionTime+solverTime+integrateTime);
\r
487 ///////////////////////////////////////////////////////////////////////////////
\r
490 void createBrick(int id,const PfxVector3 &pos,const PfxQuat &rot,const PfxVector3 &boxSize,PfxFloat mass)
\r
492 PfxBox box(boxSize);
\r
496 collidables[id].reset();
\r
497 collidables[id].addShape(shape);
\r
498 collidables[id].finish();
\r
499 bodies[id].reset();
\r
500 bodies[id].setRestitution(0.0f);
\r
501 bodies[id].setMass(mass);
\r
502 bodies[id].setInertia(pfxCalcInertiaBox(boxSize,mass));
\r
503 states[id].reset();
\r
504 states[id].setPosition(pos);
\r
505 states[id].setOrientation(rot);
\r
506 states[id].setMotionType(kPfxMotionTypeActive);
\r
507 states[id].setRigidBodyId(id);
\r
510 void createWall(const PfxVector3 &offsetPosition,int stackSize,const PfxVector3 &boxSize)
\r
512 PfxFloat bodyMass = 0.5f;
\r
514 PfxFloat diffX = boxSize[0] * 1.02f;
\r
515 PfxFloat diffY = boxSize[1] * 1.02f;
\r
516 PfxFloat diffZ = boxSize[2] * 1.02f;
\r
518 PfxFloat offset = -stackSize * (diffZ * 2.0f) * 0.5f;
\r
519 PfxVector3 pos(0.0f, diffY, 0.0f);
\r
522 for(int i=0;i<stackSize;i++) {
\r
523 pos[2] = offset + (PfxFloat)i * (diffZ * 2.0f);
\r
525 createBrick(numRigidBodies++,offsetPosition+pos,PfxQuat::identity(),boxSize,bodyMass);
\r
528 pos[1] += (diffY * 2.0f);
\r
533 void createPyramid(const PfxVector3 &offsetPosition,int stackSize,const PfxVector3 &boxSize)
\r
535 PfxFloat space = 0.0001f;
\r
536 PfxVector3 pos(0.0f, boxSize[1], 0.0f);
\r
538 PfxFloat diffX = boxSize[0] * 1.02f;
\r
539 PfxFloat diffY = boxSize[1] * 1.02f;
\r
540 PfxFloat diffZ = boxSize[2] * 1.02f;
\r
542 PfxFloat offsetX = -stackSize * (diffX * 2.0f + space) * 0.5f;
\r
543 PfxFloat offsetZ = -stackSize * (diffZ * 2.0f + space) * 0.5f;
\r
545 for(int j=0;j<stackSize;j++) {
\r
546 pos[2] = offsetZ + (PfxFloat)j * (diffZ * 2.0f + space);
\r
547 for(int i=0;i<stackSize;i++) {
\r
548 pos[0] = offsetX + (PfxFloat)i * (diffX * 2.0f + space);
\r
549 createBrick(numRigidBodies++,offsetPosition+pos,PfxQuat::identity(),boxSize,1.0f);
\r
554 pos[1] += (diffY * 2.0f + space);
\r
559 void createTowerCircle(const PfxVector3 &offsetPosition,int stackSize,int rotSize,const PfxVector3 &boxSize)
\r
561 PfxFloat radius = 1.3f * rotSize * boxSize[0] / SCE_PFX_PI;
\r
563 // create active boxes
\r
564 PfxQuat rotY = PfxQuat::identity();
\r
565 PfxFloat posY = boxSize[1];
\r
567 for(int i=0;i<stackSize;i++) {
\r
568 for(int j=0;j<rotSize;j++) {
\r
569 createBrick(numRigidBodies++,offsetPosition+rotate(rotY,PfxVector3(0.0f , posY, radius)),rotY,boxSize,0.5f);
\r
571 rotY *= PfxQuat::rotationY(SCE_PFX_PI/(rotSize*0.5f));
\r
574 posY += boxSize[1] * 2.0f;
\r
575 rotY *= PfxQuat::rotationY(SCE_PFX_PI/(PfxFloat)rotSize);
\r
579 void createScenePrimitives()
\r
583 int id = numRigidBodies++;
\r
584 PfxSphere sphere(1.0f);
\r
587 shape.setSphere(sphere);
\r
588 collidables[id].reset();
\r
589 collidables[id].addShape(shape);
\r
590 collidables[id].finish();
\r
591 bodies[id].reset();
\r
592 bodies[id].setMass(1.0f);
\r
593 bodies[id].setInertia(pfxCalcInertiaSphere(1.0f,1.0f));
\r
594 states[id].reset();
\r
595 states[id].setPosition(PfxVector3(-5.0f,5.0f,0.0f));
\r
596 states[id].setMotionType(kPfxMotionTypeActive);
\r
597 states[id].setRigidBodyId(id);
\r
602 int id = numRigidBodies++;
\r
603 PfxBox box(1.0f,1.0f,1.0f);
\r
607 collidables[id].reset();
\r
608 collidables[id].addShape(shape);
\r
609 collidables[id].finish();
\r
610 bodies[id].reset();
\r
611 bodies[id].setMass(1.0f);
\r
612 bodies[id].setInertia(pfxCalcInertiaBox(PfxVector3(1.0f),1.0f));
\r
613 states[id].reset();
\r
614 states[id].setPosition(PfxVector3(0.0f,5.0f,5.0f));
\r
615 states[id].setMotionType(kPfxMotionTypeActive);
\r
616 states[id].setRigidBodyId(id);
\r
621 int id = numRigidBodies++;
\r
622 PfxCapsule capsule(1.5f,0.5f);
\r
625 shape.setCapsule(capsule);
\r
626 collidables[id].reset();
\r
627 collidables[id].addShape(shape);
\r
628 collidables[id].finish();
\r
629 bodies[id].reset();
\r
630 bodies[id].setMass(2.0f);
\r
631 bodies[id].setInertia(pfxCalcInertiaCylinderX(2.0f,0.5f,2.0f));
\r
632 states[id].reset();
\r
633 states[id].setPosition(PfxVector3(5.0f,5.0f,0.0f));
\r
634 states[id].setMotionType(kPfxMotionTypeActive);
\r
635 states[id].setRigidBodyId(id);
\r
640 int id = numRigidBodies++;
\r
641 PfxCylinder cylinder(0.5f,1.5f);
\r
644 shape.setCylinder(cylinder);
\r
645 collidables[id].reset();
\r
646 collidables[id].addShape(shape);
\r
647 collidables[id].finish();
\r
648 bodies[id].reset();
\r
649 bodies[id].setMass(3.0f);
\r
650 bodies[id].setInertia(pfxCalcInertiaCylinderX(0.5f,1.5f,3.0f));
\r
651 states[id].reset();
\r
652 states[id].setPosition(PfxVector3(0.0f,10.0f,0.0f));
\r
653 states[id].setMotionType(kPfxMotionTypeActive);
\r
654 states[id].setRigidBodyId(id);
\r
659 PfxCreateConvexMeshParam param;
\r
661 param.verts = BarrelVtx;
\r
662 param.numVerts = BarrelVtxCount;
\r
663 param.vertexStrideBytes = sizeof(float)*6;
\r
665 param.triangles = BarrelIdx;
\r
666 param.numTriangles = BarrelIdxCount/3;
\r
667 param.triangleStrideBytes = sizeof(unsigned short)*3;
\r
669 PfxInt32 ret = pfxCreateConvexMesh(gConvex,param);
\r
670 if(ret != SCE_PFX_OK) {
\r
671 SCE_PFX_PRINTF("Can't create gConvex mesh.\n");
\r
674 int id = numRigidBodies++;
\r
677 shape.setConvexMesh(&gConvex);
\r
678 collidables[id].reset();
\r
679 collidables[id].addShape(shape);
\r
680 collidables[id].finish();
\r
681 bodies[id].reset();
\r
682 bodies[id].setMass(3.0f);
\r
683 bodies[id].setInertia(pfxCalcInertiaSphere(1.0f,1.0f));
\r
684 states[id].reset();
\r
685 states[id].setPosition(PfxVector3(0.0f,15.0f,0.0f));
\r
686 states[id].setMotionType(kPfxMotionTypeActive);
\r
687 states[id].setRigidBodyId(id);
\r
690 // combined primitives
\r
692 int id = numRigidBodies++;
\r
694 //E Both shapes and incides buffer have to be kept when creating a combined shape.
\r
695 static PfxShape shapes[3];
\r
696 PfxUInt16 shapeIds[3]={0,1,2};
\r
697 collidables[id].reset(shapes,shapeIds,3);
\r
699 PfxBox box(0.5f,0.5f,1.5f);
\r
703 shape.setOffsetPosition(PfxVector3(-2.0f,0.0f,0.0f));
\r
704 collidables[id].addShape(shape);
\r
707 PfxBox box(0.5f,1.5f,0.5f);
\r
711 shape.setOffsetPosition(PfxVector3(2.0f,0.0f,0.0f));
\r
712 collidables[id].addShape(shape);
\r
715 PfxCapsule cap(1.5f,0.5f);
\r
718 shape.setCapsule(cap);
\r
719 collidables[id].addShape(shape);
\r
721 collidables[id].finish();
\r
722 bodies[id].reset();
\r
723 bodies[id].setMass(3.0f);
\r
724 bodies[id].setInertia(pfxCalcInertiaBox(PfxVector3(2.5f,1.0f,1.0f),3.0f));
\r
725 states[id].reset();
\r
726 states[id].setPosition(PfxVector3(0.0f,5.0f,0.0f));
\r
727 states[id].setMotionType(kPfxMotionTypeActive);
\r
728 states[id].setRigidBodyId(id);
\r
732 void createSceneJoints()
\r
736 int startId = numRigidBodies;
\r
738 PfxVector3 boxSize(1.0f);
\r
739 PfxFloat boxMass = 1.0f;
\r
741 for(int i=0;i<n;i++) {
\r
742 createBrick(numRigidBodies++,PfxVector3(0,3.0f+i*2.5f*boxSize[1],0),PfxQuat::identity(),boxSize,boxMass);
\r
745 for(int i=startId;i<startId+n;i++) {
\r
746 PfxRigidState &stateA = states[i];
\r
747 PfxRigidState &stateB = states[(i+1)%numRigidBodies];
\r
749 if(i == numRigidBodies-1) {
\r
750 anchor = stateA.getPosition() + PfxVector3(0,boxSize[1],0);
\r
753 anchor = ( stateA.getPosition() + stateB.getPosition() ) * 0.5f;
\r
756 PfxSwingTwistJointInitParam jparam;
\r
757 jparam.anchorPoint = anchor;
\r
758 jparam.twistAxis = PfxVector3(0,1,0);
\r
760 pfxInitializeSwingTwistJoint(joints[numJoints],stateA,stateB,jparam);
\r
761 joints[numJoints].m_constraints[4].m_damping = 0.1f;
\r
762 joints[numJoints].m_constraints[5].m_damping = 0.1f;
\r
764 pfxUpdateJointPairs(jointPairs[numJoints],numJoints,joints[numJoints],stateA,stateB);
\r
766 SCE_PFX_ASSERT(numJoints<NUM_JOINTS);
\r
770 states[startId].setLinearVelocity(PfxVector3(0,0,5));
\r
771 states[startId].setLinearDamping(0.95f);
\r
772 states[startId].setAngularDamping(0.95f);
\r
775 void createSceneStacking()
\r
777 const float cubeSize = 1.0f;
\r
779 createPyramid(PfxVector3(-20.0f,0.0f,0.0f),12,PfxVector3(cubeSize,cubeSize,cubeSize));
\r
780 createWall(PfxVector3(-2.0f,0.0f,0.0f),12,PfxVector3(cubeSize,cubeSize,cubeSize));
\r
781 createWall(PfxVector3(4.0f,0.0f,0.0f),12,PfxVector3(cubeSize,cubeSize,cubeSize));
\r
782 createWall(PfxVector3(10.0f,0.0f,0.0f),12,PfxVector3(cubeSize,cubeSize,cubeSize));
\r
783 createTowerCircle(PfxVector3(25.0f,0.0f,0.0f),8,24,PfxVector3(cubeSize,cubeSize,cubeSize));
\r
785 // createTowerCircle(PfxVector3(0.0f,0.0f,0.0f),8,24,PfxVector3(1));
\r
788 void createSceneBoxGround()
\r
790 int id = numRigidBodies++;
\r
791 PfxBox box(150.0f,2.5f,150.0f);
\r
795 collidables[id].reset();
\r
796 collidables[id].addShape(shape);
\r
797 collidables[id].finish();
\r
798 bodies[id].reset();
\r
799 states[id].reset();
\r
800 states[id].setPosition(PfxVector3(0.0f,-2.5f,0.0f));
\r
801 states[id].setMotionType(kPfxMotionTypeFixed);
\r
802 states[id].setRigidBodyId(id);
\r
805 void createSceneLandscape()
\r
807 PfxCreateLargeTriMeshParam param;
\r
809 param.verts = LargeMeshVtx;
\r
810 param.numVerts = LargeMeshVtxCount;
\r
811 param.vertexStrideBytes = sizeof(float)*6;
\r
813 param.triangles = LargeMeshIdx;
\r
814 param.numTriangles = LargeMeshIdxCount/3;
\r
815 param.triangleStrideBytes = sizeof(unsigned short)*3;
\r
817 if(gLargeMesh.m_numIslands > 0) {
\r
818 pfxReleaseLargeTriMesh(gLargeMesh);
\r
821 PfxInt32 ret = pfxCreateLargeTriMesh(gLargeMesh,param);
\r
822 if(ret != SCE_PFX_OK) {
\r
823 SCE_PFX_PRINTF("Can't create large mesh.\n");
\r
826 int id = numRigidBodies++;
\r
829 shape.setLargeTriMesh(&gLargeMesh);
\r
830 collidables[id].reset();
\r
831 collidables[id].addShape(shape);
\r
832 collidables[id].finish();
\r
833 bodies[id].reset();
\r
834 states[id].reset();
\r
835 states[id].setPosition(PfxVector3(0.0f,-5.0f,0.0f));
\r
836 states[id].setOrientation(PfxQuat::rotationX(0.5f)*PfxQuat::rotationY(0.7f));
\r
837 states[id].setMotionType(kPfxMotionTypeFixed);
\r
838 states[id].setRigidBodyId(id);
\r
841 void physics_create_scene(int sceneId)
\r
843 const int numScenes = 4;
\r
844 int sid = sceneId % numScenes;
\r
851 numContactIdPool = 0;
\r
856 case 0: // simple primitives
\r
857 createSceneBoxGround();
\r
858 createScenePrimitives();
\r
862 createSceneBoxGround();
\r
863 createSceneJoints();
\r
866 case 2: // stacking
\r
867 createSceneBoxGround();
\r
868 createSceneStacking();
\r
871 case 3: // landscape
\r
872 createSceneLandscape();
\r
873 createScenePrimitives();
\r
877 SCE_PFX_PRINTF("----- Size of rigid body buffer ------\n");
\r
878 SCE_PFX_PRINTF(" size * num = total\n");
\r
879 SCE_PFX_PRINTF("PfxRigidState %5d * %5d = %5d bytes\n",sizeof(PfxRigidState),numRigidBodies,sizeof(PfxRigidState)*numRigidBodies);
\r
880 SCE_PFX_PRINTF("PfxRigidBody %5d * %5d = %5d bytes\n",sizeof(PfxRigidBody),numRigidBodies,sizeof(PfxRigidBody)*numRigidBodies);
\r
881 SCE_PFX_PRINTF("PfxCollidable %5d * %5d = %5d bytes\n",sizeof(PfxCollidable),numRigidBodies,sizeof(PfxCollidable)*numRigidBodies);
\r
882 SCE_PFX_PRINTF("PfxJoint %5d * %5d = %5d bytes\n",sizeof(PfxJoint),numJoints,sizeof(PfxJoint)*numJoints);
\r
883 SCE_PFX_PRINTF("PfxSolverBody %5d * %5d = %5d bytes\n",sizeof(PfxSolverBody),numRigidBodies,sizeof(PfxSolverBody)*numRigidBodies);
\r
884 SCE_PFX_PRINTF("PfxBroadphaseProxy %5d * %5d = %5d bytes\n",sizeof(PfxBroadphaseProxy),numRigidBodies,sizeof(PfxBroadphaseProxy)*numRigidBodies);
\r
885 SCE_PFX_PRINTF("PfxContactManifold %5d * %5d = %5d bytes\n",sizeof(PfxContactManifold),NUM_CONTACTS,sizeof(PfxContactManifold)*NUM_CONTACTS);
\r
886 SCE_PFX_PRINTF("PfxBroadphasePair %5d * %5d = %5d bytes\n",sizeof(PfxBroadphasePair),NUM_CONTACTS,sizeof(PfxBroadphasePair)*NUM_CONTACTS);
\r
889 (sizeof(PfxRigidState) + sizeof(PfxRigidBody) + sizeof(PfxCollidable) + sizeof(PfxSolverBody) + sizeof(PfxBroadphaseProxy)) * numRigidBodies +
\r
890 (sizeof(PfxContactManifold) + sizeof(PfxBroadphasePair)) * NUM_CONTACTS;
\r
891 SCE_PFX_PRINTF("----------------------------------------------------------\n");
\r
892 SCE_PFX_PRINTF("Total %5d bytes\n",totalBytes);
\r
894 // ARA begin insert new code
\r
895 #ifdef USE_PTHREADS
\r
898 taskManager = PfxCreateTaskManagerPthreads(NUM_THREADS, NUM_THREADS, taskPoolBuff, TASK_MANAGER_POOL_BYTES);
\r
899 taskManager->initialize();
\r
905 ///////////////////////////////////////////////////////////////////////////////
\r
906 // Initialize / Finalize Engine
\r
908 bool physics_init()
\r
913 void physics_release()
\r
915 // ARA begin insert new code
\r
917 taskManager->finalize();
\r
921 ///////////////////////////////////////////////////////////////////////////////
\r
924 PfxVector3 physics_pick_start(const PfxVector3 &p1,const PfxVector3 &p2)
\r
926 return PfxVector3(0.0f);
\r
929 void physics_pick_update(const PfxVector3 &p)
\r
933 void physics_pick_end()
\r
937 ///////////////////////////////////////////////////////////////////////////////
\r
940 int physics_get_num_rigidbodies()
\r
942 return numRigidBodies;
\r
945 const PfxRigidState& physics_get_state(int id)
\r
950 const PfxRigidBody& physics_get_body(int id)
\r
955 const PfxCollidable& physics_get_collidable(int id)
\r
957 return collidables[id];
\r
960 int physics_get_num_contacts()
\r
962 return numPairs[pairSwap];
\r
965 const PfxContactManifold &physics_get_contact(int id)
\r
967 return contacts[pfxGetConstraintId(pairsBuff[pairSwap][id])];
\r