Initialize libbullet git in 2.0_beta.
[platform/upstream/libbullet.git] / Extras / PhysicsEffects / sample / api_physics_effects / 2_stable_parallel / physics_func.cpp
1 /*\r
2 Physics Effects Copyright(C) 2010 Sony Computer Entertainment Inc.\r
3 All rights reserved.\r
4 \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
7 \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
12 \r
13 A copy of the BSD License is distributed with\r
14 Physics Effects under the filename: physics_effects_license.txt\r
15 */\r
16 \r
17 #include "physics_func.h"\r
18 #include "../common/perf_func.h"\r
19 \r
20 ///////////////////////////////////////////////////////////////////////////////\r
21 // Simulation Data\r
22 \r
23 #define NUM_RIGIDBODIES 5000\r
24 #define NUM_JOINTS    5000\r
25 #define NUM_CONTACTS  4000\r
26 \r
27 const float timeStep = 0.016f;\r
28 const float separateBias = 0.1f;\r
29 int iteration = 5;\r
30 \r
31 //J ???????\r
32 //E World size\r
33 PfxVector3 worldCenter(0.0f);\r
34 PfxVector3 worldExtent(500.0f);\r
35 \r
36 //J ??\r
37 //E Rigid body\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
43 \r
44 //J ?????????????????\r
45 //E Large mesh for representing a landscape\r
46 #include "landscape.h"\r
47 PfxLargeTriMesh gLargeMesh;\r
48 \r
49 //J ?????\r
50 //E Convex Mesh\r
51 #include "barrel.h"\r
52 PfxConvexMesh gConvex;\r
53 \r
54 //J ????\r
55 //E Proxies\r
56 PfxBroadphaseProxy proxies[NUM_RIGIDBODIES];\r
57 \r
58 //J ?????\r
59 //E Joint\r
60 PfxConstraintPair jointPairs[NUM_JOINTS];\r
61 PfxJoint joints[NUM_JOINTS];\r
62 int numJoints = 0;\r
63 \r
64 //J ??\r
65 //E Pairs\r
66 unsigned int pairSwap;\r
67 unsigned int numPairs[2];\r
68 PfxBroadphasePair pairsBuff[2][NUM_CONTACTS];\r
69 \r
70 //J ?????\r
71 //E Contacts\r
72 PfxContactManifold contacts[NUM_CONTACTS];\r
73 int numContacts;\r
74 \r
75 PfxUInt32 contactIdPool[NUM_CONTACTS];\r
76 int numContactIdPool;\r
77 \r
78 //J ??????\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
82 \r
83 //J ????????????????\r
84 //E Stack allocator for temporary buffers\r
85 PfxHeapManager pool(poolBuff,POOL_BYTES);\r
86 \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
94 // ARA end\r
95 \r
96 ///////////////////////////////////////////////////////////////////////////////\r
97 // Simulation Function\r
98 \r
99 int frame = 0;\r
100 \r
101 void broadphase()\r
102 {\r
103         pairSwap = 1-pairSwap;\r
104 \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
109 \r
110         //J ?????????????????\r
111         //E Find the axis along which all rigid bodies are most widely positioned\r
112         int axis = 0;\r
113         {\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
117                         s += c;\r
118                         s2 += mulPerElem(c,c);\r
119                 }\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
123         }\r
124 \r
125         //J ???????????????\r
126         //E Create broadpahse proxies\r
127         {\r
128                 for(int i=0;i<numRigidBodies;i++) {\r
129                         pfxUpdateBroadphaseProxy(proxies[i],states[i],collidables[i],worldCenter,worldExtent,axis);\r
130                 }\r
131 \r
132                 int workBytes = sizeof(PfxBroadphaseProxy) * numRigidBodies;\r
133                 void *workBuff = pool.allocate(workBytes);\r
134                                 \r
135                 pfxParallelSort(proxies,numRigidBodies,workBuff,workBytes);\r
136 \r
137                 pool.deallocate(workBuff);\r
138         }\r
139 \r
140         //J ??????\r
141         //E Find overlapped pairs\r
142         {\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
152 \r
153                 PfxFindPairsResult findPairsResult;\r
154 \r
155                 int ret = pfxFindPairs(findPairsParam,findPairsResult);\r
156                 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxFindPairs failed %d\n",ret);\r
157                 \r
158                 pool.deallocate(findPairsParam.workBuff);\r
159 \r
160                 //J ??????\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
171 \r
172                 PfxDecomposePairsResult decomposePairsResult;\r
173 \r
174                 ret = pfxDecomposePairs(decomposePairsParam,decomposePairsResult);\r
175                 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxDecomposePairs failed %d\n",ret);\r
176 \r
177                 pool.deallocate(decomposePairsParam.workBuff);\r
178 \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
185 \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
190                 }\r
191 \r
192                 //J ??????????????????\r
193                 //E Add new contacts and initialize\r
194                 for(PfxUInt32 i=0;i<numOutNewPairs;i++) {\r
195                         int cId = 0;\r
196                         if(numContactIdPool > 0) {\r
197                                 cId = contactIdPool[--numContactIdPool];\r
198                         }\r
199                         else {\r
200                                 cId = numContacts++;\r
201                         }\r
202                         if(cId >= NUM_CONTACTS) {\r
203                                 cId = 0;\r
204                         }\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
209                 }\r
210 \r
211                 //J ????????????\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
216                 }\r
217                 for(PfxUInt32 i=0;i<numOutNewPairs;i++) {\r
218                         currentPairs[numCurrentPairs++] = outNewPairs[i];\r
219                 }\r
220                 \r
221                 pool.deallocate(decomposePairsParam.pairBuff);\r
222                 pool.deallocate(findPairsParam.pairBuff);\r
223         }\r
224         \r
225         {\r
226                 int workBytes = sizeof(PfxBroadphasePair) * numCurrentPairs;\r
227                 void *workBuff = pool.allocate(workBytes);\r
228                 \r
229                 pfxParallelSort(currentPairs,numCurrentPairs,workBuff,workBytes);\r
230                 \r
231                 pool.deallocate(workBuff);\r
232         }\r
233 }\r
234 \r
235 void collision()\r
236 {\r
237         unsigned int numCurrentPairs = numPairs[pairSwap];\r
238         PfxBroadphasePair *currentPairs = pairsBuff[pairSwap];\r
239         \r
240         //J ????\r
241         //E Detect collisions\r
242         {\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
250 \r
251 // ARA begin insert new code\r
252 #ifdef USE_PTHREADS\r
253                 int ret = pfxDetectCollision(param, taskManager);\r
254 #else\r
255 // ARA end\r
256 \r
257                 int ret = pfxDetectCollision(param);\r
258 \r
259 // ARA begin insert new code\r
260 #endif\r
261 // ARA end\r
262 \r
263                 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxDetectCollision failed %d\n",ret);\r
264         }\r
265 \r
266         //J ??????\r
267         //E Refresh contacts\r
268         {\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
275 \r
276 // ARA begin insert new code\r
277 #ifdef USE_PTHREADS\r
278                 int ret = pfxRefreshContacts(param, taskManager);\r
279 #else\r
280 // ARA end\r
281 \r
282                 int ret = pfxRefreshContacts(param);\r
283 \r
284 // ARA begin insert new code\r
285 #endif\r
286 // ARA end\r
287 \r
288                 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxRefreshContacts failed %d\n",ret);\r
289         }\r
290 }\r
291 \r
292 void constraintSolver()\r
293 {\r
294         PfxPerfCounter pc;\r
295 \r
296         unsigned int numCurrentPairs = numPairs[pairSwap];\r
297         PfxBroadphasePair *currentPairs = pairsBuff[pairSwap];\r
298 \r
299         pc.countBegin("setup solver bodies");\r
300         {\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
306 \r
307 // ARA begin insert new code\r
308 #ifdef USE_PTHREADS\r
309                 int ret = pfxSetupSolverBodies(param, taskManager);\r
310 #else\r
311 // ARA end\r
312 \r
313                 int ret = pfxSetupSolverBodies(param);\r
314 \r
315 // ARA begin insert new code\r
316 #endif\r
317 // ARA end\r
318 \r
319                 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxSetupSolverBodies failed %d\n",ret);\r
320         }\r
321         pc.countEnd();\r
322 \r
323         pc.countBegin("setup contact constraints");\r
324         {\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
335 \r
336 // ARA begin insert new code\r
337 #ifdef USE_PTHREADS\r
338                 int ret = pfxSetupContactConstraints(param, taskManager);\r
339 #else\r
340 // ARA end\r
341 \r
342                 int ret = pfxSetupContactConstraints(param);\r
343 \r
344 // ARA begin insert new code\r
345 #endif\r
346 // ARA end\r
347 \r
348                 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxSetupJointConstraints failed %d\n",ret);\r
349         }\r
350         pc.countEnd();\r
351 \r
352         pc.countBegin("setup joint constraints");\r
353         {\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
363 \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
366                 }\r
367 \r
368 // ARA begin insert new code\r
369 #ifdef USE_PTHREADS\r
370                 int ret = pfxSetupJointConstraints(param, taskManager);\r
371 #else\r
372 // ARA end\r
373 \r
374                 int ret = pfxSetupJointConstraints(param);\r
375 \r
376 // ARA begin insert new code\r
377 #endif\r
378 // ARA end\r
379 \r
380                 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxSetupJointConstraints failed %d\n",ret);\r
381         }\r
382         pc.countEnd();\r
383 \r
384         pc.countBegin("solve constraints");\r
385         {\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
399 \r
400 // ARA begin insert new code\r
401 #ifdef USE_PTHREADS\r
402                 int ret = pfxSolveConstraints(param, taskManager);\r
403 #else\r
404 // ARA end\r
405 \r
406                 int ret = pfxSolveConstraints(param);\r
407 \r
408 // ARA begin insert new code\r
409 #endif\r
410 // ARA end\r
411 \r
412                 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxSolveConstraints failed %d\n",ret);\r
413                 \r
414                 pool.deallocate(param.workBuff);\r
415         }\r
416         pc.countEnd();\r
417 \r
418         //pc.printCount();\r
419 }\r
420 \r
421 void integrate()\r
422 {\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
428         \r
429 // ARA begin insert new code\r
430 #ifdef USE_PTHREADS\r
431         pfxUpdateRigidStates(param, taskManager);\r
432 #else\r
433 // ARA end\r
434 \r
435         pfxUpdateRigidStates(param);\r
436 \r
437 // ARA begin insert new code\r
438 #endif\r
439 // ARA end\r
440 }\r
441 \r
442 void physics_simulate()\r
443 {\r
444         PfxPerfCounter pc;\r
445 \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
448         }\r
449         \r
450         perf_push_marker("broadphase");\r
451         pc.countBegin("broadphase");\r
452         broadphase();\r
453         pc.countEnd();\r
454         perf_pop_marker();\r
455         \r
456         perf_push_marker("collision");\r
457         pc.countBegin("collision");\r
458         collision();\r
459         pc.countEnd();\r
460         perf_pop_marker();\r
461         \r
462         perf_push_marker("solver");\r
463         pc.countBegin("solver");\r
464         constraintSolver();\r
465         pc.countEnd();\r
466         perf_pop_marker();\r
467         \r
468         perf_push_marker("integrate");\r
469         pc.countBegin("integrate");\r
470         integrate();\r
471         pc.countEnd();\r
472         perf_pop_marker();\r
473         \r
474         frame++;\r
475         \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
484         }\r
485 }\r
486 \r
487 ///////////////////////////////////////////////////////////////////////////////\r
488 // Create Scene\r
489 \r
490 void createBrick(int id,const PfxVector3 &pos,const PfxQuat &rot,const PfxVector3 &boxSize,PfxFloat mass)\r
491 {\r
492         PfxBox box(boxSize);\r
493         PfxShape shape;\r
494         shape.reset();\r
495         shape.setBox(box);\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
508 }\r
509 \r
510 void createWall(const PfxVector3 &offsetPosition,int stackSize,const PfxVector3 &boxSize)\r
511 {\r
512         PfxFloat bodyMass = 0.5f;\r
513 \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
517 \r
518         PfxFloat offset = -stackSize * (diffZ * 2.0f) * 0.5f;\r
519         PfxVector3 pos(0.0f, diffY, 0.0f);\r
520 \r
521         while(stackSize) {\r
522                 for(int i=0;i<stackSize;i++) {\r
523                         pos[2] = offset + (PfxFloat)i * (diffZ * 2.0f);\r
524                 \r
525                         createBrick(numRigidBodies++,offsetPosition+pos,PfxQuat::identity(),boxSize,bodyMass);\r
526                 }\r
527                 offset += diffZ;\r
528                 pos[1] += (diffY * 2.0f);\r
529                 stackSize--;\r
530         }\r
531 }\r
532 \r
533 void createPyramid(const PfxVector3 &offsetPosition,int stackSize,const PfxVector3 &boxSize)\r
534 {\r
535         PfxFloat space = 0.0001f;\r
536         PfxVector3 pos(0.0f, boxSize[1], 0.0f);\r
537 \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
541 \r
542         PfxFloat offsetX = -stackSize * (diffX * 2.0f + space) * 0.5f;\r
543         PfxFloat offsetZ = -stackSize * (diffZ * 2.0f + space) * 0.5f;\r
544         while(stackSize) {\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
550                         }\r
551                 }\r
552                 offsetX += diffX;\r
553                 offsetZ += diffZ;\r
554                 pos[1] += (diffY * 2.0f + space);\r
555                 stackSize--;\r
556         }\r
557 }\r
558 \r
559 void createTowerCircle(const PfxVector3 &offsetPosition,int stackSize,int rotSize,const PfxVector3 &boxSize)\r
560 {\r
561         PfxFloat radius = 1.3f * rotSize * boxSize[0] / SCE_PFX_PI;\r
562 \r
563         // create active boxes\r
564         PfxQuat rotY = PfxQuat::identity();\r
565         PfxFloat posY = boxSize[1];\r
566 \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
570 \r
571                         rotY *= PfxQuat::rotationY(SCE_PFX_PI/(rotSize*0.5f));\r
572                 }\r
573 \r
574                 posY += boxSize[1] * 2.0f;\r
575                 rotY *= PfxQuat::rotationY(SCE_PFX_PI/(PfxFloat)rotSize);\r
576         }\r
577 }\r
578 \r
579 void createScenePrimitives()\r
580 {\r
581         // sphere\r
582         {\r
583                 int id = numRigidBodies++;\r
584                 PfxSphere sphere(1.0f);\r
585                 PfxShape shape;\r
586                 shape.reset();\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
598         }\r
599 \r
600         // box\r
601         {\r
602                 int id = numRigidBodies++;\r
603                 PfxBox box(1.0f,1.0f,1.0f);\r
604                 PfxShape shape;\r
605                 shape.reset();\r
606                 shape.setBox(box);\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
617         }\r
618 \r
619         // capsule\r
620         {\r
621                 int id = numRigidBodies++;\r
622                 PfxCapsule capsule(1.5f,0.5f);\r
623                 PfxShape shape;\r
624                 shape.reset();\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
636         }\r
637 \r
638         // cylinder\r
639         {\r
640                 int id = numRigidBodies++;\r
641                 PfxCylinder cylinder(0.5f,1.5f);\r
642                 PfxShape shape;\r
643                 shape.reset();\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
655         }\r
656 \r
657         // convex mesh\r
658         {\r
659                 PfxCreateConvexMeshParam param;\r
660 \r
661                 param.verts = BarrelVtx;\r
662                 param.numVerts = BarrelVtxCount;\r
663                 param.vertexStrideBytes = sizeof(float)*6;\r
664 \r
665                 param.triangles = BarrelIdx;\r
666                 param.numTriangles = BarrelIdxCount/3;\r
667                 param.triangleStrideBytes = sizeof(unsigned short)*3;\r
668 \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
672                 }\r
673 \r
674                 int id = numRigidBodies++;\r
675                 PfxShape shape;\r
676                 shape.reset();\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
688         }\r
689 \r
690         // combined primitives\r
691         {\r
692                 int id = numRigidBodies++;\r
693 \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
698                 {\r
699                         PfxBox box(0.5f,0.5f,1.5f);\r
700                         PfxShape shape;\r
701                         shape.reset();\r
702                         shape.setBox(box);\r
703                         shape.setOffsetPosition(PfxVector3(-2.0f,0.0f,0.0f));\r
704                         collidables[id].addShape(shape);\r
705                 }\r
706                 {\r
707                         PfxBox box(0.5f,1.5f,0.5f);\r
708                         PfxShape shape;\r
709                         shape.reset();\r
710                         shape.setBox(box);\r
711                         shape.setOffsetPosition(PfxVector3(2.0f,0.0f,0.0f));\r
712                         collidables[id].addShape(shape);\r
713                 }\r
714                 {\r
715                         PfxCapsule cap(1.5f,0.5f);\r
716                         PfxShape shape;\r
717                         shape.reset();\r
718                         shape.setCapsule(cap);\r
719                         collidables[id].addShape(shape);\r
720                 }\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
729         }\r
730 }\r
731 \r
732 void createSceneJoints()\r
733 {\r
734         const int n = 10;\r
735 \r
736         int startId = numRigidBodies;\r
737 \r
738         PfxVector3 boxSize(1.0f);\r
739         PfxFloat boxMass = 1.0f;\r
740 \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
743         }\r
744 \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
748                 PfxVector3 anchor;\r
749                 if(i == numRigidBodies-1) {\r
750                         anchor = stateA.getPosition() + PfxVector3(0,boxSize[1],0);\r
751                 }\r
752                 else {\r
753                         anchor = ( stateA.getPosition() + stateB.getPosition() ) * 0.5f;\r
754                 }\r
755 \r
756                 PfxSwingTwistJointInitParam jparam;\r
757                 jparam.anchorPoint = anchor;\r
758                 jparam.twistAxis = PfxVector3(0,1,0);\r
759 \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
763 \r
764                 pfxUpdateJointPairs(jointPairs[numJoints],numJoints,joints[numJoints],stateA,stateB);\r
765 \r
766                 SCE_PFX_ASSERT(numJoints<NUM_JOINTS);\r
767                 numJoints++;\r
768         }\r
769 \r
770         states[startId].setLinearVelocity(PfxVector3(0,0,5));\r
771         states[startId].setLinearDamping(0.95f);\r
772         states[startId].setAngularDamping(0.95f);\r
773 }\r
774 \r
775 void createSceneStacking()\r
776 {\r
777        const float cubeSize = 1.0f;\r
778 \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
784 \r
785 //      createTowerCircle(PfxVector3(0.0f,0.0f,0.0f),8,24,PfxVector3(1));\r
786 }\r
787 \r
788 void createSceneBoxGround()\r
789 {\r
790         int id = numRigidBodies++;\r
791         PfxBox box(150.0f,2.5f,150.0f);\r
792         PfxShape shape;\r
793         shape.reset();\r
794         shape.setBox(box);\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
803 }\r
804 \r
805 void createSceneLandscape()\r
806 {\r
807         PfxCreateLargeTriMeshParam param;\r
808 \r
809         param.verts = LargeMeshVtx;\r
810         param.numVerts = LargeMeshVtxCount;\r
811         param.vertexStrideBytes = sizeof(float)*6;\r
812 \r
813         param.triangles = LargeMeshIdx;\r
814         param.numTriangles = LargeMeshIdxCount/3;\r
815         param.triangleStrideBytes = sizeof(unsigned short)*3;\r
816 \r
817         if(gLargeMesh.m_numIslands > 0) {\r
818                 pfxReleaseLargeTriMesh(gLargeMesh);\r
819         }\r
820 \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
824         }\r
825 \r
826         int id = numRigidBodies++;\r
827         PfxShape shape;\r
828         shape.reset();\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
839 }\r
840 \r
841 void physics_create_scene(int sceneId)\r
842 {\r
843         const int numScenes = 4;\r
844         int sid = sceneId % numScenes;\r
845         \r
846         numRigidBodies= 0;\r
847         pairSwap = 0;\r
848         numPairs[0] = 0;\r
849         numPairs[1] = 0;\r
850         numContacts = 0;\r
851         numContactIdPool = 0;\r
852         numJoints = 0;\r
853         frame = 0;\r
854         \r
855         switch(sid) {\r
856                 case 0: // simple primitives\r
857                 createSceneBoxGround();\r
858                 createScenePrimitives();\r
859                 break;\r
860                 \r
861                 case 1: // joints\r
862                 createSceneBoxGround();\r
863                 createSceneJoints();\r
864                 break;\r
865 \r
866                 case 2: // stacking\r
867                 createSceneBoxGround();\r
868                 createSceneStacking();\r
869                 break;\r
870 \r
871                 case 3: // landscape\r
872                 createSceneLandscape();\r
873                 createScenePrimitives();\r
874                 break;\r
875         }\r
876 \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
887 \r
888         int totalBytes = \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
893 \r
894 // ARA begin insert new code\r
895 #ifdef USE_PTHREADS\r
896         if (!taskManager)\r
897         {\r
898                 taskManager = PfxCreateTaskManagerPthreads(NUM_THREADS, NUM_THREADS, taskPoolBuff, TASK_MANAGER_POOL_BYTES);\r
899                 taskManager->initialize();\r
900         }\r
901 #endif\r
902 // ARA end\r
903 }\r
904 \r
905 ///////////////////////////////////////////////////////////////////////////////\r
906 // Initialize / Finalize Engine\r
907 \r
908 bool physics_init()\r
909 {\r
910         return true;\r
911 }\r
912 \r
913 void physics_release()\r
914 {\r
915 // ARA begin insert new code\r
916         if (taskManager)\r
917                 taskManager->finalize();\r
918 // ARA end\r
919 }\r
920 \r
921 ///////////////////////////////////////////////////////////////////////////////\r
922 // Pick\r
923 \r
924 PfxVector3 physics_pick_start(const PfxVector3 &p1,const PfxVector3 &p2)\r
925 {\r
926         return PfxVector3(0.0f);\r
927 }\r
928 \r
929 void physics_pick_update(const PfxVector3 &p)\r
930 {\r
931 }\r
932 \r
933 void physics_pick_end()\r
934 {\r
935 }\r
936 \r
937 ///////////////////////////////////////////////////////////////////////////////\r
938 // Get Information\r
939 \r
940 int physics_get_num_rigidbodies()\r
941 {\r
942         return numRigidBodies;\r
943 }\r
944 \r
945 const PfxRigidState& physics_get_state(int id)\r
946 {\r
947         return states[id];\r
948 }\r
949 \r
950 const PfxRigidBody& physics_get_body(int id)\r
951 {\r
952         return bodies[id];\r
953 }\r
954 \r
955 const PfxCollidable& physics_get_collidable(int id)\r
956 {\r
957         return collidables[id];\r
958 }\r
959 \r
960 int physics_get_num_contacts()\r
961 {\r
962         return numPairs[pairSwap];\r
963 }\r
964 \r
965 const PfxContactManifold &physics_get_contact(int id)\r
966 {\r
967         return contacts[pfxGetConstraintId(pairsBuff[pairSwap][id])];\r
968 }\r