2 Physics Effects Copyright(C) 2010 Sony Computer Entertainment Inc.
\r
5 Physics Effects is open software; you can redistribute it and/or
\r
6 modify it under the terms of the BSD License.
\r
8 Physics Effects is distributed in the hope that it will be useful,
\r
9 but WITHOUT ANY WARRANTY; without even the implied warranty of
\r
10 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
\r
11 See the BSD License for more details.
\r
13 A copy of the BSD License is distributed with
\r
14 Physics Effects under the filename: physics_effects_license.txt
\r
17 #include "physics_func.h"
\r
18 #include "../common/perf_func.h"
\r
20 ///////////////////////////////////////////////////////////////////////////////
\r
23 #define NUM_RIGIDBODIES 500
\r
24 #define NUM_JOINTS 500
\r
25 #define NUM_CONTACTS 4000
\r
26 #define NUM_RAYS 100
\r
28 const float timeStep = 0.016f;
\r
29 const float separateBias = 0.1f;
\r
34 PfxVector3 worldCenter(0.0f);
\r
35 PfxVector3 worldExtent(500.0f);
\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
45 //J 地形を表現するためのラージメッシュ
\r
46 //E Large mesh for representing a landscape
\r
47 #include "landscape.h"
\r
48 PfxLargeTriMesh gLargeMesh;
\r
53 PfxConvexMesh gConvex;
\r
57 PfxBroadphaseProxy proxies[6][NUM_RIGIDBODIES]; // shared by simulation and raycast
\r
61 PfxConstraintPair jointPairs[NUM_JOINTS];
\r
62 PfxJoint joints[NUM_JOINTS];
\r
67 unsigned int pairSwap;
\r
68 unsigned int numPairs[2];
\r
69 PfxBroadphasePair pairsBuff[2][NUM_CONTACTS];
\r
73 PfxContactManifold contacts[NUM_CONTACTS];
\r
76 PfxUInt32 contactIdPool[NUM_CONTACTS];
\r
77 int numContactIdPool;
\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
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
94 //E Count to enter sleeping
\r
95 const PfxUInt32 sleepCount = 180;
\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
102 //E Temporary buffers
\r
103 #define POOL_BYTES (5*1024*1024)
\r
104 unsigned char SCE_PFX_ALIGNED(128) poolBuff[POOL_BYTES];
\r
106 //J 一時バッファ用スタックアロケータ
\r
107 //E Stack allocator for temporary buffers
\r
108 PfxHeapManager pool(poolBuff,POOL_BYTES);
\r
112 PfxRayInput SCE_PFX_ALIGNED(128) rayInputs[NUM_RAYS];
\r
113 PfxRayOutput SCE_PFX_ALIGNED(128) rayOutputs[NUM_RAYS];
\r
117 doAreaRaycastがtrueの場合、指定された領域内の剛体のみ判定対象とする
\r
119 bool doAreaRaycast;
\r
120 PfxVector3 areaCenter(0.0f);
\r
121 PfxVector3 areaExtent(5.0f);
\r
123 ///////////////////////////////////////////////////////////////////////////////
\r
124 // Simulation Function
\r
130 pairSwap = 1-pairSwap;
\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
137 //J 剛体が最も分散している軸を見つける
\r
138 //E Find the axis along which all rigid bodies are most widely positioned
\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
145 s2 += mulPerElem(c,c);
\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
152 //J ブロードフェーズプロキシの更新
\r
153 //E Create broadpahse proxies
\r
155 //J レイキャストと共用するため、全ての軸に対するプロキシ配列を作成する
\r
156 //E To share with ray casting, create proxy arrays for all axis
\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
173 PfxUpdateBroadphaseProxiesResult result;
\r
175 int ret = pfxUpdateBroadphaseProxies(param,result);
\r
176 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxUpdateBroadphaseProxies failed %d\n",ret);
\r
178 pool.deallocate(param.workBuff);
\r
182 //E Find overlapped pairs
\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
194 PfxFindPairsResult findPairsResult;
\r
196 int ret = pfxFindPairs(findPairsParam,findPairsResult);
\r
197 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxFindPairs failed %d\n",ret);
\r
199 pool.deallocate(findPairsParam.workBuff);
\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
213 PfxDecomposePairsResult decomposePairsResult;
\r
215 ret = pfxDecomposePairs(decomposePairsParam,decomposePairsResult);
\r
216 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxDecomposePairs failed %d\n",ret);
\r
218 pool.deallocate(decomposePairsParam.workBuff);
\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
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
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
239 if(stateB.isAsleep()) {
\r
244 //J 新規ペアのコンタクトのリンクと初期化
\r
245 //E Add new contacts and initialize
\r
246 for(PfxUInt32 i=0;i<numOutNewPairs;i++) {
\r
248 if(numContactIdPool > 0) {
\r
249 cId = contactIdPool[--numContactIdPool];
\r
252 cId = numContacts++;
\r
254 if(cId >= NUM_CONTACTS) {
\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
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
269 if(stateB.isAsleep()) {
\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
280 for(PfxUInt32 i=0;i<numOutNewPairs;i++) {
\r
281 currentPairs[numCurrentPairs++] = outNewPairs[i];
\r
284 pool.deallocate(decomposePairsParam.pairBuff);
\r
285 pool.deallocate(findPairsParam.pairBuff);
\r
289 int workBytes = sizeof(PfxBroadphasePair) * numCurrentPairs;
\r
290 void *workBuff = pool.allocate(workBytes);
\r
292 pfxParallelSort(currentPairs,numCurrentPairs,workBuff,workBytes);
\r
294 pool.deallocate(workBuff);
\r
300 unsigned int numCurrentPairs = numPairs[pairSwap];
\r
301 PfxBroadphasePair *currentPairs = pairsBuff[pairSwap];
\r
304 //E Detect collisions
\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
314 int ret = pfxDetectCollision(param);
\r
315 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxDetectCollision failed %d\n",ret);
\r
319 //E Refresh contacts
\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
328 int ret = pfxRefreshContacts(param);
\r
329 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxRefreshContacts failed %d\n",ret);
\r
333 //E Create simulation islands
\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
342 PfxGenerateIslandResult result;
\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
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
355 void constraintSolver()
\r
359 unsigned int numCurrentPairs = numPairs[pairSwap];
\r
360 PfxBroadphasePair *currentPairs = pairsBuff[pairSwap];
\r
362 pc.countBegin("setup solver bodies");
\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
370 int ret = pfxSetupSolverBodies(param);
\r
371 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxSetupSolverBodies failed %d\n",ret);
\r
375 pc.countBegin("setup contact constraints");
\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
388 int ret = pfxSetupContactConstraints(param);
\r
389 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxSetupJointConstraints failed %d\n",ret);
\r
393 pc.countBegin("setup joint constraints");
\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
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
409 int ret = pfxSetupJointConstraints(param);
\r
410 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxSetupJointConstraints failed %d\n",ret);
\r
414 pc.countBegin("solve constraints");
\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
430 int ret = pfxSolveConstraints(param);
\r
431 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxSolveConstraints failed %d\n",ret);
\r
433 pool.deallocate(param.workBuff);
\r
440 void sleepOrWakeup()
\r
442 PfxFloat sleepVelSqr = sleepVelocity * sleepVelocity;
\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
450 if(state.isAwake()) {
\r
451 if( linVelSqr < sleepVelSqr && angVelSqr < sleepVelSqr ) {
\r
452 state.incrementSleepCount();
\r
455 state.resetSleepCount();
\r
462 for(PfxUInt32 i=0;i<pfxGetNumIslands(island);i++) {
\r
465 int numCanSleep = 0;
\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
477 if(state.getSleepCount() > sleepCount) {
\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
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
507 PfxUpdateRigidStatesParam param;
\r
508 param.states = states;
\r
509 param.bodies = bodies;
\r
510 param.numRigidBodies = numRigidBodies;
\r
511 param.timeStep = timeStep;
\r
513 pfxUpdateRigidStates(param);
\r
516 int gatherBroadphaseProxiesInArea(const PfxVector3 ¢er,const PfxVector3 &extent)
\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
534 PfxUpdateBroadphaseProxiesResult result;
\r
536 int ret = pfxUpdateBroadphaseProxies(param,result);
\r
537 if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxUpdateBroadphaseProxies failed %d\n",ret);
\r
539 pool.deallocate(param.workBuff);
\r
541 return numRigidBodies - result.numOutOfWorldProxies;
\r
546 PfxRayCastParam param;
\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
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
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
581 pfxCastRays(rayInputs,rayOutputs,numRays,param);
\r
584 void physics_simulate()
\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
592 perf_push_marker("broadphase");
\r
593 pc.countBegin("broadphase");
\r
598 perf_push_marker("collision");
\r
599 pc.countBegin("collision");
\r
604 perf_push_marker("solver");
\r
605 pc.countBegin("solver");
\r
606 constraintSolver();
\r
610 perf_push_marker("sleepCheck");
\r
611 pc.countBegin("sleepCheck");
\r
616 perf_push_marker("integrate");
\r
617 pc.countBegin("integrate");
\r
622 perf_push_marker("castRays");
\r
623 pc.countBegin("castRays");
\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
643 ///////////////////////////////////////////////////////////////////////////////
\r
646 int createBrick(const PfxVector3 &pos,const PfxQuat &rot,const PfxVector3 &boxSize,PfxFloat mass)
\r
648 int id = numRigidBodies++;
\r
650 PfxBox box(boxSize);
\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
671 void createScenePrimitives()
\r
675 int id = numRigidBodies++;
\r
676 PfxSphere sphere(1.0f);
\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
695 int id = numRigidBodies++;
\r
696 PfxBox box(1.0f,1.0f,1.0f);
\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
715 int id = numRigidBodies++;
\r
716 PfxCapsule capsule(1.5f,0.5f);
\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
735 int id = numRigidBodies++;
\r
736 PfxCylinder cylinder(0.5f,1.5f);
\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
755 PfxCreateConvexMeshParam param;
\r
757 param.verts = BarrelVtx;
\r
758 param.numVerts = BarrelVtxCount;
\r
759 param.vertexStrideBytes = sizeof(float)*6;
\r
761 param.triangles = BarrelIdx;
\r
762 param.numTriangles = BarrelIdxCount/3;
\r
763 param.triangleStrideBytes = sizeof(unsigned short)*3;
\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
770 int id = numRigidBodies++;
\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
787 // combined primitives
\r
789 int id = numRigidBodies++;
\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
796 PfxBox box(0.5f,0.5f,1.5f);
\r
800 shape.setOffsetPosition(PfxVector3(-2.0f,0.0f,0.0f));
\r
801 collidables[id].addShape(shape);
\r
804 PfxBox box(0.5f,1.5f,0.5f);
\r
808 shape.setOffsetPosition(PfxVector3(2.0f,0.0f,0.0f));
\r
809 collidables[id].addShape(shape);
\r
812 PfxCapsule cap(1.5f,0.5f);
\r
815 shape.setCapsule(cap);
\r
816 collidables[id].addShape(shape);
\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
830 void createSceneFalling()
\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
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
846 offset -= 0.05f * spacing * (size-1);
\r
848 pos[1] += (cubeSize * 2.0f + spacing);
\r
852 void createSceneBoxGround()
\r
854 int id = numRigidBodies++;
\r
855 PfxBox box(150.0f,2.5f,150.0f);
\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
869 void createSceneLandscape()
\r
871 PfxCreateLargeTriMeshParam param;
\r
873 param.verts = LargeMeshVtx;
\r
874 param.numVerts = LargeMeshVtxCount;
\r
875 param.vertexStrideBytes = sizeof(float)*6;
\r
877 param.triangles = LargeMeshIdx;
\r
878 param.numTriangles = LargeMeshIdxCount/3;
\r
879 param.triangleStrideBytes = sizeof(unsigned short)*3;
\r
881 if(gLargeMesh.m_numIslands > 0) {
\r
882 pfxReleaseLargeTriMesh(gLargeMesh);
\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
890 int id = numRigidBodies++;
\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
904 void initRays(const PfxVector3 &p1,const PfxVector3 &p2)
\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
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
916 startPosition = rotate(rot,startPosition);
\r
917 targetPosition = rotate(rot,targetPosition);
\r
921 void createSceneRayFilter()
\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
928 states[idA].setMotionType(kPfxMotionTypeFixed);
\r
929 states[idB].setMotionType(kPfxMotionTypeFixed);
\r
930 states[idC].setMotionType(kPfxMotionTypeFixed);
\r
931 states[idD].setMotionType(kPfxMotionTypeFixed);
\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
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
947 void physics_create_scene(int sceneId)
\r
949 const int numScenes = 4;
\r
950 int sid = sceneId % numScenes;
\r
957 numContactIdPool = 0;
\r
961 doAreaRaycast = false;
\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
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
976 case 2: // raycast filtering
\r
977 createSceneRayFilter();
\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
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
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
1006 ///////////////////////////////////////////////////////////////////////////////
\r
1007 // Initialize / Finalize Engine
\r
1009 bool physics_init()
\r
1014 void physics_release()
\r
1018 ///////////////////////////////////////////////////////////////////////////////
\r
1021 PfxVector3 physics_pick_start(const PfxVector3 &p1,const PfxVector3 &p2)
\r
1023 return PfxVector3(0.0f);
\r
1026 void physics_pick_update(const PfxVector3 &p)
\r
1030 void physics_pick_end()
\r
1034 ///////////////////////////////////////////////////////////////////////////////
\r
1035 // Get Information
\r
1037 int physics_get_num_rigidbodies()
\r
1039 return numRigidBodies;
\r
1042 const PfxRigidState& physics_get_state(int id)
\r
1044 return states[id];
\r
1047 const PfxRigidBody& physics_get_body(int id)
\r
1049 return bodies[id];
\r
1052 const PfxCollidable& physics_get_collidable(int id)
\r
1054 return collidables[id];
\r
1057 int physics_get_num_rays()
\r
1062 const PfxRayInput& physics_get_rayinput(int id)
\r
1064 return rayInputs[id];
\r
1067 const PfxRayOutput& physics_get_rayoutput(int id)
\r
1069 return rayOutputs[id];
\r
1072 int physics_get_num_contacts()
\r
1074 return numPairs[pairSwap];
\r
1077 const PfxContactManifold &physics_get_contact(int id)
\r
1079 return contacts[pfxGetConstraintId(pairsBuff[pairSwap][id])];
\r
1082 const PfxIsland* physics_get_islands()
\r