static void runNEONTests() { uint64_t features = android_getCpuFeatures(); if ((features & ANDROID_CPU_ARM_FEATURE_NEON) == 0) { SCE_PFX_PRINTF("CPU has NO support for NEON."); } else { SCE_PFX_PRINTF("CPU HAS support for NEON.\n"); #ifdef __ARM_NEON__ #ifdef TEST_NEON_PERFORMANCE SCE_PFX_PRINTF("Running NEON performance tests...\n"); TestNeonDotProduct(); TestNeonCrossProduct(); TestNeonMatrix3OperatorMultiply(); TestNeonMatrix4OperatorMultiply(); TestNeonOrthoInverseTransform3(); TestNeonTransform3OperatorMultiply(); TestNeonTransposeMatrix3(); TestNeonSolveLinearConstraintRow(); SCE_PFX_PRINTF("Finished NEON performance tests."); #endif // TEST_NEON_PERFORMANCE #endif // __ARM_NEON__ } }
void collision() { unsigned int numCurrentPairs = numPairs[pairSwap]; PfxBroadphasePair *currentPairs = pairsBuff[pairSwap]; //J 衝突検出 //E Detect collisions { PfxDetectCollisionParam param; param.contactPairs = currentPairs; param.numContactPairs = numCurrentPairs; param.offsetContactManifolds = contacts; param.offsetRigidStates = states; param.offsetCollidables = collidables; param.numRigidBodies = numRigidBodies; int ret = pfxDetectCollision(param); if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxDetectCollision failed %d\n",ret); } //J リフレッシュ //E Refresh contacts { PfxRefreshContactsParam param; param.contactPairs = currentPairs; param.numContactPairs = numCurrentPairs; param.offsetContactManifolds = contacts; param.offsetRigidStates = states; param.numRigidBodies = numRigidBodies; int ret = pfxRefreshContacts(param); if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxRefreshContacts failed %d\n",ret); } }
void collision() { unsigned int numCurrentPairs = numPairs[pairSwap]; PfxBroadphasePair *currentPairs = pairsBuff[pairSwap]; //J 衝突検出 //E Detect collisions { PfxDetectCollisionParam param; param.contactPairs = currentPairs; param.numContactPairs = numCurrentPairs; param.offsetContactManifolds = contacts; param.offsetRigidStates = states; param.offsetCollidables = collidables; param.numRigidBodies = numRigidBodies; int ret = pfxDetectCollision(param); if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxDetectCollision failed %d\n",ret); } //J リフレッシュ //E Refresh contacts { PfxRefreshContactsParam param; param.contactPairs = currentPairs; param.numContactPairs = numCurrentPairs; param.offsetContactManifolds = contacts; param.offsetRigidStates = states; param.numRigidBodies = numRigidBodies; int ret = pfxRefreshContacts(param); if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxRefreshContacts failed %d\n",ret); } //J アイランド生成 //E Create simulation islands { PfxGenerateIslandParam param; param.islandBuff = islandBuff; param.islandBytes = 32*NUM_RIGIDBODIES; param.pairs = currentPairs; param.numPairs = numCurrentPairs; param.numObjects = numRigidBodies; PfxGenerateIslandResult result; int ret = pfxGenerateIsland(param,result); if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxGenerateIsland failed %d\n",ret); island = result.island; //J ジョイント分のペアを追加 //E Add joint pairs to islands ret = pfxAppendPairs(island,jointPairs,numJoints); if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxAppendPairs failed %d\n",ret); } }
void collision() { unsigned int numCurrentPairs = numPairs[pairSwap]; PfxBroadphasePair *currentPairs = pairsBuff[pairSwap]; //J 衝突検出 //E Detect collisions { PfxDetectCollisionParam param; param.contactPairs = currentPairs; param.numContactPairs = numCurrentPairs; param.offsetContactManifolds = contacts; param.offsetRigidStates = states; param.offsetCollidables = collidables; param.numRigidBodies = numRigidBodies; int ret = pfxDetectCollision(param); if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxDetectCollision failed %d\n",ret); } curTotalContacts = 0; for(PfxUInt32 i=0;i<numCurrentPairs;i++) { PfxConstraintPair &pair = currentPairs[i]; PfxUInt16 iA = pfxGetObjectIdA(pair); PfxUInt16 iB = pfxGetObjectIdB(pair); PfxUInt32 iConstraint = pfxGetConstraintId(pair); PfxContactManifold &contact = contacts[iConstraint]; curTotalContacts += contact.getNumContacts(); } //J リフレッシュ //E Refresh contacts { PfxRefreshContactsParam param; param.contactPairs = currentPairs; param.numContactPairs = numCurrentPairs; param.offsetContactManifolds = contacts; param.offsetRigidStates = states; param.numRigidBodies = numRigidBodies; int ret = pfxRefreshContacts(param); if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxRefreshContacts failed %d\n",ret); } }
int WINAPI WinMain(HINSTANCE hInstance,HINSTANCE hPrevInstance,LPSTR lpCmdLine,int nCmdShow) { if(!createWindow(SAMPLE_NAME,DISPLAY_WIDTH,DISPLAY_HEIGHT)) { MessageBox(NULL,"Can't create gl window.","ERROR",MB_OK|MB_ICONEXCLAMATION); return 0; } init(); physics_create_scene(sceneId); PfxPerfCounter counter; counter.countBegin("dt"); SCE_PFX_PRINTF("## %s: INIT SUCCEEDED ##\n", SAMPLE_NAME); MSG msg; while(s_isRunning) { if(PeekMessage(&msg,NULL,0,0,PM_REMOVE)) { if(msg.message==WM_QUIT) { s_isRunning = false; } else { TranslateMessage(&msg); DispatchMessage(&msg); } } else { counter.countEnd(); float dt = counter.getCountTime(0)/1000.f; update(dt); if(simulating) stepSimulation(dt); counter.resetCount(); counter.countBegin("dt"); render(); perf_sync(); } } shutdown(); SCE_PFX_PRINTF("## %s: FINISHED ##\n", SAMPLE_NAME); releaseWindow(); return (msg.wParam); }
void createSceneLandscape() { PfxCreateLargeTriMeshParam param; param.verts = LargeMeshVtx; param.numVerts = LargeMeshVtxCount; param.vertexStrideBytes = sizeof(float)*6; param.triangles = LargeMeshIdx; param.numTriangles = LargeMeshIdxCount/3; param.triangleStrideBytes = sizeof(unsigned short)*3; if(gLargeMesh.m_numIslands > 0) { pfxReleaseLargeTriMesh(gLargeMesh); } PfxInt32 ret = pfxCreateLargeTriMesh(gLargeMesh,param); if(ret != SCE_PFX_OK) { SCE_PFX_PRINTF("Can't create large mesh.\n"); } int id = numRigidBodies++; PfxShape shape; shape.reset(); shape.setLargeTriMesh(&gLargeMesh); collidables[id].reset(); collidables[id].addShape(shape); collidables[id].finish(); bodies[id].reset(); states[id].reset(); states[id].setPosition(PfxVector3(0.0f,-5.0f,0.0f)); states[id].setOrientation(PfxQuat::rotationX(0.5f)*PfxQuat::rotationY(0.7f)); states[id].setMotionType(kPfxMotionTypeFixed); states[id].setRigidBodyId(id); }
int gatherBroadphaseProxiesInArea(const PfxVector3 ¢er,const PfxVector3 &extent) { PfxUpdateBroadphaseProxiesParam param; param.workBytes = pfxGetWorkBytesOfUpdateBroadphaseProxies(numRigidBodies); param.workBuff = pool.allocate(param.workBytes,128); param.numRigidBodies = numRigidBodies; param.offsetRigidStates = states; param.offsetCollidables = collidables; param.proxiesX = proxies[0]; param.proxiesY = proxies[1]; param.proxiesZ = proxies[2]; param.proxiesXb = proxies[3]; param.proxiesYb = proxies[4]; param.proxiesZb = proxies[5]; param.worldCenter = center; param.worldExtent = extent; param.outOfWorldBehavior = SCE_PFX_OUT_OF_WORLD_BEHAVIOR_REMOVE_PROXY; PfxUpdateBroadphaseProxiesResult result; int ret = pfxUpdateBroadphaseProxies(param,result); if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxUpdateBroadphaseProxies failed %d\n",ret); pool.deallocate(param.workBuff); return numRigidBodies - result.numOutOfWorldProxies; }
void physics_simulate() { PfxPerfCounter pc; for(int i=1;i<numRigidBodies;i++) { pfxApplyExternalForce(states[i],bodies[i],bodies[i].getMass()*PfxVector3(0.0f,-9.8f,0.0f),PfxVector3(0.0f),timeStep); } perf_push_marker("broadphase"); pc.countBegin("broadphase"); broadphase(); pc.countEnd(); perf_pop_marker(); perf_push_marker("collision"); pc.countBegin("collision"); collision(); pc.countEnd(); perf_pop_marker(); perf_push_marker("solver"); pc.countBegin("solver"); constraintSolver(); pc.countEnd(); perf_pop_marker(); perf_push_marker("sleepCheck"); pc.countBegin("sleepCheck"); sleepOrWakeup(); pc.countEnd(); perf_pop_marker(); perf_push_marker("integrate"); pc.countBegin("integrate"); integrate(); pc.countEnd(); perf_pop_marker(); perf_push_marker("castRays"); pc.countBegin("castRays"); castRays(); pc.countEnd(); perf_pop_marker(); frame++; if(frame%100 == 0) { float broadphaseTime = pc.getCountTime(0); float collisionTime = pc.getCountTime(2); float solverTime = pc.getCountTime(4); float sleepTime = pc.getCountTime(6); float integrateTime = pc.getCountTime(8); float raycastTime = pc.getCountTime(10); SCE_PFX_PRINTF("frame %3d broadphase %.2f collision %.2f solver %.2f sleepCheck %.2f integrate %.2f raycast %.2f | total %.2f\n",frame, broadphaseTime,collisionTime,solverTime,sleepTime,integrateTime,raycastTime, broadphaseTime+collisionTime+solverTime+sleepTime+integrateTime+raycastTime); } }
void collision() { //J 衝突検出 //E Detect collisions { PfxDetectCollisionParam param; param.contactPairs = pairs; param.numContactPairs = numPairs; param.offsetContactManifolds = contacts; param.offsetRigidStates = states; param.offsetCollidables = collidables; param.numRigidBodies = numRigidBodies; int ret = pfxDetectCollision(param); if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxDetectCollision failed %d\n",ret); } }
void broadphase() { //J 剛体が最も分散している軸を見つける //E Find the axis along which all rigid bodies are most widely positioned int axis = 0; { PfxVector3 s(0.0f),s2(0.0f); for(int i=0;i<numRigidBodies;i++) { PfxVector3 c = states[i].getPosition(); s += c; s2 += mulPerElem(c,c); } PfxVector3 v = s2 - mulPerElem(s,s) / (float)numRigidBodies; if(v[1] > v[0]) axis = 1; if(v[2] > v[axis]) axis = 2; } //J ブロードフェーズプロキシの更新 //E Create broadpahse proxies { for(int i=0;i<numRigidBodies;i++) { pfxUpdateBroadphaseProxy(proxies[i],states[i],collidables[i],worldCenter,worldExtent,axis); } int workBytes = sizeof(PfxBroadphaseProxy) * numRigidBodies; void *workBuff = pool.allocate(workBytes); pfxParallelSort(proxies,numRigidBodies,workBuff,workBytes); pool.deallocate(workBuff); } //J 交差ペア探索 //E Find overlapped pairs { PfxFindPairsParam param; param.workBytes = pfxGetWorkBytesOfFindPairs(NUM_CONTACTS); param.workBuff = pool.allocate(param.workBytes); param.pairBytes = pfxGetPairBytesOfFindPairs(NUM_CONTACTS); param.pairBuff = pool.allocate(param.pairBytes); param.proxies = proxies; param.numProxies = numRigidBodies; param.maxPairs = NUM_CONTACTS; param.axis = axis; PfxFindPairsResult result; int ret = pfxFindPairs(param,result); if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxFindPairs failed %d\n",ret); memcpy(pairs,result.pairs,sizeof(PfxBroadphasePair)*result.numPairs); numPairs = result.numPairs; pool.deallocate(param.pairBuff); pool.deallocate(param.workBuff); } //J 新規ペアのコンタクトを初期化 //E Add new contacts and initialize for(PfxUInt32 i=0;i<numPairs;i++) { pfxSetContactId(pairs[i],i); PfxContactManifold &contact = contacts[i]; contact.reset(pfxGetObjectIdA(pairs[i]),pfxGetObjectIdB(pairs[i])); } numContacts = numPairs; }
virtual void reportErrorWarning(const char* warningString) { SCE_PFX_PRINTF(warningString); }
void physics_create_scene(int sceneId) { const int numScenes = 4; int sid = sceneId % numScenes; numRigidBodies= 0; // numPairs = 0; // numContacts = 0; numJoints = 0; // //frame = 0; switch(sid) { case 0: // simple primitives createSceneBoxGround(); createScenePrimitives(); break; case 1: // joints createSceneBoxGround(); createSceneJoints(); break; case 2: // stacking createSceneBoxGround(); createSceneStacking(); break; case 3: // landscape createSceneLandscape(); createScenePrimitives(); break; } SCE_PFX_PRINTF("----- Size of rigid body buffer ------\n"); SCE_PFX_PRINTF(" size * num = total\n"); SCE_PFX_PRINTF("PfxRigidState %5d * %5d = %5d bytes\n",sizeof(PfxRigidState),numRigidBodies,sizeof(PfxRigidState)*numRigidBodies); SCE_PFX_PRINTF("PfxRigidBody %5d * %5d = %5d bytes\n",sizeof(PfxRigidBody),numRigidBodies,sizeof(PfxRigidBody)*numRigidBodies); SCE_PFX_PRINTF("PfxCollidable %5d * %5d = %5d bytes\n",sizeof(PfxCollidable),numRigidBodies,sizeof(PfxCollidable)*numRigidBodies); //SCE_PFX_PRINTF("PfxJoint %5d * %5d = %5d bytes\n",sizeof(PfxJoint),numJoints,sizeof(PfxJoint)*numJoints); SCE_PFX_PRINTF("PfxSolverBody %5d * %5d = %5d bytes\n",sizeof(PfxSolverBody),numRigidBodies,sizeof(PfxSolverBody)*numRigidBodies); SCE_PFX_PRINTF("PfxBroadphaseProxy %5d * %5d = %5d bytes\n",sizeof(PfxBroadphaseProxy),numRigidBodies,sizeof(PfxBroadphaseProxy)*numRigidBodies); SCE_PFX_PRINTF("PfxContactManifold %5d * %5d = %5d bytes\n",sizeof(PfxContactManifold),NUM_CONTACTS,sizeof(PfxContactManifold)*NUM_CONTACTS); SCE_PFX_PRINTF("PfxBroadphasePair %5d * %5d = %5d bytes\n",sizeof(PfxBroadphasePair),NUM_CONTACTS,sizeof(PfxBroadphasePair)*NUM_CONTACTS); int totalBytes = (sizeof(PfxRigidState) + sizeof(PfxRigidBody) + sizeof(PfxCollidable) + sizeof(PfxSolverBody) + sizeof(PfxBroadphaseProxy)) * numRigidBodies + (sizeof(PfxContactManifold) + sizeof(PfxBroadphasePair)) * NUM_CONTACTS; //convert to Bullet convertToBullet(); SCE_PFX_PRINTF("----------------------------------------------------------\n"); SCE_PFX_PRINTF("Total %5d bytes\n",totalBytes); }
void createScenePrimitives() { // sphere { int id = numRigidBodies++; PfxSphere sphere(1.0f); PfxShape shape; shape.reset(); shape.setSphere(sphere); collidables[id].reset(); collidables[id].addShape(shape); collidables[id].finish(); bodies[id].reset(); bodies[id].setMass(1.0f); bodies[id].setInertia(pfxCalcInertiaSphere(1.0f,1.0f)); states[id].reset(); states[id].setPosition(PfxVector3(-5.0f,5.0f,0.0f)); states[id].setMotionType(kPfxMotionTypeActive); states[id].setRigidBodyId(id); } // box { int id = numRigidBodies++; PfxBox box(1.0f,1.0f,1.0f); PfxShape shape; shape.reset(); shape.setBox(box); collidables[id].reset(); collidables[id].addShape(shape); collidables[id].finish(); bodies[id].reset(); bodies[id].setMass(1.0f); bodies[id].setInertia(pfxCalcInertiaBox(PfxVector3(1.0f),1.0f)); states[id].reset(); states[id].setPosition(PfxVector3(0.0f,5.0f,5.0f)); states[id].setMotionType(kPfxMotionTypeActive); states[id].setRigidBodyId(id); } // capsule { int id = numRigidBodies++; PfxCapsule capsule(1.5f,0.5f); PfxShape shape; shape.reset(); shape.setCapsule(capsule); collidables[id].reset(); collidables[id].addShape(shape); collidables[id].finish(); bodies[id].reset(); bodies[id].setMass(2.0f); bodies[id].setInertia(pfxCalcInertiaCylinderX(2.0f,0.5f,2.0f)); states[id].reset(); states[id].setPosition(PfxVector3(5.0f,5.0f,0.0f)); states[id].setMotionType(kPfxMotionTypeActive); states[id].setRigidBodyId(id); } // cylinder { int id = numRigidBodies++; PfxCylinder cylinder(0.5f,1.5f); PfxShape shape; shape.reset(); shape.setCylinder(cylinder); collidables[id].reset(); collidables[id].addShape(shape); collidables[id].finish(); bodies[id].reset(); bodies[id].setMass(3.0f); bodies[id].setInertia(pfxCalcInertiaCylinderX(0.5f,1.5f,3.0f)); states[id].reset(); states[id].setPosition(PfxVector3(0.0f,10.0f,0.0f)); states[id].setMotionType(kPfxMotionTypeActive); states[id].setRigidBodyId(id); } // convex mesh { PfxCreateConvexMeshParam param; param.verts = BarrelVtx; param.numVerts = BarrelVtxCount; param.vertexStrideBytes = sizeof(float)*6; param.triangles = BarrelIdx; param.numTriangles = BarrelIdxCount/3; param.triangleStrideBytes = sizeof(unsigned short)*3; PfxInt32 ret = pfxCreateConvexMesh(gConvex,param); if(ret != SCE_PFX_OK) { SCE_PFX_PRINTF("Can't create gConvex mesh.\n"); } int id = numRigidBodies++; PfxShape shape; shape.reset(); shape.setConvexMesh(&gConvex); collidables[id].reset(); collidables[id].addShape(shape); collidables[id].finish(); bodies[id].reset(); bodies[id].setMass(3.0f); bodies[id].setInertia(pfxCalcInertiaSphere(1.0f,1.0f)); states[id].reset(); states[id].setPosition(PfxVector3(0.0f,15.0f,0.0f)); states[id].setMotionType(kPfxMotionTypeActive); states[id].setRigidBodyId(id); } // combined primitives { int id = numRigidBodies++; //E Both shapes and incides buffer have to be kept when creating a combined shape. static PfxShape shapes[3]; PfxUInt16 shapeIds[3]={0,1,2}; collidables[id].reset(shapes,shapeIds,3); { PfxBox box(0.5f,0.5f,1.5f); PfxShape shape; shape.reset(); shape.setBox(box); shape.setOffsetPosition(PfxVector3(-2.0f,0.0f,0.0f)); collidables[id].addShape(shape); } { PfxBox box(0.5f,1.5f,0.5f); PfxShape shape; shape.reset(); shape.setBox(box); shape.setOffsetPosition(PfxVector3(2.0f,0.0f,0.0f)); collidables[id].addShape(shape); } { PfxCapsule cap(1.5f,0.5f); PfxShape shape; shape.reset(); shape.setCapsule(cap); collidables[id].addShape(shape); } collidables[id].finish(); bodies[id].reset(); bodies[id].setMass(3.0f); bodies[id].setInertia(pfxCalcInertiaBox(PfxVector3(2.5f,1.0f,1.0f),3.0f)); states[id].reset(); states[id].setPosition(PfxVector3(0.0f,5.0f,0.0f)); states[id].setMotionType(kPfxMotionTypeActive); states[id].setRigidBodyId(id); } }
void constraintSolver() { PfxPerfCounter pc; pc.countBegin("setup solver bodies"); { PfxSetupSolverBodiesParam param; param.states = states; param.bodies = bodies; param.solverBodies = solverBodies; param.numRigidBodies = numRigidBodies; int ret = pfxSetupSolverBodies(param); if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxSetupSolverBodies failed %d\n",ret); } pc.countEnd(); pc.countBegin("setup contact constraints"); { PfxSetupContactConstraintsParam param; param.contactPairs = pairs; param.numContactPairs = numPairs; param.offsetContactManifolds = contacts; param.offsetRigidStates = states; param.offsetRigidBodies = bodies; param.offsetSolverBodies = solverBodies; param.numRigidBodies = numRigidBodies; param.timeStep = timeStep; param.separateBias = separateBias; int ret = pfxSetupContactConstraints(param); if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxSetupJointConstraints failed %d\n",ret); } pc.countEnd(); pc.countBegin("setup joint constraints"); { PfxSetupJointConstraintsParam param; param.jointPairs = jointPairs; param.numJointPairs = numJoints; param.offsetJoints = joints; param.offsetRigidStates = states; param.offsetRigidBodies = bodies; param.offsetSolverBodies = solverBodies; param.numRigidBodies = numRigidBodies; param.timeStep = timeStep; for(int i=0;i<numJoints;i++) { pfxUpdateJointPairs(jointPairs[i],i,joints[i],states[joints[i].m_rigidBodyIdA],states[joints[i].m_rigidBodyIdB]); } int ret = pfxSetupJointConstraints(param); if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxSetupJointConstraints failed %d\n",ret); } pc.countEnd(); pc.countBegin("solve constraints"); { PfxSolveConstraintsParam param; param.workBytes = pfxGetWorkBytesOfSolveConstraints(numRigidBodies,numPairs,numJoints); param.workBuff = pool.allocate(param.workBytes); param.contactPairs = pairs; param.numContactPairs = numPairs; param.offsetContactManifolds = contacts; param.jointPairs = jointPairs; param.numJointPairs = numJoints; param.offsetJoints = joints; param.offsetRigidStates = states; param.offsetSolverBodies = solverBodies; param.numRigidBodies = numRigidBodies; param.iteration = iteration; int ret = pfxSolveConstraints(param); if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxSolveConstraints failed %d\n",ret); pool.deallocate(param.workBuff); } pc.countEnd(); //pc.printCount(); }
PfxInt32 pfxCreateLargeTriMesh(PfxLargeTriMesh &lmesh,const PfxCreateLargeTriMeshParam ¶m) { // Check input if(param.numVerts == 0 || param.numTriangles == 0 || !param.verts || !param.triangles) return SCE_PFX_ERR_INVALID_VALUE; if(param.islandsRatio < 0.0f || param.islandsRatio > 1.0f) return SCE_PFX_ERR_OUT_OF_RANGE; if(param.numFacetsLimit == 0 || param.numFacetsLimit > SCE_PFX_NUMMESHFACETS) return SCE_PFX_ERR_OUT_OF_RANGE; const PfxFloat epsilon = 0.00001f; PfxArray<PfxMcVert> vertList(param.numVerts); // 頂点配列 PfxArray<PfxMcFacet> facetList(param.numTriangles); // 面配列 PfxArray<PfxMcEdge> edgeList(param.numTriangles*3); // エッジ配列 PfxArray<PfxMcEdge*> edgeHead(param.numTriangles*3); //J 頂点配列作成 for(PfxUInt32 i=0;i<param.numVerts;i++) { PfxFloat *vtx = (PfxFloat*)((uintptr_t)param.verts + param.vertexStrideBytes * i); PfxMcVert mcv; mcv.flag = 0; mcv.i = i; mcv.coord = pfxReadVector3(vtx); vertList.push(mcv); } // 面配列作成 for(PfxUInt32 i=0;i<param.numTriangles;i++) { void *ids = (void*)((uintptr_t)param.triangles + param.triangleStrideBytes * i); PfxUInt32 idx[3]; if(param.flag & SCE_PFX_MESH_FLAG_32BIT_INDEX) { if(param.flag & SCE_PFX_MESH_FLAG_NORMAL_FLIP) { idx[0] = ((PfxUInt32*)ids)[2]; idx[1] = ((PfxUInt32*)ids)[1]; idx[2] = ((PfxUInt32*)ids)[0]; } else { idx[0] = ((PfxUInt32*)ids)[0]; idx[1] = ((PfxUInt32*)ids)[1]; idx[2] = ((PfxUInt32*)ids)[2]; } } else if(param.flag & SCE_PFX_MESH_FLAG_16BIT_INDEX) { if(param.flag & SCE_PFX_MESH_FLAG_NORMAL_FLIP) { idx[0] = ((PfxUInt16*)ids)[2]; idx[1] = ((PfxUInt16*)ids)[1]; idx[2] = ((PfxUInt16*)ids)[0]; } else { idx[0] = ((PfxUInt16*)ids)[0]; idx[1] = ((PfxUInt16*)ids)[1]; idx[2] = ((PfxUInt16*)ids)[2]; } } else { return SCE_PFX_ERR_INVALID_FLAG; } const PfxVector3 pnts[3] = { vertList[idx[0]].coord, vertList[idx[1]].coord, vertList[idx[2]].coord, }; // 面積が0の面を排除 PfxFloat area = lengthSqr(cross(pnts[1]-pnts[0],pnts[2]-pnts[0])); if((param.flag & SCE_PFX_MESH_FLAG_AUTO_ELIMINATION) && area < 0.00001f) { continue; } PfxMcFacet facet; facet.v[0] = &vertList[idx[0]]; facet.v[1] = &vertList[idx[1]]; facet.v[2] = &vertList[idx[2]]; facet.e[0] = facet.e[1] = facet.e[2] = NULL; facet.n = normalize(cross(pnts[2]-pnts[1],pnts[0]-pnts[1])); facet.area = area; facet.thickness = param.defaultThickness; facet.neighbor[0] = facet.neighbor[1] = facet.neighbor[2] = -1; facet.neighborEdgeId[0] = facet.neighborEdgeId[1] = facet.neighborEdgeId[2] = -1; facetList.push(facet); } const PfxUInt32 numTriangles = facetList.size(); { PfxArray<PfxMcTriList> triEntry(numTriangles*3); PfxArray<PfxMcTriList*> triHead(numTriangles*3); // 頂点から面への参照リスト PfxInt32 cnt = 0; PfxMcTriList* nl = NULL; triEntry.assign(numTriangles*3,PfxMcTriList()); triHead.assign(numTriangles*3,nl); // 頂点から面への参照リストを作成 for(PfxUInt32 i=0;i<numTriangles;i++) { for(PfxUInt32 v=0;v<3;v++) { PfxUInt32 vertId = facetList[i].v[v]->i; triEntry[cnt].facet = &facetList[i]; triEntry[cnt].next = triHead[vertId]; triHead[vertId] = &triEntry[cnt++]; } } // 同一頂点をまとめる if(param.flag & SCE_PFX_MESH_FLAG_AUTO_ELIMINATION) { for(PfxUInt32 i=0;i<param.numVerts;i++) { if(vertList[i].flag == 1) continue; for(PfxUInt32 j=i+1;j<param.numVerts;j++) { if(vertList[j].flag == 1) continue; PfxFloat lenSqr = lengthSqr(vertList[i].coord-vertList[j].coord); if(lenSqr < epsilon) { //SCE_PFX_PRINTF("same position %d,%d\n",i,j); vertList[j].flag = 1; // 同一点なのでフラグを立てる for(PfxMcTriList *f=triHead[j];f!=NULL;f=f->next) { for(PfxInt32 k=0;k<3;k++) { if(f->facet->v[k] == &vertList[j]) { f->facet->v[k] = &vertList[i]; // 頂点を付け替える break; } } } } } } } } // 接続面間の角度を算出して面にセット PfxMcEdge *nl = NULL; edgeHead.assign(numTriangles*3,nl); edgeList.assign(numTriangles*3,PfxMcEdge()); // エッジ配列の作成 PfxUInt32 ecnt = 0; for(PfxUInt32 i=0;i<numTriangles;i++) { PfxMcFacet &f = facetList[i]; for(PfxUInt32 v=0;v<3;v++) { uintptr_t vp1 = ((uintptr_t)f.v[v]-(uintptr_t)&vertList[0])/sizeof(PfxMcVert); uintptr_t vp2 = ((uintptr_t)f.v[(v+1)%3]-(uintptr_t)&vertList[0])/sizeof(PfxMcVert); PfxUInt32 viMin = SCE_PFX_MIN(vp1,vp2); PfxUInt32 viMax = SCE_PFX_MAX(vp1,vp2); PfxInt32 key = ((0x8da6b343*viMin+0xd8163841*viMax)%(numTriangles*3)); for(PfxMcEdge *e = edgeHead[key];;e=e->next) { if(!e) { edgeList[ecnt].vertId[0] = viMin; edgeList[ecnt].vertId[1] = viMax; edgeList[ecnt].facetId[0] = i; edgeList[ecnt].edgeId[0] = v; edgeList[ecnt].numFacets = 1; edgeList[ecnt].next = edgeHead[key]; edgeList[ecnt].angleType = SCE_PFX_EDGE_CONVEX; edgeList[ecnt].angle = 0.0f; edgeHead[key] = &edgeList[ecnt]; f.e[v] = &edgeList[ecnt]; ecnt++; break; } if(e->vertId[0] == viMin && e->vertId[1] == viMax) { SCE_PFX_ALWAYS_ASSERT_MSG(e->numFacets == 1,"An edge connected with over 2 triangles is invalid"); e->facetId[1] = i; e->edgeId[1] = v; e->numFacets = 2; f.e[v] = e; f.neighbor[v] = e->facetId[0]; f.neighborEdgeId[v] = e->edgeId[0]; facetList[e->facetId[0]].neighbor[e->edgeId[0]] = i; facetList[e->facetId[0]].neighborEdgeId[e->edgeId[0]] = e->edgeId[1]; break; } } } } // 角度を計算 for(PfxUInt32 i=0;i<numTriangles;i++) { PfxMcFacet &facetA = facetList[i]; PfxQueue<PfxMcFacetLink> cqueue(ecnt); for(PfxUInt32 j=0;j<3;j++) { if(facetA.neighbor[j] >= 0) { cqueue.push(PfxMcFacetLink( j, facetA.e[j]->vertId[0],facetA.e[j]->vertId[1], i,j, facetA.neighbor[j],facetA.neighborEdgeId[j])); } } while(!cqueue.empty()) { PfxMcFacetLink link = cqueue.front(); cqueue.pop(); PfxMcFacet &ofacet = facetList[link.ofacetId]; PfxMcEdge *edge = ofacet.e[link.oedgeId]; // facetAとのなす角を計算 { // 面に含まれるが、このエッジに含まれない点 PfxUInt32 ids[3] = {2,0,1}; PfxVector3 v1 = facetA.v[ids[link.baseEdgeId]]->coord; PfxVector3 v2 = ofacet.v[ids[link.oedgeId]]->coord; // エッジの凹凸判定 PfxVector3 midPnt = (v1 + v2) * 0.5f; PfxVector3 pntOnEdge = facetA.v[link.baseEdgeId]->coord; PfxFloat chk1 = dot(facetA.n,midPnt-pntOnEdge); PfxFloat chk2 = dot(ofacet.n,midPnt-pntOnEdge); if(chk1 < -epsilon && chk2 < -epsilon) { if(link.ifacetId == i) edge->angleType = SCE_PFX_EDGE_CONVEX; // 厚み角の判定に使う角度をセット if(param.flag & SCE_PFX_MESH_FLAG_AUTO_THICKNESS) { edge->angle = 0.5f*acosf(dot(facetA.n,ofacet.n)); } } else if(chk1 > epsilon && chk2 > epsilon) { if(link.ifacetId == i) edge->angleType = SCE_PFX_EDGE_CONCAVE; } else { if(link.ifacetId == i) edge->angleType = SCE_PFX_EDGE_FLAT; } } // 次の接続面を登録(コメントアウトすると頂点で接続された面を考慮しない) if(param.flag & SCE_PFX_MESH_FLAG_AUTO_THICKNESS) { PfxInt32 nextEdgeId = (link.oedgeId+1)%3; PfxMcEdge *nextEdge = ofacet.e[nextEdgeId]; if(ofacet.neighbor[nextEdgeId] >= 0 && ofacet.neighbor[nextEdgeId] != i && ((PfxInt32)nextEdge->vertId[0] == link.vid1 || (PfxInt32)nextEdge->vertId[0] == link.vid2 || (PfxInt32)nextEdge->vertId[1] == link.vid1 || (PfxInt32)nextEdge->vertId[1] == link.vid2) ) { cqueue.push(PfxMcFacetLink( link.baseEdgeId, link.vid1,link.vid2, link.ofacetId,link.iedgeId, ofacet.neighbor[nextEdgeId],ofacet.neighborEdgeId[nextEdgeId])); } nextEdgeId = (link.oedgeId+2)%3; nextEdge = ofacet.e[nextEdgeId]; if(ofacet.neighbor[nextEdgeId] >= 0 && ofacet.neighbor[nextEdgeId] != i && ((PfxInt32)nextEdge->vertId[0] == link.vid1 || (PfxInt32)nextEdge->vertId[0] == link.vid2 || (PfxInt32)nextEdge->vertId[1] == link.vid1 || (PfxInt32)nextEdge->vertId[1] == link.vid2) ) { cqueue.push(PfxMcFacetLink( link.baseEdgeId, link.vid1,link.vid2, link.ofacetId,link.iedgeId, ofacet.neighbor[nextEdgeId],ofacet.neighborEdgeId[nextEdgeId])); } } } } // 面に厚みを付ける if(param.flag & SCE_PFX_MESH_FLAG_AUTO_THICKNESS) { for(PfxUInt32 i=0;i<numTriangles;i++) { PfxMcFacet &facetA = facetList[i]; for(PfxUInt32 j=0;j<numTriangles;j++) { // 隣接面は比較対象にしない if( i==j || j == (PfxInt32)facetA.e[0]->facetId[0] || j == (PfxInt32)facetA.e[0]->facetId[1] || j == (PfxInt32)facetA.e[1]->facetId[0] || j == (PfxInt32)facetA.e[1]->facetId[1] || j == (PfxInt32)facetA.e[2]->facetId[0] || j == (PfxInt32)facetA.e[2]->facetId[1]) { continue; } PfxMcFacet &facetB = facetList[j]; // 交差判定 PfxFloat closestDistance=0; if(intersect(facetA,facetB,closestDistance)) { // 最近接距離/2を厚みとして採用 facetA.thickness = SCE_PFX_MAX(param.defaultThickness,SCE_PFX_MIN(facetA.thickness,closestDistance * 0.5f)); } } } } // 面の面積によって3種類に分類する PfxFloat areaMin=SCE_PFX_FLT_MAX,areaMax=-SCE_PFX_FLT_MAX; for(PfxUInt32 f=0;f<(PfxUInt32)numTriangles;f++) { PfxVector3 pnts[3] = { facetList[f].v[0]->coord, facetList[f].v[1]->coord, facetList[f].v[2]->coord, }; areaMin = SCE_PFX_MIN(areaMin,facetList[f].area); areaMax = SCE_PFX_MAX(areaMax,facetList[f].area); // 面のAABBを算出 facetList[f].aabbMin = minPerElem(pnts[2],minPerElem(pnts[1],pnts[0])); facetList[f].aabbMax = maxPerElem(pnts[2],maxPerElem(pnts[1],pnts[0])); } PfxFloat areaDiff = (areaMax-areaMin)/3.0f; PfxFloat areaLevel0,areaLevel1; areaLevel0 = areaMin + areaDiff; areaLevel1 = areaMin + areaDiff * 2.0f; PfxArray<PfxMcFacetPtr> facetsLv0(numTriangles); PfxArray<PfxMcFacetPtr> facetsLv1(numTriangles); PfxArray<PfxMcFacetPtr> facetsLv2(numTriangles); for(PfxUInt32 f=0;f<numTriangles;f++) { PfxFloat area = facetList[f].area; PfxMcFacet *fct = &facetList[f]; if(area < areaLevel0) { facetsLv0.push(fct); } else if(area > areaLevel1) { facetsLv2.push(fct); } else { facetsLv1.push(fct); } } // アイランドの配列 PfxMcIslands islands; PfxVector3 lmeshSize; // レベル毎にPfxTriMeshを作成 if(!facetsLv0.empty()) { // 全体のAABBを求める PfxVector3 aabbMin,aabbMax,center,half; aabbMin =facetsLv0[0]->aabbMin; aabbMax = facetsLv0[0]->aabbMax; for(PfxUInt32 f=1;f<facetsLv0.size();f++) { aabbMin = minPerElem(facetsLv0[f]->aabbMin,aabbMin); aabbMax = maxPerElem(facetsLv0[f]->aabbMax,aabbMax); } center = ( aabbMin + aabbMax ) * 0.5f; half = ( aabbMax - aabbMin ) * 0.5f; // 再帰的に処理 divideMeshes( param.numFacetsLimit,param.islandsRatio, islands, facetsLv0, center,half); lmeshSize = maxPerElem(lmeshSize,maxPerElem(absPerElem(aabbMin),absPerElem(aabbMax))); } if(!facetsLv1.empty()) { // 全体のAABBを求める PfxVector3 aabbMin,aabbMax,center,half; aabbMin =facetsLv1[0]->aabbMin; aabbMax = facetsLv1[0]->aabbMax; for(PfxUInt32 f=1;f<facetsLv1.size();f++) { aabbMin = minPerElem(facetsLv1[f]->aabbMin,aabbMin); aabbMax = maxPerElem(facetsLv1[f]->aabbMax,aabbMax); } center = ( aabbMin + aabbMax ) * 0.5f; half = ( aabbMax - aabbMin ) * 0.5f; // 再帰的に処理 divideMeshes( param.numFacetsLimit,param.islandsRatio, islands, facetsLv1, center,half); lmeshSize = maxPerElem(lmeshSize,maxPerElem(absPerElem(aabbMin),absPerElem(aabbMax))); } if(!facetsLv2.empty()) { // 全体のAABBを求める PfxVector3 aabbMin,aabbMax,center,half; aabbMin =facetsLv2[0]->aabbMin; aabbMax = facetsLv2[0]->aabbMax; for(PfxUInt32 f=1;f<facetsLv2.size();f++) { aabbMin = minPerElem(facetsLv2[f]->aabbMin,aabbMin); aabbMax = maxPerElem(facetsLv2[f]->aabbMax,aabbMax); } center = ( aabbMin + aabbMax ) * 0.5f; half = ( aabbMax - aabbMin ) * 0.5f; // 再帰的に処理 divideMeshes( param.numFacetsLimit,param.islandsRatio, islands, facetsLv2, center,half); lmeshSize = maxPerElem(lmeshSize,maxPerElem(absPerElem(aabbMin),absPerElem(aabbMax))); } lmesh.m_half = lmeshSize; // Check Islands //for(PfxInt32 i=0;i<islands.numIslands;i++) { // SCE_PFX_PRINTF("island %d\n",i); // for(PfxInt32 f=0;f<islands.facetsInIsland[i].size();f++) { // PfxMcFacet *facet = islands.facetsInIsland[i][f]; // SCE_PFX_PRINTF(" %d %d %d\n",facet->v[0]->i,facet->v[1]->i,facet->v[2]->i); // } //} // PfxLargeTriMeshの生成 if(islands.numIslands > 0) { lmesh.m_numIslands = 0; lmesh.m_aabbList = (PfxAabb16*)SCE_PFX_UTIL_ALLOC(128,sizeof(PfxAabb16)*islands.numIslands); lmesh.m_islands = (PfxTriMesh*)SCE_PFX_UTIL_ALLOC(128,sizeof(PfxTriMesh)*islands.numIslands); PfxInt32 maxFacets=0,maxVerts=0,maxEdges=0; for(PfxUInt32 i=0;i<islands.numIslands;i++) { PfxTriMesh island; createIsland(island,islands.facetsInIsland[i]); addIslandToLargeTriMesh(lmesh,island); maxFacets = SCE_PFX_MAX(maxFacets,island.m_numFacets); maxVerts = SCE_PFX_MAX(maxVerts,island.m_numVerts); maxEdges = SCE_PFX_MAX(maxEdges,island.m_numEdges); //SCE_PFX_PRINTF("island %d verts %d edges %d facets %d\n",i,island.m_numVerts,island.m_numEdges,island.m_numFacets); } SCE_PFX_PRINTF("generate completed!\n\tinput mesh verts %d triangles %d\n\tislands %d max triangles %d verts %d edges %d\n", param.numVerts,param.numTriangles, lmesh.m_numIslands,maxFacets,maxVerts,maxEdges); SCE_PFX_PRINTF("\tsizeof(PfxLargeTriMesh) %d sizeof(PfxTriMesh) %d\n",sizeof(PfxLargeTriMesh),sizeof(PfxTriMesh)); } else { SCE_PFX_PRINTF("islands overflow! %d/%d\n",islands.numIslands,SCE_PFX_LARGETRIMESH_MAX_ISLANDS); return SCE_PFX_ERR_OUT_OF_RANGE; } return SCE_PFX_OK; }
void physics_simulate() { BT_PROFILE("physics_simulate"); PfxPerfCounter pc; for(int i=1;i<numRigidBodies;i++) { pfxApplyExternalForce(states[i],bodies[i],bodies[i].getMass()*PfxVector3(0.0f,-9.8f,0.0f),PfxVector3(0.0f),timeStep); } // perf_push_marker("broadphase"); // pc.countBegin("broadphase"); { BT_PROFILE("broadphase"); broadphase(); } // //pc.countEnd(); // perf_pop_marker(); // perf_push_marker("collision"); // pc.countBegin("collision"); { BT_PROFILE("collision"); collision(); } // pc.countEnd(); // perf_pop_marker(); // perf_push_marker("solver"); // pc.countBegin("solver"); { BT_PROFILE("constraintSolver"); constraintSolver(); if (!peSolverEnabled) BulletConstraintSolver(); } // pc.countEnd(); // perf_pop_marker(); // perf_push_marker("integrate"); // pc.countBegin("integrate"); { BT_PROFILE("integrate"); integrate(); } // pc.countEnd(); // perf_pop_marker(); frame++; if (0)////if(frame%100 == 0) { float broadphaseTime = pc.getCountTime(0); float collisionTime = pc.getCountTime(2); float solverTime = pc.getCountTime(4); float integrateTime = pc.getCountTime(6); SCE_PFX_PRINTF("#pairs = %d, #contacts = %d\n", curNumPairs, curTotalContacts); SCE_PFX_PRINTF("frame %3d broadphase %.2f collision %.2f solver %.2f integrate %.2f | total %.2f\n",frame, broadphaseTime,collisionTime,solverTime,integrateTime, broadphaseTime+collisionTime+solverTime+integrateTime); } }
void broadphase() { pairSwap = 1-pairSwap; unsigned int &numPreviousPairs = numPairs[1-pairSwap]; unsigned int &numCurrentPairs = numPairs[pairSwap]; PfxBroadphasePair *previousPairs = pairsBuff[1-pairSwap]; PfxBroadphasePair *currentPairs = pairsBuff[pairSwap]; //J 剛体が最も分散している軸を見つける //E Find the axis along which all rigid bodies are most widely positioned int axis = 0; { PfxVector3 s(0.0f),s2(0.0f); for(int i=0;i<numRigidBodies;i++) { PfxVector3 c = states[i].getPosition(); s += c; s2 += mulPerElem(c,c); } PfxVector3 v = s2 - mulPerElem(s,s) / (float)numRigidBodies; if(v[1] > v[0]) axis = 1; if(v[2] > v[axis]) axis = 2; } //J ブロードフェーズプロキシの更新 //E Create broadpahse proxies { for(int i=0;i<numRigidBodies;i++) { pfxUpdateBroadphaseProxy(proxies[i],states[i],collidables[i],worldCenter,worldExtent,axis); } int workBytes = sizeof(PfxBroadphaseProxy) * numRigidBodies; void *workBuff = pool.allocate(workBytes); pfxParallelSort(proxies,numRigidBodies,workBuff,workBytes); pool.deallocate(workBuff); } //J 交差ペア探索 //E Find overlapped pairs { PfxFindPairsParam findPairsParam; findPairsParam.pairBytes = pfxGetPairBytesOfFindPairs(NUM_CONTACTS); findPairsParam.pairBuff = pool.allocate(findPairsParam.pairBytes); findPairsParam.workBytes = pfxGetWorkBytesOfFindPairs(NUM_CONTACTS); findPairsParam.workBuff = pool.allocate(findPairsParam.workBytes); findPairsParam.proxies = proxies; findPairsParam.numProxies = numRigidBodies; findPairsParam.maxPairs = NUM_CONTACTS; findPairsParam.axis = axis; PfxFindPairsResult findPairsResult; int ret = pfxFindPairs(findPairsParam,findPairsResult); if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxFindPairs failed %d\n",ret); pool.deallocate(findPairsParam.workBuff); curNumPairs = findPairsResult.numPairs; //J 交差ペア合成 //E Decompose overlapped pairs into 3 arrays PfxDecomposePairsParam decomposePairsParam; decomposePairsParam.pairBytes = pfxGetPairBytesOfDecomposePairs(numPreviousPairs,findPairsResult.numPairs); decomposePairsParam.pairBuff = pool.allocate(decomposePairsParam.pairBytes); decomposePairsParam.workBytes = pfxGetWorkBytesOfDecomposePairs(numPreviousPairs,findPairsResult.numPairs); decomposePairsParam.workBuff = pool.allocate(decomposePairsParam.workBytes); decomposePairsParam.previousPairs = previousPairs; decomposePairsParam.numPreviousPairs = numPreviousPairs; decomposePairsParam.currentPairs = findPairsResult.pairs; // Set pairs from pfxFindPairs() decomposePairsParam.numCurrentPairs = findPairsResult.numPairs; // Set the number of pairs from pfxFindPairs() PfxDecomposePairsResult decomposePairsResult; ret = pfxDecomposePairs(decomposePairsParam,decomposePairsResult); if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxDecomposePairs failed %d\n",ret); pool.deallocate(decomposePairsParam.workBuff); PfxBroadphasePair *outNewPairs = decomposePairsResult.outNewPairs; PfxBroadphasePair *outKeepPairs = decomposePairsResult.outKeepPairs; PfxBroadphasePair *outRemovePairs = decomposePairsResult.outRemovePairs; PfxUInt32 numOutNewPairs = decomposePairsResult.numOutNewPairs; PfxUInt32 numOutKeepPairs = decomposePairsResult.numOutKeepPairs; PfxUInt32 numOutRemovePairs = decomposePairsResult.numOutRemovePairs; //J 廃棄ペアのコンタクトをプールに戻す //E Put removed contacts into the contact pool for(PfxUInt32 i=0;i<numOutRemovePairs;i++) { contactIdPool[numContactIdPool++] = pfxGetContactId(outRemovePairs[i]); } //J 新規ペアのコンタクトのリンクと初期化 //E Add new contacts and initialize for(PfxUInt32 i=0;i<numOutNewPairs;i++) { int cId = 0; if(numContactIdPool > 0) { cId = contactIdPool[--numContactIdPool]; } else { cId = numContacts++; } if(cId >= NUM_CONTACTS) { cId = 0; } SCE_PFX_ASSERT(cId < NUM_CONTACTS); pfxSetContactId(outNewPairs[i],cId); PfxContactManifold &contact = contacts[cId]; contact.reset(pfxGetObjectIdA(outNewPairs[i]),pfxGetObjectIdB(outNewPairs[i])); } //J 新規ペアと維持ペアを合成 //E Merge 'new' and 'keep' pairs numCurrentPairs = 0; for(PfxUInt32 i=0;i<numOutKeepPairs;i++) { currentPairs[numCurrentPairs++] = outKeepPairs[i]; } for(PfxUInt32 i=0;i<numOutNewPairs;i++) { currentPairs[numCurrentPairs++] = outNewPairs[i]; } bool verboseStats = true; if (verboseStats) { printf("===============================================\n"); printf("num bodies/states = %d\n", physics_get_num_rigidbodies()); for (int i=0;i<physics_get_num_rigidbodies();i++) { PfxVector3 pos = physics_get_state(i).getPosition(); printf("body %d has position %f,%f,%f\n",i,pos.getX(),pos.getY(),pos.getZ()); } printf("numCurrentPairs (total) = %d\n", numCurrentPairs); for (int i=0;i<numCurrentPairs;i++) { int idA = pfxGetObjectIdA(currentPairs[i]); int idB = pfxGetObjectIdB(currentPairs[i]); printf("pfx pair[%d] idA = %d, idB = %d\n", i, idA,idB); int cId = pfxGetContactId(currentPairs[i]); printf("contact duration = %d\n", contacts[cId].getDuration()); if (1) { printf("num contacts = %d\n", contacts[cId].getNumContacts()); for (int c=0;c<contacts[cId].getNumContacts();c++) { const PfxContactPoint& cp = contacts[cId].getContactPoint(c); printf("localPosA = %f,%f,%f. ", cp.m_localPointA[0],cp.m_localPointA[1],cp.m_localPointA[2]); printf("localPosB = %f,%f,%f. ", cp.m_localPointB[0],cp.m_localPointB[1],cp.m_localPointB[2]); for (int r=0;r<3;r++) { printf("row %d accumImpulse = %f. ", r, cp.m_constraintRow[r].m_accumImpulse); printf("row %d normal = %f,%f,%f. ", r, cp.m_constraintRow[r].m_normal[0],cp.m_constraintRow[r].m_normal[1],cp.m_constraintRow[r].m_normal[2]); printf("row %d distance %f and duration %d\n", r, cp.m_distance1,cp.m_duration); } } } } } //printf("numOutRemovePairs = %d\n", numOutRemovePairs); //printf("numOutNewPairs = %d\n",numOutNewPairs); pool.deallocate(decomposePairsParam.pairBuff); pool.deallocate(findPairsParam.pairBuff); } { int workBytes = sizeof(PfxBroadphasePair) * numCurrentPairs; void *workBuff = pool.allocate(workBytes); pfxParallelSort(currentPairs,numCurrentPairs,workBuff,workBytes); pool.deallocate(workBuff); } }
void physics_create_scene(int sceneId) { const int numScenes = 4; int sid = sceneId % numScenes; numRigidBodies= 0; pairSwap = 0; numPairs[0] = 0; numPairs[1] = 0; numContacts = 0; numContactIdPool = 0; numJoints = 0; island = NULL; frame = 0; doAreaRaycast = false; switch(sid) { case 0: // simple primitives createSceneBoxGround(); createScenePrimitives(); initRays(PfxVector3(15.0f,5.0f,0.0f),PfxVector3(3.0f,1.0f,0.0f)); break; case 1: // landscape createSceneLandscape(); createScenePrimitives(); initRays(PfxVector3(15.0f,2.5f,0.0f),PfxVector3(3.0f,-2.5f,0.0f)); break; case 2: // raycast filtering createSceneRayFilter(); break; case 3: // area raycast createSceneBoxGround(); createSceneFalling(); initRays(PfxVector3(15.0f,5.0f,0.0f),PfxVector3(14.0f,-1.0f,0.0f)); doAreaRaycast = true; break; } SCE_PFX_PRINTF("----- Size of rigid body buffer ------\n"); SCE_PFX_PRINTF(" size * num = total\n"); SCE_PFX_PRINTF("PfxRigidState %5d * %5d = %5d bytes\n",sizeof(PfxRigidState),numRigidBodies,sizeof(PfxRigidState)*numRigidBodies); SCE_PFX_PRINTF("PfxRigidBody %5d * %5d = %5d bytes\n",sizeof(PfxRigidBody),numRigidBodies,sizeof(PfxRigidBody)*numRigidBodies); SCE_PFX_PRINTF("PfxCollidable %5d * %5d = %5d bytes\n",sizeof(PfxCollidable),numRigidBodies,sizeof(PfxCollidable)*numRigidBodies); SCE_PFX_PRINTF("PfxJoint %5d * %5d = %5d bytes\n",sizeof(PfxJoint),numJoints,sizeof(PfxJoint)*numJoints); SCE_PFX_PRINTF("PfxSolverBody %5d * %5d = %5d bytes\n",sizeof(PfxSolverBody),numRigidBodies,sizeof(PfxSolverBody)*numRigidBodies); SCE_PFX_PRINTF("PfxBroadphaseProxy %5d * %5d = %5d bytes\n",sizeof(PfxBroadphaseProxy),numRigidBodies,sizeof(PfxBroadphaseProxy)*numRigidBodies); SCE_PFX_PRINTF("PfxContactManifold %5d * %5d = %5d bytes\n",sizeof(PfxContactManifold),NUM_CONTACTS,sizeof(PfxContactManifold)*NUM_CONTACTS); SCE_PFX_PRINTF("PfxBroadphasePair %5d * %5d = %5d bytes\n",sizeof(PfxBroadphasePair),NUM_CONTACTS,sizeof(PfxBroadphasePair)*NUM_CONTACTS); int totalBytes = (sizeof(PfxRigidState) + sizeof(PfxRigidBody) + sizeof(PfxCollidable) + sizeof(PfxSolverBody) + sizeof(PfxBroadphaseProxy)) * numRigidBodies + (sizeof(PfxContactManifold) + sizeof(PfxBroadphasePair)) * NUM_CONTACTS; SCE_PFX_PRINTF("----------------------------------------------------------\n"); SCE_PFX_PRINTF("Total %5d bytes\n",totalBytes); }
static void convertShape(btCollisionShape* bulletShape, btAlignedObjectArray<sce::PhysicsEffects::PfxShape>& shapes) { switch (bulletShape->getShapeType()) { case BOX_SHAPE_PROXYTYPE: { btBoxShape* boxshape = (btBoxShape*)bulletShape; sce::PhysicsEffects::PfxBox box(boxshape->getHalfExtentsWithMargin().getX(),boxshape->getHalfExtentsWithMargin().getY(),boxshape->getHalfExtentsWithMargin().getZ()); sce::PhysicsEffects::PfxShape& shape = shapes.expand(); shape.reset(); shape.setBox(box); break; } case TRIANGLE_MESH_SHAPE_PROXYTYPE: { btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*) bulletShape; int numSubParts = trimesh->getMeshInterface()->getNumSubParts(); btAssert(numSubParts>0); for (int i=0;i<numSubParts;i++) { unsigned char* vertexBase=0; int numVerts = 0; PHY_ScalarType vertexType; int vertexStride=0; unsigned char* indexBase=0; int indexStride=0; int numFaces=0; PHY_ScalarType indexType; trimesh->getMeshInterface()->getLockedVertexIndexBase(&vertexBase,numVerts,vertexType,vertexStride,&indexBase,indexStride,numFaces,indexType,i); sce::PhysicsEffects::PfxCreateLargeTriMeshParam param; btAssert(param.flag&SCE_PFX_MESH_FLAG_16BIT_INDEX); unsigned short int* copyIndices = new unsigned short int[numFaces*3]; switch (indexType) { case PHY_UCHAR: { for (int p=0;p<numFaces;p++) { copyIndices[p*3]=indexBase[p*indexStride]; copyIndices[p*3+1]=indexBase[p*indexStride+1]; copyIndices[p*3+2]=indexBase[p*indexStride+2]; } break; } //PHY_INTEGER: //PHY_SHORT: default: { btAssert(0); } }; param.verts = (float*)vertexBase; param.numVerts = numVerts; param.vertexStrideBytes = vertexStride; param.triangles = copyIndices; param.numTriangles = numFaces; param.triangleStrideBytes = sizeof(unsigned short int)*3; sce::PhysicsEffects::PfxLargeTriMesh* largeMesh = new sce::PhysicsEffects::PfxLargeTriMesh(); sce::PhysicsEffects::PfxInt32 ret = pfxCreateLargeTriMesh(*largeMesh,param); if(ret != SCE_PFX_OK) { SCE_PFX_PRINTF("Can't create large mesh.\n"); } sce::PhysicsEffects::PfxShape& shape = shapes.expand(); shape.reset(); shape.setLargeTriMesh(largeMesh); } break; } case SPHERE_SHAPE_PROXYTYPE: { btSphereShape* sphereshape = (btSphereShape*)bulletShape; sce::PhysicsEffects::PfxSphere sphere(sphereshape->getRadius()); sce::PhysicsEffects::PfxShape& shape = shapes.expand(); shape.reset(); shape.setSphere(sphere); break; } case CAPSULE_SHAPE_PROXYTYPE: { btCapsuleShape* capsuleshape= (btCapsuleShape*)bulletShape;//assume btCapsuleShapeX for now sce::PhysicsEffects::PfxCapsule capsule(capsuleshape->getHalfHeight(),capsuleshape->getRadius()); sce::PhysicsEffects::PfxShape& shape = shapes.expand(); shape.reset(); shape.setCapsule(capsule); break; } case CYLINDER_SHAPE_PROXYTYPE: { btCylinderShape* cylindershape= (btCylinderShape*)bulletShape;//assume btCylinderShapeX for now sce::PhysicsEffects::PfxCylinder cylinder(cylindershape->getHalfExtentsWithMargin()[0],cylindershape->getRadius()); sce::PhysicsEffects::PfxShape& shape = shapes.expand(); shape.reset(); shape.setCylinder(cylinder); break; } case CONVEX_HULL_SHAPE_PROXYTYPE: { btConvexHullShape* convexHullShape = (btConvexHullShape*)bulletShape; sce::PhysicsEffects::PfxConvexMesh* convex = new sce::PhysicsEffects::PfxConvexMesh(); convex->m_numVerts = convexHullShape->getNumPoints(); convex->m_numIndices = 0;//todo for ray intersection test support for (int i=0;i<convex->m_numVerts;i++) { convex->m_verts[i].setX(convexHullShape->getPoints()[i].getX()); convex->m_verts[i].setY(convexHullShape->getPoints()[i].getY()); convex->m_verts[i].setZ(convexHullShape->getPoints()[i].getZ()); } convex->updateAABB(); sce::PhysicsEffects::PfxShape& shape = shapes.expand(); shape.reset(); shape.setConvexMesh(convex); break; } case COMPOUND_SHAPE_PROXYTYPE: { btCompoundShape* compound = (btCompoundShape*) bulletShape; for (int s=0;s<compound->getNumChildShapes();s++) { convertShape(compound->getChildShape(s),shapes); sce::PhysicsEffects::PfxMatrix3 rotMat = getVmMatrix3(compound->getChildTransform(s).getBasis()); sce::PhysicsEffects::PfxVector3 translate = getVmVector3(compound->getChildTransform(s).getOrigin()); sce::PhysicsEffects::PfxTransform3 childtransform(rotMat,translate); shapes[shapes.size()-1].setOffsetTransform(childtransform); } break; } default: { btAssert(0); } }; }
void broadphase() { pairSwap = 1-pairSwap; unsigned int &numPreviousPairs = numPairs[1-pairSwap]; unsigned int &numCurrentPairs = numPairs[pairSwap]; PfxBroadphasePair *previousPairs = pairsBuff[1-pairSwap]; PfxBroadphasePair *currentPairs = pairsBuff[pairSwap]; //J 剛体が最も分散している軸を見つける //E Find the axis along which all rigid bodies are most widely positioned int axis = 0; { PfxVector3 s(0.0f),s2(0.0f); for(int i=0;i<numRigidBodies;i++) { PfxVector3 c = states[i].getPosition(); s += c; s2 += mulPerElem(c,c); } PfxVector3 v = s2 - mulPerElem(s,s) / (float)numRigidBodies; if(v[1] > v[0]) axis = 1; if(v[2] > v[axis]) axis = 2; } //J ブロードフェーズプロキシの更新 //E Create broadpahse proxies { //J レイキャストと共用するため、全ての軸に対するプロキシ配列を作成する //E To share with ray casting, create proxy arrays for all axis PfxUpdateBroadphaseProxiesParam param; param.workBytes = pfxGetWorkBytesOfUpdateBroadphaseProxies(numRigidBodies); param.workBuff = pool.allocate(param.workBytes,128); param.numRigidBodies = numRigidBodies; param.offsetRigidStates = states; param.offsetCollidables = collidables; param.proxiesX = proxies[0]; param.proxiesY = proxies[1]; param.proxiesZ = proxies[2]; param.proxiesXb = proxies[3]; param.proxiesYb = proxies[4]; param.proxiesZb = proxies[5]; param.worldCenter = worldCenter; param.worldExtent = worldExtent; PfxUpdateBroadphaseProxiesResult result; int ret = pfxUpdateBroadphaseProxies(param,result); if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxUpdateBroadphaseProxies failed %d\n",ret); pool.deallocate(param.workBuff); } //J 交差ペア探索 //E Find overlapped pairs { PfxFindPairsParam findPairsParam; findPairsParam.pairBytes = pfxGetPairBytesOfFindPairs(NUM_CONTACTS); findPairsParam.pairBuff = pool.allocate(findPairsParam.pairBytes); findPairsParam.workBytes = pfxGetWorkBytesOfFindPairs(NUM_CONTACTS); findPairsParam.workBuff = pool.allocate(findPairsParam.workBytes); findPairsParam.proxies = proxies[axis]; findPairsParam.numProxies = numRigidBodies; findPairsParam.maxPairs = NUM_CONTACTS; findPairsParam.axis = axis; PfxFindPairsResult findPairsResult; int ret = pfxFindPairs(findPairsParam,findPairsResult); if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxFindPairs failed %d\n",ret); pool.deallocate(findPairsParam.workBuff); //J 交差ペア合成 //E Decompose overlapped pairs into 3 arrays PfxDecomposePairsParam decomposePairsParam; decomposePairsParam.pairBytes = pfxGetPairBytesOfDecomposePairs(numPreviousPairs,findPairsResult.numPairs); decomposePairsParam.pairBuff = pool.allocate(decomposePairsParam.pairBytes); decomposePairsParam.workBytes = pfxGetWorkBytesOfDecomposePairs(numPreviousPairs,findPairsResult.numPairs); decomposePairsParam.workBuff = pool.allocate(decomposePairsParam.workBytes); decomposePairsParam.previousPairs = previousPairs; decomposePairsParam.numPreviousPairs = numPreviousPairs; decomposePairsParam.currentPairs = findPairsResult.pairs; // Set pairs from pfxFindPairs() decomposePairsParam.numCurrentPairs = findPairsResult.numPairs; // Set the number of pairs from pfxFindPairs() PfxDecomposePairsResult decomposePairsResult; ret = pfxDecomposePairs(decomposePairsParam,decomposePairsResult); if(ret != SCE_PFX_OK) SCE_PFX_PRINTF("pfxDecomposePairs failed %d\n",ret); pool.deallocate(decomposePairsParam.workBuff); PfxBroadphasePair *outNewPairs = decomposePairsResult.outNewPairs; PfxBroadphasePair *outKeepPairs = decomposePairsResult.outKeepPairs; PfxBroadphasePair *outRemovePairs = decomposePairsResult.outRemovePairs; PfxUInt32 numOutNewPairs = decomposePairsResult.numOutNewPairs; PfxUInt32 numOutKeepPairs = decomposePairsResult.numOutKeepPairs; PfxUInt32 numOutRemovePairs = decomposePairsResult.numOutRemovePairs; //J 廃棄ペアのコンタクトをプールに戻す //E Put removed contacts into the contact pool for(PfxUInt32 i=0;i<numOutRemovePairs;i++) { contactIdPool[numContactIdPool++] = pfxGetContactId(outRemovePairs[i]); //J 寝てる剛体を起こす //E Wake up sleeping rigid bodies PfxRigidState &stateA = states[pfxGetObjectIdA(outRemovePairs[i])]; PfxRigidState &stateB = states[pfxGetObjectIdB(outRemovePairs[i])]; if(stateA.isAsleep()) { stateA.wakeup(); } if(stateB.isAsleep()) { stateB.wakeup(); } } //J 新規ペアのコンタクトのリンクと初期化 //E Add new contacts and initialize for(PfxUInt32 i=0;i<numOutNewPairs;i++) { int cId = 0; if(numContactIdPool > 0) { cId = contactIdPool[--numContactIdPool]; } else { cId = numContacts++; } if(cId >= NUM_CONTACTS) { cId = 0; } SCE_PFX_ASSERT(cId < NUM_CONTACTS); pfxSetContactId(outNewPairs[i],cId); PfxContactManifold &contact = contacts[cId]; contact.reset(pfxGetObjectIdA(outNewPairs[i]),pfxGetObjectIdB(outNewPairs[i])); //J 寝てる剛体を起こす //E Wake up sleeping rigid bodies PfxRigidState &stateA = states[pfxGetObjectIdA(outNewPairs[i])]; PfxRigidState &stateB = states[pfxGetObjectIdB(outNewPairs[i])]; if(stateA.isAsleep()) { stateA.wakeup(); } if(stateB.isAsleep()) { stateB.wakeup(); } } //J 新規ペアと維持ペアを合成 //E Merge 'new' and 'keep' pairs numCurrentPairs = 0; for(PfxUInt32 i=0;i<numOutKeepPairs;i++) { currentPairs[numCurrentPairs++] = outKeepPairs[i]; } for(PfxUInt32 i=0;i<numOutNewPairs;i++) { currentPairs[numCurrentPairs++] = outNewPairs[i]; } pool.deallocate(decomposePairsParam.pairBuff); pool.deallocate(findPairsParam.pairBuff); } { int workBytes = sizeof(PfxBroadphasePair) * numCurrentPairs; void *workBuff = pool.allocate(workBytes); pfxParallelSort(currentPairs,numCurrentPairs,workBuff,workBytes); pool.deallocate(workBuff); } }