void render(void)
{
render_begin();
const PfxVector3 colorWhite(1.0f);
const PfxVector3 colorGray(0.7f);
for(int i=0;i<physics_get_num_rigidbodies();i++) {
const PfxRigidState &state = physics_get_state(i);
const PfxCollidable &coll = physics_get_collidable(i);
PfxVector3 color = state.isAsleep()?colorGray:colorWhite;
PfxTransform3 rbT(state.getOrientation(), state.getPosition());
PfxShapeIterator itrShape(coll);
for(PfxUInt32 j=0;j<coll.getNumShapes();j++,++itrShape) {
const PfxShape &shape = *itrShape;
PfxTransform3 offsetT = shape.getOffsetTransform();
PfxTransform3 worldT = rbT * offsetT;
switch(shape.getType()) {
case kPfxShapeSphere:
render_sphere(
worldT,
color,
PfxFloatInVec(shape.getSphere().m_radius));
break;
case kPfxShapeBox:
render_box(
worldT,
color,
shape.getBox().m_half);
break;
case kPfxShapeCapsule:
render_capsule(
worldT,
color,
PfxFloatInVec(shape.getCapsule().m_radius),
PfxFloatInVec(shape.getCapsule().m_halfLen));
break;
case kPfxShapeCylinder:
render_cylinder(
worldT,
color,
PfxFloatInVec(shape.getCylinder().m_radius),
PfxFloatInVec(shape.getCylinder().m_halfLen));
break;
case kPfxShapeConvexMesh:
render_mesh(
worldT,
color,
convexMeshId);
break;
case kPfxShapeLargeTriMesh:
render_mesh(
worldT,
color,
landscapeMeshId);
break;
default:
break;
}
}
}
render_debug_begin();
#ifdef ENABLE_DEBUG_DRAW_CONTACT
for(int i=0;i<physics_get_num_contacts();i++) {
const PfxContactManifold &contact = physics_get_contact(i);
const PfxRigidState &stateA = physics_get_state(contact.getRigidBodyIdA());
const PfxRigidState &stateB = physics_get_state(contact.getRigidBodyIdB());
for(int j=0;j<contact.getNumContacts();j++) {
const PfxContactPoint &cp = contact.getContactPoint(j);
PfxVector3 pA = stateA.getPosition()+rotate(stateA.getOrientation(),pfxReadVector3(cp.m_localPointA));
const float w = 0.05f;
render_debug_line(pA+PfxVector3(-w,0.0f,0.0f),pA+PfxVector3(w,0.0f,0.0f),PfxVector3(0,0,1));
render_debug_line(pA+PfxVector3(0.0f,-w,0.0f),pA+PfxVector3(0.0f,w,0.0f),PfxVector3(0,0,1));
render_debug_line(pA+PfxVector3(0.0f,0.0f,-w),pA+PfxVector3(0.0f,0.0f,w),PfxVector3(0,0,1));
}
}
#endif
#ifdef ENABLE_DEBUG_DRAW_AABB
for(int i=0;i<physics_get_num_rigidbodies();i++) {
const PfxRigidState &state = physics_get_state(i);
const PfxCollidable &coll = physics_get_collidable(i);
PfxVector3 center = state.getPosition() + coll.getCenter();
PfxVector3 half = absPerElem(PfxMatrix3(state.getOrientation())) * coll.getHalf();
render_debug_box(center,half,PfxVector3(1,0,0));
}
#endif
#ifdef ENABLE_DEBUG_DRAW_ISLAND
const PfxIsland *island = physics_get_islands();
if(island) {
for(PfxUInt32 i=0;i<pfxGetNumIslands(island);i++) {
PfxIslandUnit *islandUnit = pfxGetFirstUnitInIsland(island,i);
PfxVector3 aabbMin(SCE_PFX_FLT_MAX);
PfxVector3 aabbMax(-SCE_PFX_FLT_MAX);
for(;islandUnit!=NULL;islandUnit=pfxGetNextUnitInIsland(islandUnit)) {
const PfxRigidState &state = physics_get_state(pfxGetUnitId(islandUnit));
const PfxCollidable &coll = physics_get_collidable(pfxGetUnitId(islandUnit));
PfxVector3 center = state.getPosition() + coll.getCenter();
PfxVector3 half = absPerElem(PfxMatrix3(state.getOrientation())) * coll.getHalf();
aabbMin = minPerElem(aabbMin,center-half);
aabbMax = maxPerElem(aabbMax,center+half);
}
render_debug_box((aabbMax+aabbMin)*0.5f,(aabbMax-aabbMin)*0.5f,PfxVector3(0,1,0));
}
}
#endif
for(int i=0;i<physics_get_num_rays();i++) {
const PfxRayInput& rayInput = physics_get_rayinput(i);
const PfxRayOutput& rayOutput = physics_get_rayoutput(i);
if(rayOutput.m_contactFlag) {
render_debug_line(
rayInput.m_startPosition,
rayOutput.m_contactPoint,
PfxVector3(1.0f,0.0f,1.0f));
render_debug_line(
rayOutput.m_contactPoint,
rayOutput.m_contactPoint+rayOutput.m_contactNormal,
PfxVector3(1.0f,0.0f,1.0f));
}
else {
render_debug_line(rayInput.m_startPosition,
rayInput.m_startPosition+rayInput.m_direction,
PfxVector3(0.5f,0.0f,0.5f));
}
}
extern bool doAreaRaycast;
extern PfxVector3 areaCenter;
extern PfxVector3 areaExtent;
if(doAreaRaycast) {
render_debug_box(areaCenter,areaExtent,PfxVector3(0,0,1));
}
render_debug_end();
render_end();
}
void render(void)
{
render_begin();
for(int i=0;i<physics_get_num_rigidbodies();i++) {
const PfxRigidState &state = physics_get_state(i);
const PfxCollidable &coll = physics_get_collidable(i);
PfxTransform3 rbT(state.getOrientation(), state.getPosition());
PfxShapeIterator itrShape(coll);
for(int j=0;j<coll.getNumShapes();j++,++itrShape) {
const PfxShape &shape = *itrShape;
PfxTransform3 offsetT = shape.getOffsetTransform();
PfxTransform3 worldT = rbT * offsetT;
switch(shape.getType()) {
case kPfxShapeSphere:
render_sphere(
worldT,
PfxVector3(1,1,1),
PfxFloatInVec(shape.getSphere().m_radius));
break;
case kPfxShapeBox:
render_box(
worldT,
PfxVector3(1,1,1),
shape.getBox().m_half);
break;
case kPfxShapeCapsule:
render_capsule(
worldT,
PfxVector3(1,1,1),
PfxFloatInVec(shape.getCapsule().m_radius),
PfxFloatInVec(shape.getCapsule().m_halfLen));
break;
case kPfxShapeCylinder:
render_cylinder(
worldT,
PfxVector3(1,1,1),
PfxFloatInVec(shape.getCylinder().m_radius),
PfxFloatInVec(shape.getCylinder().m_halfLen));
break;
case kPfxShapeConvexMesh:
render_mesh(
worldT,
PfxVector3(1,1,1),
convexMeshId);
break;
case kPfxShapeLargeTriMesh:
render_mesh(
worldT,
PfxVector3(1,1,1),
landscapeMeshId);
break;
default:
break;
}
}
}
render_end();
}
void graphics_from_physics(GLInstancingRenderer& renderer, bool syncTransformsOnly)
{
int cubeShapeIndex = -1;
int strideInBytes = sizeof(float)*9;
if (!syncTransformsOnly)
{
int numVertices = sizeof(cube_vertices)/strideInBytes;
int numIndices = sizeof(cube_indices)/sizeof(int);
cubeShapeIndex = renderer.registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices);
}
int curGraphicsIndex=0;
for(int i=0;i<physics_get_num_rigidbodies();i++)
{
const PfxRigidState &state = physics_get_state(i);
const PfxCollidable &coll = physics_get_collidable(i);
const PfxRigidBody& body = physics_get_body(i);
float color[4]={0,0,0,1};
if (body.getMass()==0.f)
{
color[0]=1.f;
} else
{
color[1]=1.f;
}
PfxTransform3 rbT(state.getOrientation(), state.getPosition());
PfxShapeIterator itrShape(coll);
for(int j=0;j<coll.getNumShapes();j++,++itrShape) {
const PfxShape &shape = *itrShape;
PfxTransform3 offsetT = shape.getOffsetTransform();
PfxTransform3 worldT = rbT * offsetT;
PfxQuat ornWorld( worldT.getUpper3x3());
switch(shape.getType()) {
case kPfxShapeSphere:
printf("render sphere\n");
/*render_sphere(
worldT,
PfxVector3(1,1,1),
PfxFloatInVec(shape.getSphere().m_radius));
*/
break;
case kPfxShapeBox:
{
// printf("render box\n");
float cubeScaling[4] = {shape.getBox().m_half.getX(),shape.getBox().m_half.getY(),shape.getBox().m_half.getZ(),1};
float rotOrn[4] = {ornWorld.getX(),ornWorld.getY(),ornWorld.getZ(),ornWorld.getW()};
float position[4]={worldT.getTranslation().getX(),worldT.getTranslation().getY(),worldT.getTranslation().getZ(),0};
if (!syncTransformsOnly)
{
renderer.registerGraphicsInstance(cubeShapeIndex,position,rotOrn,color,cubeScaling);
}
else
{
renderer.writeSingleInstanceTransformToCPU(position,rotOrn,curGraphicsIndex);
}
curGraphicsIndex++;
/* render_box(
worldT,
PfxVector3(1,1,1),
shape.getBox().m_half);
*/
break;
}
case kPfxShapeCapsule:
printf("render_capsule\n");
/*render_capsule(
worldT,
PfxVector3(1,1,1),
PfxFloatInVec(shape.getCapsule().m_radius),
PfxFloatInVec(shape.getCapsule().m_halfLen));
*/
break;
case kPfxShapeCylinder:
printf("render_cylinder\n");
/* render_cylinder(
worldT,
PfxVector3(1,1,1),
PfxFloatInVec(shape.getCylinder().m_radius),
PfxFloatInVec(shape.getCylinder().m_halfLen));
*/
break;
case kPfxShapeConvexMesh:
printf("render_mesh\n");
/*
render_mesh(
worldT,
PfxVector3(1,1,1),
convexMeshId);
*/
break;
case kPfxShapeLargeTriMesh:
printf("render_mesh\n");
/*
render_mesh(
worldT,
PfxVector3(1,1,1),
landscapeMeshId);
*/
break;
default:
break;
}
}
}
}
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);
}
}
