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