Extras/PhysicsEffects/sample/api_physics_effects/5_raycast/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 500\r
  24 #define NUM_JOINTS    500\r
  25 #define NUM_CONTACTS  4000\r
  26 #define NUM_RAYS 100\r
  27 \r
  28 const float timeStep = 0.016f;\r
  29 const float separateBias = 0.1f;\r
  30 int iteration = 5;\r
  31 \r
  32 //J ワールドサイズ\r
  33 //E World size\r
  34 PfxVector3 worldCenter(0.0f);\r
  35 PfxVector3 worldExtent(500.0f);\r
  36 \r
  37 //J 剛体\r
  38 //E Rigid body\r
  39 PfxRigidState states[NUM_RIGIDBODIES];\r
  40 PfxRigidBody  bodies[NUM_RIGIDBODIES];\r
  41 PfxCollidable collidables[NUM_RIGIDBODIES];\r
  42 PfxSolverBody solverBodies[NUM_RIGIDBODIES];\r
  43 int numRigidBodies = 0;\r
  44 \r
  45 //J 地形を表現するためのラージメッシュ\r
  46 //E Large mesh for representing a landscape\r
  47 #include "landscape.h"\r
  48 PfxLargeTriMesh gLargeMesh;\r
  49 \r
  50 //J 凸メッシュ\r
  51 //E Convex Mesh\r
  52 #include "barrel.h"\r
  53 PfxConvexMesh gConvex;\r
  54 \r
  55 //J プロキシ\r
  56 //E Proxies\r
  57 PfxBroadphaseProxy proxies[6][NUM_RIGIDBODIES]; // shared by simulation and raycast\r
  58 \r
  59 //J ジョイント\r
  60 //E Joint\r
  61 PfxConstraintPair jointPairs[NUM_JOINTS];\r
  62 PfxJoint joints[NUM_JOINTS];\r
  63 int numJoints = 0;\r
  64 \r
  65 //J ペア\r
  66 //E Pairs\r
  67 unsigned int pairSwap;\r
  68 unsigned int numPairs[2];\r
  69 PfxBroadphasePair pairsBuff[2][NUM_CONTACTS];\r
  70 \r
  71 //J コンタクト\r
  72 //E Contacts\r
  73 PfxContactManifold contacts[NUM_CONTACTS];\r
  74 int numContacts;\r
  75 \r
  76 PfxUInt32 contactIdPool[NUM_CONTACTS];\r
  77 int numContactIdPool;\r
  78 \r
  79 //J シミュレーションアイランド\r
  80 //E Island generation\r
  81 PfxIsland *island=NULL;\r
  82 PfxUInt8 SCE_PFX_ALIGNED(16) islandBuff[32*NUM_RIGIDBODIES]; // Island buffer should be 32 * the number of rigid bodies.\r
  83 \r
  84 //J スリープ制御\r
  85 //E Sleep control\r
  86 /*\r
  87         A sleeping object wakes up, when \r
  88         * a new pair related to this rigid body is created\r
  89         * a pair releated to this rigid body is removed\r
  90         * a rigid body's velocity or position are updated\r
  91  */\r
  92 \r
  93 //J スリープに入るカウント\r
  94 //E Count to enter sleeping\r
  95 const PfxUInt32 sleepCount = 180;\r
  96 \r
  97 //J 速度が閾値以下ならばスリープカウントが増加\r
  98 //E If velocity is under the following value, sleep count is increased.\r
  99 const PfxFloat sleepVelocity = 0.1f;\r
 100 \r
 101 //J 一時バッファ\r
 102 //E Temporary buffers\r
 103 #define POOL_BYTES (5*1024*1024)\r
 104 unsigned char SCE_PFX_ALIGNED(128) poolBuff[POOL_BYTES];\r
 105 \r
 106 //J 一時バッファ用スタックアロケータ\r
 107 //E Stack allocator for temporary buffers\r
 108 PfxHeapManager pool(poolBuff,POOL_BYTES);\r
 109 \r
 110 //J レイ\r
 111 //E Ray\r
 112 PfxRayInput SCE_PFX_ALIGNED(128) rayInputs[NUM_RAYS];\r
 113 PfxRayOutput SCE_PFX_ALIGNED(128) rayOutputs[NUM_RAYS];\r
 114 int numRays;\r
 115 \r
 116 /* \r
 117         doAreaRaycastがtrueの場合、指定された領域内の剛体のみ判定対象とする\r
 118 */\r
 119 bool doAreaRaycast;\r
 120 PfxVector3 areaCenter(0.0f);\r
 121 PfxVector3 areaExtent(5.0f);\r
 122 \r
 123 ///////////////////////////////////////////////////////////////////////////////\r
 124 // Simulation Function\r
 125 \r
 126 int frame = 0;\r
 127 \r
 128 void broadphase()\r
 129 {\r
 130         pairSwap = 1-pairSwap;\r
 131 \r
 132         unsigned int &numPreviousPairs = numPairs[1-pairSwap];\r
 133         unsigned int &numCurrentPairs = numPairs[pairSwap];\r
 134         PfxBroadphasePair *previousPairs = pairsBuff[1-pairSwap];\r
 135         PfxBroadphasePair *currentPairs = pairsBuff[pairSwap];\r
 136 \r
 137         //J 剛体が最も分散している軸を見つける\r
 138         //E Find the axis along which all rigid bodies are most widely positioned\r
 139         int axis = 0;\r
 140         {\r
 141                 PfxVector3 s(0.0f),s2(0.0f);\r
 142                 for(int i=0;i<numRigidBodies;i++) {\r
 143                         PfxVector3 c = states[i].getPosition();\r
 144                         s += c;\r
 145                         s2 += mulPerElem(c,c);\r
 146                 }\r
 147                 PfxVector3 v = s2 - mulPerElem(s,s) / (float)numRigidBodies;\r
 148                 if(v[1] > v[0]) axis = 1;\r
 149                 if(v[2] > v[axis]) axis = 2;\r
 150         }\r
 151 \r
 152         //J ブロードフェーズプロキシの更新\r
 153         //E Create broadpahse proxies\r
 154         {\r
 155                 //J レイキャストと共用するため、全ての軸に対するプロキシ配列を作成する\r
 156                 //E To share with ray casting, create proxy arrays for all axis\r
 157 \r
 158                 PfxUpdateBroadphaseProxiesParam param;\r
 159                 param.workBytes = pfxGetWorkBytesOfUpdateBroadphaseProxies(numRigidBodies);\r
 160                 param.workBuff = pool.allocate(param.workBytes,128);\r
 161                 param.numRigidBodies = numRigidBodies;\r
 162                 param.offsetRigidStates = states;\r
 163                 param.offsetCollidables = collidables;\r
 164                 param.proxiesX = proxies[0];\r
 165                 param.proxiesY = proxies[1];\r
 166                 param.proxiesZ = proxies[2];\r
 167                 param.proxiesXb = proxies[3];\r
 168                 param.proxiesYb = proxies[4];\r
 169                 param.proxiesZb = proxies[5];\r
 170                 param.worldCenter = worldCenter;\r
 171                 param.worldExtent = worldExtent;\r
 172 \r
 173                 PfxUpdateBroadphaseProxiesResult result;\r
 174 \r
 175                 int ret = pfxUpdateBroadphaseProxies(param,result);\r
 176                 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxUpdateBroadphaseProxies failed %d\n",ret);\r
 177                 \r
 178                 pool.deallocate(param.workBuff);\r
 179         }\r
 180 \r
 181         //J 交差ペア探索\r
 182         //E Find overlapped pairs\r
 183         {\r
 184                 PfxFindPairsParam findPairsParam;\r
 185                 findPairsParam.pairBytes = pfxGetPairBytesOfFindPairs(NUM_CONTACTS);\r
 186                 findPairsParam.pairBuff = pool.allocate(findPairsParam.pairBytes);\r
 187                 findPairsParam.workBytes = pfxGetWorkBytesOfFindPairs(NUM_CONTACTS);\r
 188                 findPairsParam.workBuff = pool.allocate(findPairsParam.workBytes);\r
 189                 findPairsParam.proxies = proxies[axis];\r
 190                 findPairsParam.numProxies = numRigidBodies;\r
 191                 findPairsParam.maxPairs = NUM_CONTACTS;\r
 192                 findPairsParam.axis = axis;\r
 193 \r
 194                 PfxFindPairsResult findPairsResult;\r
 195 \r
 196                 int ret = pfxFindPairs(findPairsParam,findPairsResult);\r
 197                 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxFindPairs failed %d\n",ret);\r
 198                 \r
 199                 pool.deallocate(findPairsParam.workBuff);\r
 200 \r
 201                 //J 交差ペア合成\r
 202                 //E Decompose overlapped pairs into 3 arrays\r
 203                 PfxDecomposePairsParam decomposePairsParam;\r
 204                 decomposePairsParam.pairBytes = pfxGetPairBytesOfDecomposePairs(numPreviousPairs,findPairsResult.numPairs);\r
 205                 decomposePairsParam.pairBuff = pool.allocate(decomposePairsParam.pairBytes);\r
 206                 decomposePairsParam.workBytes = pfxGetWorkBytesOfDecomposePairs(numPreviousPairs,findPairsResult.numPairs);\r
 207                 decomposePairsParam.workBuff = pool.allocate(decomposePairsParam.workBytes);\r
 208                 decomposePairsParam.previousPairs = previousPairs;\r
 209                 decomposePairsParam.numPreviousPairs = numPreviousPairs;\r
 210                 decomposePairsParam.currentPairs = findPairsResult.pairs; // Set pairs from pfxFindPairs()\r
 211                 decomposePairsParam.numCurrentPairs = findPairsResult.numPairs; // Set the number of pairs from pfxFindPairs()\r
 212 \r
 213                 PfxDecomposePairsResult decomposePairsResult;\r
 214 \r
 215                 ret = pfxDecomposePairs(decomposePairsParam,decomposePairsResult);\r
 216                 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxDecomposePairs failed %d\n",ret);\r
 217 \r
 218                 pool.deallocate(decomposePairsParam.workBuff);\r
 219 \r
 220                 PfxBroadphasePair *outNewPairs = decomposePairsResult.outNewPairs;\r
 221                 PfxBroadphasePair *outKeepPairs = decomposePairsResult.outKeepPairs;\r
 222                 PfxBroadphasePair *outRemovePairs = decomposePairsResult.outRemovePairs;\r
 223                 PfxUInt32 numOutNewPairs = decomposePairsResult.numOutNewPairs;\r
 224                 PfxUInt32 numOutKeepPairs = decomposePairsResult.numOutKeepPairs;\r
 225                 PfxUInt32 numOutRemovePairs = decomposePairsResult.numOutRemovePairs;\r
 226 \r
 227                 //J 廃棄ペアのコンタクトをプールに戻す\r
 228                 //E Put removed contacts into the contact pool\r
 229                 for(PfxUInt32 i=0;i<numOutRemovePairs;i++) {\r
 230                         contactIdPool[numContactIdPool++] = pfxGetContactId(outRemovePairs[i]);\r
 231 \r
 232                         //J 寝てる剛体を起こす\r
 233                         //E Wake up sleeping rigid bodies\r
 234                         PfxRigidState &stateA = states[pfxGetObjectIdA(outRemovePairs[i])];\r
 235                         PfxRigidState &stateB = states[pfxGetObjectIdB(outRemovePairs[i])];\r
 236                         if(stateA.isAsleep()) {\r
 237                                 stateA.wakeup();\r
 238                         }\r
 239                         if(stateB.isAsleep()) {\r
 240                                 stateB.wakeup();\r
 241                         }\r
 242                 }\r
 243 \r
 244                 //J 新規ペアのコンタクトのリンクと初期化\r
 245                 //E Add new contacts and initialize\r
 246                 for(PfxUInt32 i=0;i<numOutNewPairs;i++) {\r
 247                         int cId = 0;\r
 248                         if(numContactIdPool > 0) {\r
 249                                 cId = contactIdPool[--numContactIdPool];\r
 250                         }\r
 251                         else {\r
 252                                 cId = numContacts++;\r
 253                         }\r
 254                         if(cId >= NUM_CONTACTS) {\r
 255                                 cId = 0;\r
 256                         }\r
 257                         SCE_PFX_ASSERT(cId < NUM_CONTACTS);\r
 258                         pfxSetContactId(outNewPairs[i],cId);\r
 259                         PfxContactManifold &contact = contacts[cId];\r
 260                         contact.reset(pfxGetObjectIdA(outNewPairs[i]),pfxGetObjectIdB(outNewPairs[i]));\r
 261 \r
 262                         //J 寝てる剛体を起こす\r
 263                         //E Wake up sleeping rigid bodies\r
 264                         PfxRigidState &stateA = states[pfxGetObjectIdA(outNewPairs[i])];\r
 265                         PfxRigidState &stateB = states[pfxGetObjectIdB(outNewPairs[i])];\r
 266                         if(stateA.isAsleep()) {\r
 267                                 stateA.wakeup();\r
 268                         }\r
 269                         if(stateB.isAsleep()) {\r
 270                                 stateB.wakeup();\r
 271                         }\r
 272                 }\r
 273 \r
 274                 //J 新規ペアと維持ペアを合成\r
 275                 //E Merge 'new' and 'keep' pairs\r
 276                 numCurrentPairs = 0;\r
 277                 for(PfxUInt32 i=0;i<numOutKeepPairs;i++) {\r
 278                         currentPairs[numCurrentPairs++] = outKeepPairs[i];\r
 279                 }\r
 280                 for(PfxUInt32 i=0;i<numOutNewPairs;i++) {\r
 281                         currentPairs[numCurrentPairs++] = outNewPairs[i];\r
 282                 }\r
 283                 \r
 284                 pool.deallocate(decomposePairsParam.pairBuff);\r
 285                 pool.deallocate(findPairsParam.pairBuff);\r
 286         }\r
 287         \r
 288         {\r
 289                 int workBytes = sizeof(PfxBroadphasePair) * numCurrentPairs;\r
 290                 void *workBuff = pool.allocate(workBytes);\r
 291                 \r
 292                 pfxParallelSort(currentPairs,numCurrentPairs,workBuff,workBytes);\r
 293                 \r
 294                 pool.deallocate(workBuff);\r
 295         }\r
 296 }\r
 297 \r
 298 void collision()\r
 299 {\r
 300         unsigned int numCurrentPairs = numPairs[pairSwap];\r
 301         PfxBroadphasePair *currentPairs = pairsBuff[pairSwap];\r
 302         \r
 303         //J 衝突検出\r
 304         //E Detect collisions\r
 305         {\r
 306                 PfxDetectCollisionParam param;\r
 307                 param.contactPairs = currentPairs;\r
 308                 param.numContactPairs = numCurrentPairs;\r
 309                 param.offsetContactManifolds = contacts;\r
 310                 param.offsetRigidStates = states;\r
 311                 param.offsetCollidables = collidables;\r
 312                 param.numRigidBodies = numRigidBodies;\r
 313 \r
 314                 int ret = pfxDetectCollision(param);\r
 315                 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxDetectCollision failed %d\n",ret);\r
 316         }\r
 317 \r
 318         //J リフレッシュ\r
 319         //E Refresh contacts\r
 320         {\r
 321                 PfxRefreshContactsParam param;\r
 322                 param.contactPairs = currentPairs;\r
 323                 param.numContactPairs = numCurrentPairs;\r
 324                 param.offsetContactManifolds = contacts;\r
 325                 param.offsetRigidStates = states;\r
 326                 param.numRigidBodies = numRigidBodies;\r
 327 \r
 328                 int ret = pfxRefreshContacts(param);\r
 329                 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxRefreshContacts failed %d\n",ret);\r
 330         }\r
 331 \r
 332         //J アイランド生成\r
 333         //E Create simulation islands\r
 334         {\r
 335                 PfxGenerateIslandParam param;\r
 336                 param.islandBuff = islandBuff;\r
 337                 param.islandBytes = 32*NUM_RIGIDBODIES;\r
 338                 param.pairs = currentPairs;\r
 339                 param.numPairs = numCurrentPairs;\r
 340                 param.numObjects = numRigidBodies;\r
 341 \r
 342                 PfxGenerateIslandResult result;\r
 343 \r
 344                 int ret = pfxGenerateIsland(param,result);\r
 345                 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxGenerateIsland failed %d\n",ret);\r
 346                 island = result.island;\r
 347 \r
 348                 //J ジョイント分のペアを追加\r
 349                 //E Add joint pairs to islands\r
 350                 ret = pfxAppendPairs(island,jointPairs,numJoints);\r
 351                 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxAppendPairs failed %d\n",ret);\r
 352         }\r
 353 }\r
 354 \r
 355 void constraintSolver()\r
 356 {\r
 357         PfxPerfCounter pc;\r
 358 \r
 359         unsigned int numCurrentPairs = numPairs[pairSwap];\r
 360         PfxBroadphasePair *currentPairs = pairsBuff[pairSwap];\r
 361 \r
 362         pc.countBegin("setup solver bodies");\r
 363         {\r
 364                 PfxSetupSolverBodiesParam param;\r
 365                 param.states = states;\r
 366                 param.bodies = bodies;\r
 367                 param.solverBodies = solverBodies;\r
 368                 param.numRigidBodies = numRigidBodies;\r
 369                 \r
 370                 int ret = pfxSetupSolverBodies(param);\r
 371                 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxSetupSolverBodies failed %d\n",ret);\r
 372         }\r
 373         pc.countEnd();\r
 374 \r
 375         pc.countBegin("setup contact constraints");\r
 376         {\r
 377                 PfxSetupContactConstraintsParam param;\r
 378                 param.contactPairs = currentPairs;\r
 379                 param.numContactPairs = numCurrentPairs;\r
 380                 param.offsetContactManifolds = contacts;\r
 381                 param.offsetRigidStates = states;\r
 382                 param.offsetRigidBodies = bodies;\r
 383                 param.offsetSolverBodies = solverBodies;\r
 384                 param.numRigidBodies = numRigidBodies;\r
 385                 param.timeStep = timeStep;\r
 386                 param.separateBias = separateBias;\r
 387                 \r
 388                 int ret = pfxSetupContactConstraints(param);\r
 389                 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxSetupJointConstraints failed %d\n",ret);\r
 390         }\r
 391         pc.countEnd();\r
 392 \r
 393         pc.countBegin("setup joint constraints");\r
 394         {\r
 395                 PfxSetupJointConstraintsParam param;\r
 396                 param.jointPairs = jointPairs;\r
 397                 param.numJointPairs = numJoints;\r
 398                 param.offsetJoints = joints;\r
 399                 param.offsetRigidStates = states;\r
 400                 param.offsetRigidBodies = bodies;\r
 401                 param.offsetSolverBodies = solverBodies;\r
 402                 param.numRigidBodies = numRigidBodies;\r
 403                 param.timeStep = timeStep;\r
 404 \r
 405                 for(int i=0;i<numJoints;i++) {\r
 406                         pfxUpdateJointPairs(jointPairs[i],i,joints[i],states[joints[i].m_rigidBodyIdA],states[joints[i].m_rigidBodyIdB]);\r
 407                 }\r
 408 \r
 409                 int ret = pfxSetupJointConstraints(param);\r
 410                 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxSetupJointConstraints failed %d\n",ret);\r
 411         }\r
 412         pc.countEnd();\r
 413 \r
 414         pc.countBegin("solve constraints");\r
 415         {\r
 416                 PfxSolveConstraintsParam param;\r
 417                 param.workBytes = pfxGetWorkBytesOfSolveConstraints(numRigidBodies,numCurrentPairs,numJoints);\r
 418                 param.workBuff = pool.allocate(param.workBytes);\r
 419                 param.contactPairs = currentPairs;\r
 420                 param.numContactPairs = numCurrentPairs;\r
 421                 param.offsetContactManifolds = contacts;\r
 422                 param.jointPairs = jointPairs;\r
 423                 param.numJointPairs = numJoints;\r
 424                 param.offsetJoints = joints;\r
 425                 param.offsetRigidStates = states;\r
 426                 param.offsetSolverBodies = solverBodies;\r
 427                 param.numRigidBodies = numRigidBodies;\r
 428                 param.iteration = iteration;\r
 429 \r
 430                 int ret = pfxSolveConstraints(param);\r
 431                 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxSolveConstraints failed %d\n",ret);\r
 432                 \r
 433                 pool.deallocate(param.workBuff);\r
 434         }\r
 435         pc.countEnd();\r
 436 \r
 437         //pc.printCount();\r
 438 }\r
 439 \r
 440 void sleepOrWakeup()\r
 441 {\r
 442         PfxFloat sleepVelSqr = sleepVelocity * sleepVelocity;\r
 443 \r
 444         for(PfxUInt32 i=0;i<(PfxUInt32)numRigidBodies;i++) {\r
 445                 PfxRigidState &state = states[i];\r
 446                 if(SCE_PFX_MOTION_MASK_CAN_SLEEP(state.getMotionType())) {\r
 447                         PfxFloat linVelSqr = lengthSqr(state.getLinearVelocity());\r
 448                         PfxFloat angVelSqr = lengthSqr(state.getAngularVelocity());\r
 449 \r
 450                         if(state.isAwake()) {\r
 451                                 if( linVelSqr < sleepVelSqr && angVelSqr < sleepVelSqr ) {\r
 452                                         state.incrementSleepCount();\r
 453                                 }\r
 454                                 else {\r
 455                                         state.resetSleepCount();\r
 456                                 }\r
 457                         }\r
 458                 }\r
 459         }\r
 460 \r
 461         if(island) {\r
 462                 for(PfxUInt32 i=0;i<pfxGetNumIslands(island);i++) {\r
 463                         int numActive = 0;\r
 464                         int numSleep = 0;\r
 465                         int numCanSleep = 0;\r
 466                         \r
 467                         {\r
 468                                 PfxIslandUnit *islandUnit = pfxGetFirstUnitInIsland(island,(PfxUInt32)i);\r
 469                                 for(;islandUnit!=NULL;islandUnit=pfxGetNextUnitInIsland(islandUnit)) {\r
 470                                         if(!(SCE_PFX_MOTION_MASK_CAN_SLEEP(states[pfxGetUnitId(islandUnit)].getMotionType()))) continue;\r
 471                                         PfxRigidState &state = states[pfxGetUnitId(islandUnit)];\r
 472                                         if(state.isAsleep()) {\r
 473                                                 numSleep++;\r
 474                                         }\r
 475                                         else {\r
 476                                                 numActive++;\r
 477                                                 if(state.getSleepCount() > sleepCount) {\r
 478                                                         numCanSleep++;\r
 479                                                 }\r
 480                                         }\r
 481                                 }\r
 482                         }\r
 483 \r
 484                         // Deactivate Island\r
 485                         if(numCanSleep > 0 && numCanSleep == numActive + numSleep) {\r
 486                                 PfxIslandUnit *islandUnit = pfxGetFirstUnitInIsland(island,(PfxUInt32)i);\r
 487                                 for(;islandUnit!=NULL;islandUnit=pfxGetNextUnitInIsland(islandUnit)) {\r
 488                                         if(!(SCE_PFX_MOTION_MASK_CAN_SLEEP(states[pfxGetUnitId(islandUnit)].getMotionType()))) continue;\r
 489                                         states[pfxGetUnitId(islandUnit)].sleep();\r
 490                                 }\r
 491                         }\r
 492 \r
 493                         // Activate Island\r
 494                         else if(numSleep > 0 && numActive > 0) {\r
 495                                 PfxIslandUnit *islandUnit = pfxGetFirstUnitInIsland(island,(PfxUInt32)i);\r
 496                                 for(;islandUnit!=NULL;islandUnit=pfxGetNextUnitInIsland(islandUnit)) {\r
 497                                         if(!(SCE_PFX_MOTION_MASK_CAN_SLEEP(states[pfxGetUnitId(islandUnit)].getMotionType()))) continue;\r
 498                                         states[pfxGetUnitId(islandUnit)].wakeup();\r
 499                                 }\r
 500                         }\r
 501                 }\r
 502         }\r
 503 }\r
 504 \r
 505 void integrate()\r
 506 {\r
 507         PfxUpdateRigidStatesParam param;\r
 508         param.states = states;\r
 509         param.bodies = bodies;\r
 510         param.numRigidBodies = numRigidBodies;\r
 511         param.timeStep = timeStep;\r
 512         \r
 513         pfxUpdateRigidStates(param);\r
 514 }\r
 515 \r
 516 int gatherBroadphaseProxiesInArea(const PfxVector3 &center,const PfxVector3 &extent)\r
 517 {\r
 518         PfxUpdateBroadphaseProxiesParam param;\r
 519         param.workBytes = pfxGetWorkBytesOfUpdateBroadphaseProxies(numRigidBodies);\r
 520         param.workBuff = pool.allocate(param.workBytes,128);\r
 521         param.numRigidBodies = numRigidBodies;\r
 522         param.offsetRigidStates = states;\r
 523         param.offsetCollidables = collidables;\r
 524         param.proxiesX = proxies[0];\r
 525         param.proxiesY = proxies[1];\r
 526         param.proxiesZ = proxies[2];\r
 527         param.proxiesXb = proxies[3];\r
 528         param.proxiesYb = proxies[4];\r
 529         param.proxiesZb = proxies[5];\r
 530         param.worldCenter = center;\r
 531         param.worldExtent = extent;\r
 532         param.outOfWorldBehavior = SCE_PFX_OUT_OF_WORLD_BEHAVIOR_REMOVE_PROXY;\r
 533 \r
 534         PfxUpdateBroadphaseProxiesResult result;\r
 535 \r
 536         int ret = pfxUpdateBroadphaseProxies(param,result);\r
 537         if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxUpdateBroadphaseProxies failed %d\n",ret);\r
 538         \r
 539         pool.deallocate(param.workBuff);\r
 540 \r
 541         return numRigidBodies - result.numOutOfWorldProxies;\r
 542 }\r
 543 \r
 544 void castRays()\r
 545 {\r
 546         PfxRayCastParam param;\r
 547 \r
 548         if(doAreaRaycast) {\r
 549                 static PfxFloat deltaRotY = 0.0f;\r
 550                 PfxQuat rotY = PfxQuat::rotationY(deltaRotY);\r
 551                 areaCenter = rotate(rotY,PfxVector3(15.0f,0.0f,0.0f));\r
 552                 deltaRotY += 0.02f;\r
 553 \r
 554                 int num = gatherBroadphaseProxiesInArea(areaCenter,areaExtent);\r
 555                 param.offsetRigidStates = states;\r
 556                 param.offsetCollidables = collidables;\r
 557                 param.proxiesX  = proxies[0];\r
 558                 param.proxiesY  = proxies[1];\r
 559                 param.proxiesZ  = proxies[2];\r
 560                 param.proxiesXb = proxies[3];\r
 561                 param.proxiesYb = proxies[4];\r
 562                 param.proxiesZb = proxies[5];\r
 563                 param.numProxies = num;\r
 564                 param.rangeCenter = areaCenter;\r
 565                 param.rangeExtent = areaExtent;\r
 566         }\r
 567         else {\r
 568                 param.offsetRigidStates = states;\r
 569                 param.offsetCollidables = collidables;\r
 570                 param.proxiesX  = proxies[0];\r
 571                 param.proxiesY  = proxies[1];\r
 572                 param.proxiesZ  = proxies[2];\r
 573                 param.proxiesXb = proxies[3];\r
 574                 param.proxiesYb = proxies[4];\r
 575                 param.proxiesZb = proxies[5];\r
 576                 param.numProxies = numRigidBodies;\r
 577                 param.rangeCenter = worldCenter;\r
 578                 param.rangeExtent = worldExtent;\r
 579         }\r
 580         \r
 581         pfxCastRays(rayInputs,rayOutputs,numRays,param);\r
 582 }\r
 583 \r
 584 void physics_simulate()\r
 585 {\r
 586         PfxPerfCounter pc;\r
 587 \r
 588         for(int i=1;i<numRigidBodies;i++) {\r
 589                 pfxApplyExternalForce(states[i],bodies[i],bodies[i].getMass()*PfxVector3(0.0f,-9.8f,0.0f),PfxVector3(0.0f),timeStep);\r
 590         }\r
 591         \r
 592         perf_push_marker("broadphase");\r
 593         pc.countBegin("broadphase");\r
 594         broadphase();\r
 595         pc.countEnd();\r
 596         perf_pop_marker();\r
 597         \r
 598         perf_push_marker("collision");\r
 599         pc.countBegin("collision");\r
 600         collision();\r
 601         pc.countEnd();\r
 602         perf_pop_marker();\r
 603         \r
 604         perf_push_marker("solver");\r
 605         pc.countBegin("solver");\r
 606         constraintSolver();\r
 607         pc.countEnd();\r
 608         perf_pop_marker();\r
 609         \r
 610         perf_push_marker("sleepCheck");\r
 611         pc.countBegin("sleepCheck");\r
 612         sleepOrWakeup();\r
 613         pc.countEnd();\r
 614         perf_pop_marker();\r
 615         \r
 616         perf_push_marker("integrate");\r
 617         pc.countBegin("integrate");\r
 618         integrate();\r
 619         pc.countEnd();\r
 620         perf_pop_marker();\r
 621 \r
 622         perf_push_marker("castRays");\r
 623         pc.countBegin("castRays");\r
 624         castRays();\r
 625         pc.countEnd();\r
 626         perf_pop_marker();\r
 627         \r
 628         frame++;\r
 629         \r
 630         if(frame%100 == 0) {\r
 631                 float broadphaseTime  = pc.getCountTime(0);\r
 632                 float collisionTime   = pc.getCountTime(2);\r
 633                 float solverTime      = pc.getCountTime(4);\r
 634                 float sleepTime           = pc.getCountTime(6);\r
 635                 float integrateTime   = pc.getCountTime(8);\r
 636                 float raycastTime     = pc.getCountTime(10);\r
 637                 SCE_PFX_PRINTF("frame %3d broadphase %.2f collision %.2f solver %.2f sleepCheck %.2f integrate %.2f raycast %.2f | total %.2f\n",frame,\r
 638                         broadphaseTime,collisionTime,solverTime,sleepTime,integrateTime,raycastTime,\r
 639                         broadphaseTime+collisionTime+solverTime+sleepTime+integrateTime+raycastTime);\r
 640         }\r
 641 }\r
 642 \r
 643 ///////////////////////////////////////////////////////////////////////////////\r
 644 // Create Scene\r
 645 \r
 646 int createBrick(const PfxVector3 &pos,const PfxQuat &rot,const PfxVector3 &boxSize,PfxFloat mass)\r
 647 {\r
 648         int id = numRigidBodies++;\r
 649         \r
 650         PfxBox box(boxSize);\r
 651         PfxShape shape;\r
 652         shape.reset();\r
 653         shape.setBox(box);\r
 654         collidables[id].reset();\r
 655         collidables[id].addShape(shape);\r
 656         collidables[id].finish();\r
 657         bodies[id].reset();\r
 658         bodies[id].setRestitution(0.0f);\r
 659         bodies[id].setMass(mass);\r
 660         bodies[id].setInertia(pfxCalcInertiaBox(boxSize,mass));\r
 661         states[id].reset();\r
 662         states[id].setPosition(pos);\r
 663         states[id].setOrientation(rot);\r
 664         states[id].setMotionType(kPfxMotionTypeActive);\r
 665         states[id].setUseSleep(1); // sleep mode ON\r
 666         states[id].setRigidBodyId(id);\r
 667         \r
 668         return id;\r
 669 }\r
 670 \r
 671 void createScenePrimitives()\r
 672 {\r
 673         // sphere\r
 674         {\r
 675                 int id = numRigidBodies++;\r
 676                 PfxSphere sphere(1.0f);\r
 677                 PfxShape shape;\r
 678                 shape.reset();\r
 679                 shape.setSphere(sphere);\r
 680                 collidables[id].reset();\r
 681                 collidables[id].addShape(shape);\r
 682                 collidables[id].finish();\r
 683                 bodies[id].reset();\r
 684                 bodies[id].setMass(1.0f);\r
 685                 bodies[id].setInertia(pfxCalcInertiaSphere(1.0f,1.0f));\r
 686                 states[id].reset();\r
 687                 states[id].setPosition(PfxVector3(-5.0f,5.0f,0.0f));\r
 688                 states[id].setMotionType(kPfxMotionTypeActive);\r
 689                 states[id].setUseSleep(1); // sleep mode ON\r
 690                 states[id].setRigidBodyId(id);\r
 691         }\r
 692 \r
 693         // box\r
 694         {\r
 695                 int id = numRigidBodies++;\r
 696                 PfxBox box(1.0f,1.0f,1.0f);\r
 697                 PfxShape shape;\r
 698                 shape.reset();\r
 699                 shape.setBox(box);\r
 700                 collidables[id].reset();\r
 701                 collidables[id].addShape(shape);\r
 702                 collidables[id].finish();\r
 703                 bodies[id].reset();\r
 704                 bodies[id].setMass(1.0f);\r
 705                 bodies[id].setInertia(pfxCalcInertiaBox(PfxVector3(1.0f),1.0f));\r
 706                 states[id].reset();\r
 707                 states[id].setPosition(PfxVector3(0.0f,5.0f,5.0f));\r
 708                 states[id].setMotionType(kPfxMotionTypeActive);\r
 709                 states[id].setUseSleep(1); // sleep mode ON\r
 710                 states[id].setRigidBodyId(id);\r
 711         }\r
 712 \r
 713         // capsule\r
 714         {\r
 715                 int id = numRigidBodies++;\r
 716                 PfxCapsule capsule(1.5f,0.5f);\r
 717                 PfxShape shape;\r
 718                 shape.reset();\r
 719                 shape.setCapsule(capsule);\r
 720                 collidables[id].reset();\r
 721                 collidables[id].addShape(shape);\r
 722                 collidables[id].finish();\r
 723                 bodies[id].reset();\r
 724                 bodies[id].setMass(2.0f);\r
 725                 bodies[id].setInertia(pfxCalcInertiaCylinderX(2.0f,0.5f,2.0f));\r
 726                 states[id].reset();\r
 727                 states[id].setPosition(PfxVector3(5.0f,5.0f,0.0f));\r
 728                 states[id].setMotionType(kPfxMotionTypeActive);\r
 729                 states[id].setUseSleep(1); // sleep mode ON\r
 730                 states[id].setRigidBodyId(id);\r
 731         }\r
 732 \r
 733         // cylinder\r
 734         {\r
 735                 int id = numRigidBodies++;\r
 736                 PfxCylinder cylinder(0.5f,1.5f);\r
 737                 PfxShape shape;\r
 738                 shape.reset();\r
 739                 shape.setCylinder(cylinder);\r
 740                 collidables[id].reset();\r
 741                 collidables[id].addShape(shape);\r
 742                 collidables[id].finish();\r
 743                 bodies[id].reset();\r
 744                 bodies[id].setMass(3.0f);\r
 745                 bodies[id].setInertia(pfxCalcInertiaCylinderX(0.5f,1.5f,3.0f));\r
 746                 states[id].reset();\r
 747                 states[id].setPosition(PfxVector3(0.0f,10.0f,0.0f));\r
 748                 states[id].setMotionType(kPfxMotionTypeActive);\r
 749                 states[id].setUseSleep(1); // sleep mode ON\r
 750                 states[id].setRigidBodyId(id);\r
 751         }\r
 752 \r
 753         // convex mesh\r
 754         {\r
 755                 PfxCreateConvexMeshParam param;\r
 756 \r
 757                 param.verts = BarrelVtx;\r
 758                 param.numVerts = BarrelVtxCount;\r
 759                 param.vertexStrideBytes = sizeof(float)*6;\r
 760 \r
 761                 param.triangles = BarrelIdx;\r
 762                 param.numTriangles = BarrelIdxCount/3;\r
 763                 param.triangleStrideBytes = sizeof(unsigned short)*3;\r
 764 \r
 765                 PfxInt32 ret = pfxCreateConvexMesh(gConvex,param);\r
 766                 if(ret != SCE_PFX_OK) {\r
 767                         SCE_PFX_PRINTF("Can't create gConvex mesh.\n");\r
 768                 }\r
 769 \r
 770                 int id = numRigidBodies++;\r
 771                 PfxShape shape;\r
 772                 shape.reset();\r
 773                 shape.setConvexMesh(&gConvex);\r
 774                 collidables[id].reset();\r
 775                 collidables[id].addShape(shape);\r
 776                 collidables[id].finish();\r
 777                 bodies[id].reset();\r
 778                 bodies[id].setMass(3.0f);\r
 779                 bodies[id].setInertia(pfxCalcInertiaSphere(1.0f,1.0f));\r
 780                 states[id].reset();\r
 781                 states[id].setPosition(PfxVector3(0.0f,15.0f,0.0f));\r
 782                 states[id].setMotionType(kPfxMotionTypeActive);\r
 783                 states[id].setUseSleep(1); // sleep mode ON\r
 784                 states[id].setRigidBodyId(id);\r
 785         }\r
 786 \r
 787         // combined primitives\r
 788         {\r
 789                 int id = numRigidBodies++;\r
 790 \r
 791                 //E Both shapes and incides buffer have to be kept when creating a combined shape.\r
 792                 static PfxShape shapes[3];\r
 793                 PfxUInt16 shapeIds[3]={0,1,2};\r
 794                 collidables[id].reset(shapes,shapeIds,3);\r
 795                 {\r
 796                         PfxBox box(0.5f,0.5f,1.5f);\r
 797                         PfxShape shape;\r
 798                         shape.reset();\r
 799                         shape.setBox(box);\r
 800                         shape.setOffsetPosition(PfxVector3(-2.0f,0.0f,0.0f));\r
 801                         collidables[id].addShape(shape);\r
 802                 }\r
 803                 {\r
 804                         PfxBox box(0.5f,1.5f,0.5f);\r
 805                         PfxShape shape;\r
 806                         shape.reset();\r
 807                         shape.setBox(box);\r
 808                         shape.setOffsetPosition(PfxVector3(2.0f,0.0f,0.0f));\r
 809                         collidables[id].addShape(shape);\r
 810                 }\r
 811                 {\r
 812                         PfxCapsule cap(1.5f,0.5f);\r
 813                         PfxShape shape;\r
 814                         shape.reset();\r
 815                         shape.setCapsule(cap);\r
 816                         collidables[id].addShape(shape);\r
 817                 }\r
 818                 collidables[id].finish();\r
 819                 bodies[id].reset();\r
 820                 bodies[id].setMass(3.0f);\r
 821                 bodies[id].setInertia(pfxCalcInertiaBox(PfxVector3(2.5f,1.0f,1.0f),3.0f));\r
 822                 states[id].reset();\r
 823                 states[id].setPosition(PfxVector3(0.0f,5.0f,0.0f));\r
 824                 states[id].setMotionType(kPfxMotionTypeActive);\r
 825                 states[id].setUseSleep(1); // sleep mode ON\r
 826                 states[id].setRigidBodyId(id);\r
 827         }\r
 828 }\r
 829 \r
 830 void createSceneFalling()\r
 831 {\r
 832         int size = 6;\r
 833         const float cubeSize = 1.0f;\r
 834         float spacing = cubeSize * 5.0f;\r
 835         PfxVector3 pos(0.0f, cubeSize * 2, 0.0f);\r
 836         float offset = -size * (cubeSize * 2.0f + spacing) * 0.5f;\r
 837 \r
 838         for(int k=0;k<5;k++) {\r
 839                 for(int j=0;j<size;j++) {\r
 840                         pos[2] = offset + (float)j * (cubeSize * 2.0f + spacing);\r
 841                         for(int i=0;i<size;i++) {\r
 842                                 pos[0] = offset + (float)i * (cubeSize * 2.0f + spacing);\r
 843                                 createBrick(pos,PfxQuat::identity(),PfxVector3(cubeSize),1.0f);\r
 844                         }\r
 845                 }\r
 846                 offset -= 0.05f * spacing * (size-1);\r
 847                 spacing *= 1.01f;\r
 848                 pos[1] += (cubeSize * 2.0f + spacing);\r
 849         }\r
 850 }\r
 851 \r
 852 void createSceneBoxGround()\r
 853 {\r
 854         int id = numRigidBodies++;\r
 855         PfxBox box(150.0f,2.5f,150.0f);\r
 856         PfxShape shape;\r
 857         shape.reset();\r
 858         shape.setBox(box);\r
 859         collidables[id].reset();\r
 860         collidables[id].addShape(shape);\r
 861         collidables[id].finish();\r
 862         bodies[id].reset();\r
 863         states[id].reset();\r
 864         states[id].setPosition(PfxVector3(0.0f,-2.5f,0.0f));\r
 865         states[id].setMotionType(kPfxMotionTypeFixed);\r
 866         states[id].setRigidBodyId(id);\r
 867 }\r
 868 \r
 869 void createSceneLandscape()\r
 870 {\r
 871         PfxCreateLargeTriMeshParam param;\r
 872 \r
 873         param.verts = LargeMeshVtx;\r
 874         param.numVerts = LargeMeshVtxCount;\r
 875         param.vertexStrideBytes = sizeof(float)*6;\r
 876 \r
 877         param.triangles = LargeMeshIdx;\r
 878         param.numTriangles = LargeMeshIdxCount/3;\r
 879         param.triangleStrideBytes = sizeof(unsigned short)*3;\r
 880 \r
 881         if(gLargeMesh.m_numIslands > 0) {\r
 882                 pfxReleaseLargeTriMesh(gLargeMesh);\r
 883         }\r
 884 \r
 885         PfxInt32 ret = pfxCreateLargeTriMesh(gLargeMesh,param);\r
 886         if(ret != SCE_PFX_OK) {\r
 887                 SCE_PFX_PRINTF("Can't create large mesh.\n");\r
 888         }\r
 889 \r
 890         int id = numRigidBodies++;\r
 891         PfxShape shape;\r
 892         shape.reset();\r
 893         shape.setLargeTriMesh(&gLargeMesh);\r
 894         collidables[id].reset();\r
 895         collidables[id].addShape(shape);\r
 896         collidables[id].finish();\r
 897         bodies[id].reset();\r
 898         states[id].reset();\r
 899         states[id].setPosition(PfxVector3(0.0f,0.0f,0.0f));\r
 900         states[id].setMotionType(kPfxMotionTypeFixed);\r
 901         states[id].setRigidBodyId(id);\r
 902 }\r
 903 \r
 904 void initRays(const PfxVector3 &p1,const PfxVector3 &p2)\r
 905 {\r
 906         numRays = NUM_RAYS;\r
 907         PfxQuat rot = PfxQuat::rotationY(2.0f*SCE_PFX_PI/(float)numRays);\r
 908         PfxVector3 startPosition = p1;\r
 909         PfxVector3 targetPosition = p2;\r
 910 \r
 911         for(int i=0;i<numRays;i++) {\r
 912                 rayInputs[i].reset();\r
 913                 rayInputs[i].m_startPosition = startPosition;\r
 914                 rayInputs[i].m_direction = targetPosition - startPosition;\r
 915 \r
 916                 startPosition = rotate(rot,startPosition);\r
 917                 targetPosition = rotate(rot,targetPosition);\r
 918         }\r
 919 }\r
 920 \r
 921 void createSceneRayFilter()\r
 922 {\r
 923         int idA = createBrick(PfxVector3(0.0f,2.0f,0.0f),PfxQuat::identity(),PfxVector3(5.0f,0.25f,5.0f),1.0f);\r
 924         int idB = createBrick(PfxVector3(0.0f,4.0f,0.0f),PfxQuat::identity(),PfxVector3(5.0f,0.25f,5.0f),1.0f);\r
 925         int idC = createBrick(PfxVector3(0.0f,6.0f,0.0f),PfxQuat::identity(),PfxVector3(5.0f,0.25f,5.0f),1.0f);\r
 926         int idD = createBrick(PfxVector3(0.0f,8.0f,0.0f),PfxQuat::identity(),PfxVector3(5.0f,0.25f,5.0f),1.0f);\r
 927 \r
 928         states[idA].setMotionType(kPfxMotionTypeFixed);\r
 929         states[idB].setMotionType(kPfxMotionTypeFixed);\r
 930         states[idC].setMotionType(kPfxMotionTypeFixed);\r
 931         states[idD].setMotionType(kPfxMotionTypeFixed);\r
 932 \r
 933         states[idA].setContactFilterSelf(0x01);\r
 934         states[idB].setContactFilterSelf(0x02);\r
 935         states[idC].setContactFilterSelf(0x04);\r
 936         states[idD].setContactFilterSelf(0x08);\r
 937 \r
 938         numRays = 5;\r
 939         for(int i=0;i<numRays;i++) {\r
 940                 rayInputs[i].reset();\r
 941                 rayInputs[i].m_startPosition = PfxVector3(-2.0f+i,10.0f,0.0f);\r
 942                 rayInputs[i].m_direction = PfxVector3(0.0f,-10.0f,2.0f);\r
 943                 rayInputs[i].m_contactFilterTarget = 1<<i;\r
 944         }\r
 945 }\r
 946 \r
 947 void physics_create_scene(int sceneId)\r
 948 {\r
 949         const int numScenes = 4;\r
 950         int sid = sceneId % numScenes;\r
 951         \r
 952         numRigidBodies= 0;\r
 953         pairSwap = 0;\r
 954         numPairs[0] = 0;\r
 955         numPairs[1] = 0;\r
 956         numContacts = 0;\r
 957         numContactIdPool = 0;\r
 958         numJoints = 0;\r
 959         island = NULL;\r
 960         frame = 0;\r
 961         doAreaRaycast = false;\r
 962         \r
 963         switch(sid) {\r
 964                 case 0: // simple primitives\r
 965                 createSceneBoxGround();\r
 966                 createScenePrimitives();\r
 967                 initRays(PfxVector3(15.0f,5.0f,0.0f),PfxVector3(3.0f,1.0f,0.0f));\r
 968                 break;\r
 969 \r
 970                 case 1: // landscape\r
 971                 createSceneLandscape();\r
 972                 createScenePrimitives();\r
 973                 initRays(PfxVector3(15.0f,2.5f,0.0f),PfxVector3(3.0f,-2.5f,0.0f));\r
 974                 break;\r
 975 \r
 976                 case 2: // raycast filtering\r
 977                 createSceneRayFilter();\r
 978                 break;\r
 979 \r
 980                 case 3: // area raycast\r
 981                 createSceneBoxGround();\r
 982                 createSceneFalling();\r
 983                 initRays(PfxVector3(15.0f,5.0f,0.0f),PfxVector3(14.0f,-1.0f,0.0f));\r
 984                 doAreaRaycast = true;\r
 985                 break;\r
 986         }\r
 987 \r
 988         SCE_PFX_PRINTF("----- Size of rigid body buffer ------\n");\r
 989         SCE_PFX_PRINTF("                    size *   num = total\n");\r
 990         SCE_PFX_PRINTF("PfxRigidState      %5d * %5d = %5d bytes\n",sizeof(PfxRigidState),numRigidBodies,sizeof(PfxRigidState)*numRigidBodies);\r
 991         SCE_PFX_PRINTF("PfxRigidBody       %5d * %5d = %5d bytes\n",sizeof(PfxRigidBody),numRigidBodies,sizeof(PfxRigidBody)*numRigidBodies);\r
 992         SCE_PFX_PRINTF("PfxCollidable      %5d * %5d = %5d bytes\n",sizeof(PfxCollidable),numRigidBodies,sizeof(PfxCollidable)*numRigidBodies);\r
 993         SCE_PFX_PRINTF("PfxJoint           %5d * %5d = %5d bytes\n",sizeof(PfxJoint),numJoints,sizeof(PfxJoint)*numJoints);\r
 994         SCE_PFX_PRINTF("PfxSolverBody      %5d * %5d = %5d bytes\n",sizeof(PfxSolverBody),numRigidBodies,sizeof(PfxSolverBody)*numRigidBodies);\r
 995         SCE_PFX_PRINTF("PfxBroadphaseProxy %5d * %5d = %5d bytes\n",sizeof(PfxBroadphaseProxy),numRigidBodies,sizeof(PfxBroadphaseProxy)*numRigidBodies);\r
 996         SCE_PFX_PRINTF("PfxContactManifold %5d * %5d = %5d bytes\n",sizeof(PfxContactManifold),NUM_CONTACTS,sizeof(PfxContactManifold)*NUM_CONTACTS);\r
 997         SCE_PFX_PRINTF("PfxBroadphasePair  %5d * %5d = %5d bytes\n",sizeof(PfxBroadphasePair),NUM_CONTACTS,sizeof(PfxBroadphasePair)*NUM_CONTACTS);\r
 998 \r
 999         int totalBytes = \r
1000                 (sizeof(PfxRigidState) + sizeof(PfxRigidBody) + sizeof(PfxCollidable) + sizeof(PfxSolverBody) + sizeof(PfxBroadphaseProxy)) * numRigidBodies +\r
1001                 (sizeof(PfxContactManifold) + sizeof(PfxBroadphasePair)) * NUM_CONTACTS;\r
1002         SCE_PFX_PRINTF("----------------------------------------------------------\n");\r
1003         SCE_PFX_PRINTF("Total %5d bytes\n",totalBytes);\r
1004 }\r
1005 \r
1006 ///////////////////////////////////////////////////////////////////////////////\r
1007 // Initialize / Finalize Engine\r
1008 \r
1009 bool physics_init()\r
1010 {\r
1011         return true;\r
1012 }\r
1013 \r
1014 void physics_release()\r
1015 {\r
1016 }\r
1017 \r
1018 ///////////////////////////////////////////////////////////////////////////////\r
1019 // Pick\r
1020 \r
1021 PfxVector3 physics_pick_start(const PfxVector3 &p1,const PfxVector3 &p2)\r
1022 {\r
1023         return PfxVector3(0.0f);\r
1024 }\r
1025 \r
1026 void physics_pick_update(const PfxVector3 &p)\r
1027 {\r
1028 }\r
1029 \r
1030 void physics_pick_end()\r
1031 {\r
1032 }\r
1033 \r
1034 ///////////////////////////////////////////////////////////////////////////////\r
1035 // Get Information\r
1036 \r
1037 int physics_get_num_rigidbodies()\r
1038 {\r
1039         return numRigidBodies;\r
1040 }\r
1041 \r
1042 const PfxRigidState& physics_get_state(int id)\r
1043 {\r
1044         return states[id];\r
1045 }\r
1046 \r
1047 const PfxRigidBody& physics_get_body(int id)\r
1048 {\r
1049         return bodies[id];\r
1050 }\r
1051 \r
1052 const PfxCollidable& physics_get_collidable(int id)\r
1053 {\r
1054         return collidables[id];\r
1055 }\r
1056 \r
1057 int physics_get_num_rays()\r
1058 {\r
1059         return numRays;\r
1060 }\r
1061 \r
1062 const PfxRayInput& physics_get_rayinput(int id)\r
1063 {\r
1064         return rayInputs[id];\r
1065 }\r
1066 \r
1067 const PfxRayOutput& physics_get_rayoutput(int id)\r
1068 {\r
1069         return rayOutputs[id];\r
1070 }\r
1071 \r
1072 int physics_get_num_contacts()\r
1073 {\r
1074         return numPairs[pairSwap];\r
1075 }\r
1076 \r
1077 const PfxContactManifold &physics_get_contact(int id)\r
1078 {\r
1079         return contacts[pfxGetConstraintId(pairsBuff[pairSwap][id])];\r
1080 }\r
1081 \r
1082 const PfxIsland* physics_get_islands()\r
1083 {\r
1084         return island;\r
1085 }\r