static
int addIslandToLargeTriMesh(PfxLargeTriMesh &lmesh,PfxTriMesh &island)
{
SCE_PFX_ASSERT(island.m_numFacets <= SCE_PFX_NUMMESHFACETS);
int newIsland = lmesh.m_numIslands++;
lmesh.m_islands[newIsland] = island;
// アイランドローカルのAABBを計算
if(island.m_numFacets > 0) {
PfxVector3 aabbMin(SCE_PFX_FLT_MAX),aabbMax(-SCE_PFX_FLT_MAX);
for(PfxUInt32 i=0;i<island.m_numFacets;i++) {
aabbMin = minPerElem(aabbMin,pfxReadVector3(island.m_facets[i].m_center)-pfxReadVector3(island.m_facets[i].m_half));
aabbMax = maxPerElem(aabbMax,pfxReadVector3(island.m_facets[i].m_center)+pfxReadVector3(island.m_facets[i].m_half));
}
PfxVecInt3 aabbMinL,aabbMaxL;
lmesh.getLocalPosition(aabbMin,aabbMax,aabbMinL,aabbMaxL);
pfxSetXMin(lmesh.m_aabbList[newIsland],aabbMinL.getX());
pfxSetXMax(lmesh.m_aabbList[newIsland],aabbMaxL.getX());
pfxSetYMin(lmesh.m_aabbList[newIsland],aabbMinL.getY());
pfxSetYMax(lmesh.m_aabbList[newIsland],aabbMaxL.getY());
pfxSetZMin(lmesh.m_aabbList[newIsland],aabbMinL.getZ());
pfxSetZMax(lmesh.m_aabbList[newIsland],aabbMaxL.getZ());
}
return newIsland;
}
void createSceneJoints()
{
const int n = 10;
int startId = numRigidBodies;
PfxVector3 boxSize(1.0f);
PfxFloat boxMass = 1.0f;
for(int i=0;i<n;i++) {
createBrick(numRigidBodies++,PfxVector3(0,3.0f+i*2.5f*boxSize[1],0),PfxQuat::identity(),boxSize,boxMass);
}
for(int i=startId;i<startId+n;i++) {
PfxRigidState &stateA = states[i];
PfxRigidState &stateB = states[(i+1)%numRigidBodies];
PfxVector3 anchor;
if(i == numRigidBodies-1) {
anchor = stateA.getPosition() + PfxVector3(0,boxSize[1],0);
}
else {
anchor = ( stateA.getPosition() + stateB.getPosition() ) * 0.5f;
}
PfxSwingTwistJointInitParam jparam;
jparam.anchorPoint = anchor;
jparam.twistAxis = PfxVector3(0,1,0);
pfxInitializeSwingTwistJoint(joints[numJoints],stateA,stateB,jparam);
joints[numJoints].m_constraints[4].m_damping = 0.1f;
joints[numJoints].m_constraints[5].m_damping = 0.1f;
SCE_PFX_ASSERT(numJoints<NUM_JOINTS);
numJoints++;
}
states[startId].setLinearVelocity(PfxVector3(0,0,5));
states[startId].setLinearDamping(0.95f);
states[startId].setAngularDamping(0.95f);
}
static
void createIsland(PfxTriMesh &island,const PfxArray<PfxMcFacetPtr> &facets)
{
if(facets.empty()) return;
island.m_numFacets = facets.size();
PfxUInt32 vertsFlag[(0xff*SCE_PFX_NUMMESHFACETS*3+31)/32];
memset(vertsFlag,0,sizeof(PfxUInt32)*((0xff*SCE_PFX_NUMMESHFACETS*3+31)/32));
PfxArray<PfxMcEdgeEntry*> edgeHead(facets.size()*3);
PfxArray<PfxMcEdgeEntry> edgeList(facets.size()*3);
PfxMcEdgeEntry* nl = NULL;
edgeHead.assign(facets.size()*3,nl);
edgeList.assign(facets.size()*3,PfxMcEdgeEntry());
int vcnt = 0;
int ecnt = 0;
for(PfxUInt32 f=0;f<facets.size();f++) {
PfxMcFacet &iFacet = *facets[f];
PfxMcEdge *iEdge[3] = {
iFacet.e[0],
iFacet.e[1],
iFacet.e[2],
};
PfxFacet &oFacet = island.m_facets[f];
oFacet.m_half[0] = oFacet.m_half[1] = oFacet.m_half[2] = 0.0f;
oFacet.m_center[0] = oFacet.m_center[1] = oFacet.m_center[2] = 0.0f;
pfxStoreVector3(iFacet.n,oFacet.m_normal);
oFacet.m_thickness = iFacet.thickness;
// Vertex
for(int v=0;v<3;v++) {
PfxMcVert *vert = facets[f]->v[v];
PfxUInt32 idx = vert->i;
PfxUInt32 mask = 1 << (idx & 31);
if((vertsFlag[idx>>5] & mask) == 0) {
SCE_PFX_ASSERT(vcnt<SCE_PFX_NUMMESHVERTICES);
vertsFlag[idx>>5] |= mask;
island.m_verts[vcnt] = vert->coord;
vert->flag = vcnt;// 新しいインデックス
vcnt++;
}
oFacet.m_vertIds[v] = (PfxUInt8)vert->flag;
}
// Edge
for(int v=0;v<3;v++) {
PfxUInt8 viMin = SCE_PFX_MIN(oFacet.m_vertIds[v],oFacet.m_vertIds[(v+1)%3]);
PfxUInt8 viMax = SCE_PFX_MAX(oFacet.m_vertIds[v],oFacet.m_vertIds[(v+1)%3]);
int key = ((0x8da6b343*viMin+0xd8163841*viMax)%(island.m_numFacets*3));
for(PfxMcEdgeEntry *e=edgeHead[key];;e=e->next) {
if(!e) {
edgeList[ecnt].vertId[0] = viMin;
edgeList[ecnt].vertId[1] = viMax;
edgeList[ecnt].facetId[0] = f;
edgeList[ecnt].numFacets = 1;
edgeList[ecnt].edgeNum[0] = v;
edgeList[ecnt].edgeId = ecnt;
edgeList[ecnt].dir = normalize(island.m_verts[viMax]-island.m_verts[viMin]);
edgeList[ecnt].next = edgeHead[key];
edgeHead[key] = &edgeList[ecnt];
PfxEdge edge;
edge.m_angleType = iEdge[v]->angleType;
// 厚み角の設定 0～πを0～255の整数値に変換して格納
edge.m_tilt = (PfxUInt8)((iEdge[v]->angle/(0.5f*SCE_PFX_PI))*255.0f);
edge.m_vertId[0] = viMin;
edge.m_vertId[1] = viMax;
oFacet.m_edgeIds[v] = ecnt;
island.m_edges[ecnt] = edge;
SCE_PFX_ASSERT(ecnt <= SCE_PFX_NUMMESHEDGES);
ecnt++;
break;
}
if(e->vertId[0] == viMin && e->vertId[1] == viMax) {
SCE_PFX_ASSERT(e->numFacets==1);
e->facetId[1] = f;
e->edgeNum[1] = v;
e->numFacets = 2;
oFacet.m_edgeIds[v] = e->edgeId;
break;
}
}
}
}
island.m_numEdges = ecnt;
island.m_numVerts = vcnt;
island.updateAABB();
}
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);
}
}
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);
}
}
PfxSetupJointConstraintFunc pfxGetSetupJointConstraintFunc(PfxUInt8 jointType)
{
SCE_PFX_ASSERT(jointType<kPfxJointCount);
return funcTbl_setupJointConstraint[jointType];
}
PfxWarmStartJointConstraintFunc pfxGetWarmStartJointConstraintFunc(PfxUInt8 jointType)
{
SCE_PFX_ASSERT(jointType<kPfxJointCount);
return funcTbl_warmStartJointConstraint[jointType];
}
