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,
};
// 面積が０の面を排除
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));
}
}
}
}
// 面の面積によって３種類に分類する
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);
}
}
