PfxInt32 BulletWriteWarmstartContactConstraints(PfxSetupContactConstraintsParam ¶m)
{
// PfxInt32 ret = pfxCheckParamOfSetupContactConstraints(param);
//if(ret != SCE_PFX_OK) return ret;
SCE_PFX_PUSH_MARKER("pfxSetupContactConstraints");
PfxConstraintPair *contactPairs = param.contactPairs;
PfxUInt32 numContactPairs = param.numContactPairs;
PfxContactManifold *offsetContactManifolds = param.offsetContactManifolds;
PfxRigidState *offsetRigidStates = param.offsetRigidStates;
PfxRigidBody *offsetRigidBodies = param.offsetRigidBodies;
PfxSolverBody *offsetSolverBodies = param.offsetSolverBodies;
for(PfxUInt32 i=0;i<numContactPairs;i++)
{
PfxConstraintPair &pair = contactPairs[i];
PfxUInt16 iA = pfxGetObjectIdA(pair);
PfxUInt16 iB = pfxGetObjectIdB(pair);
PfxUInt32 iConstraint = pfxGetConstraintId(pair);
PfxContactManifold &contact = offsetContactManifolds[iConstraint];
btPersistentManifold& manifold = manifolds[i];
for (int c=0;c<manifold.m_cachedPoints;c++)
{
contact.getContactPoint(c).m_constraintRow[0].m_accumImpulse = manifold.m_pointCache[c].m_appliedImpulse;
contact.getContactPoint(c).m_constraintRow[1].m_accumImpulse = manifold.m_pointCache[c].m_appliedImpulseLateral1;
contact.getContactPoint(c).m_constraintRow[2].m_accumImpulse = manifold.m_pointCache[c].m_appliedImpulseLateral2;
}
}
return 0;
}
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);
}
}
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;
}
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);
}
}
PfxInt32 pfxDetectCollision(PfxDetectCollisionParam ¶m)
{
PfxInt32 ret = pfxCheckParamOfDetectCollision(param);
if(ret != SCE_PFX_OK)
return ret;
SCE_PFX_PUSH_MARKER("pfxDetectCollision");
PfxConstraintPair *contactPairs = param.contactPairs;
PfxUInt32 numContactPairs = param.numContactPairs;
PfxContactManifold *offsetContactManifolds = param.offsetContactManifolds;
PfxRigidState *offsetRigidStates = param.offsetRigidStates;
PfxCollidable *offsetCollidables = param.offsetCollidables;
PfxUInt32 numRigidBodies = param.numRigidBodies;
for(PfxUInt32 i=0;i<numContactPairs;i++) {
const PfxBroadphasePair &pair = contactPairs[i];
if(!pfxCheckCollidableInCollision(pair)) {
continue;
}
PfxUInt32 iContact = pfxGetContactId(pair);
PfxUInt32 iA = pfxGetObjectIdA(pair);
PfxUInt32 iB = pfxGetObjectIdB(pair);
PfxContactManifold &contact = offsetContactManifolds[iContact];
SCE_PFX_ALWAYS_ASSERT(iA==contact.getRigidBodyIdA());
SCE_PFX_ALWAYS_ASSERT(iB==contact.getRigidBodyIdB());
PfxRigidState &stateA = offsetRigidStates[iA];
PfxRigidState &stateB = offsetRigidStates[iB];
PfxCollidable &collA = offsetCollidables[iA];
PfxCollidable &collB = offsetCollidables[iB];
PfxTransform3 tA0(stateA.getOrientation(), stateA.getPosition());
PfxTransform3 tB0(stateB.getOrientation(), stateB.getPosition());
PfxContactCache contactCache;
PfxShapeIterator itrShapeA(collA);
for(PfxUInt32 j=0;j<collA.getNumShapes();j++,++itrShapeA) {
const PfxShape &shapeA = *itrShapeA;
PfxTransform3 offsetTrA = shapeA.getOffsetTransform();
PfxTransform3 worldTrA = tA0 * offsetTrA;
PfxShapeIterator itrShapeB(collB);
for(PfxUInt32 k=0;k<collB.getNumShapes();k++,++itrShapeB) {
const PfxShape &shapeB = *itrShapeB;
PfxTransform3 offsetTrB = shapeB.getOffsetTransform();
PfxTransform3 worldTrB = tB0 * offsetTrB;
if( (shapeA.getContactFilterSelf()&shapeB.getContactFilterTarget()) &&
(shapeA.getContactFilterTarget()&shapeB.getContactFilterSelf()) ) {
pfxGetDetectCollisionFunc(shapeA.getType(),shapeB.getType())(
contactCache,
shapeA,offsetTrA,worldTrA,j,
shapeB,offsetTrB,worldTrB,k,
SCE_PFX_CONTACT_THRESHOLD);
}
}
}
for(int j=0;j<contactCache.getNumContacts();j++) {
const PfxCachedContactPoint &cp = contactCache.getContactPoint(j);
contact.addContactPoint(
cp.m_distance,
cp.m_normal,
cp.m_localPointA,
cp.m_localPointB,
cp.m_subData
);
}
}
SCE_PFX_POP_MARKER();
(void) numRigidBodies;
return SCE_PFX_OK;
}
PfxInt32 BulletSetupContactConstraints(PfxSetupContactConstraintsParam ¶m)
{
// PfxInt32 ret = pfxCheckParamOfSetupContactConstraints(param);
//if(ret != SCE_PFX_OK) return ret;
SCE_PFX_PUSH_MARKER("pfxSetupContactConstraints");
PfxConstraintPair *contactPairs = param.contactPairs;
PfxUInt32 numContactPairs = param.numContactPairs;
PfxContactManifold *offsetContactManifolds = param.offsetContactManifolds;
PfxRigidState *offsetRigidStates = param.offsetRigidStates;
PfxRigidBody *offsetRigidBodies = param.offsetRigidBodies;
PfxSolverBody *offsetSolverBodies = param.offsetSolverBodies;
manifolds.resize(0);
for(PfxUInt32 i=0;i<numContactPairs;i++) {
PfxConstraintPair &pair = contactPairs[i];
// if(!sce::PhysicsEffects::pfxCheckSolver(pair)) {
// continue;
//}
PfxUInt16 iA = pfxGetObjectIdA(pair);
PfxUInt16 iB = pfxGetObjectIdB(pair);
PfxUInt32 iConstraint = pfxGetConstraintId(pair);
PfxContactManifold &contact = offsetContactManifolds[iConstraint];
btPersistentManifold& manifold = manifolds.expand();
memset(&manifold,0xff,sizeof(btPersistentManifold));
manifold.m_body0 = &rbs[iA];
manifold.m_body1 = &rbs[iB];
manifold.m_cachedPoints = contact.getNumContacts();
if (!contact.getNumContacts())
continue;
SCE_PFX_ALWAYS_ASSERT(iA==contact.getRigidBodyIdA());
SCE_PFX_ALWAYS_ASSERT(iB==contact.getRigidBodyIdB());
PfxRigidState &stateA = offsetRigidStates[iA];
PfxRigidBody &bodyA = offsetRigidBodies[iA];
PfxSolverBody &solverBodyA = offsetSolverBodies[iA];
PfxRigidState &stateB = offsetRigidStates[iB];
PfxRigidBody &bodyB = offsetRigidBodies[iB];
PfxSolverBody &solverBodyB = offsetSolverBodies[iB];
contact.setInternalFlag(0);
PfxFloat restitution = 0.5f * (bodyA.getRestitution() + bodyB.getRestitution());
if(contact.getDuration() > 1) restitution = 0.0f;
PfxFloat friction = sqrtf(bodyA.getFriction() * bodyB.getFriction());
manifold.m_cachedPoints = contact.getNumContacts();
manifold.m_contactProcessingThreshold = 0.01f;//SCE_PFX_CONTACT_THRESHOLD_NORMAL;
manifold.m_contactBreakingThreshold = 0.01f;
for(int j=0;j<contact.getNumContacts();j++) {
PfxContactPoint &cp = contact.getContactPoint(j);
PfxVector3 ptA = pfxReadVector3(cp.m_localPointA);
manifold.m_pointCache[j].m_localPointA.setValue(ptA.getX(),ptA.getY(),ptA.getZ());
PfxVector3 ptB = pfxReadVector3(cp.m_localPointB);
manifold.m_pointCache[j].m_localPointB.setValue(ptB.getX(),ptB.getY(),ptB.getZ());
manifold.m_pointCache[j].m_normalWorldOnB.setValue(
cp.m_constraintRow[0].m_normal[0],
cp.m_constraintRow[0].m_normal[1],
cp.m_constraintRow[0].m_normal[2]);
manifold.m_pointCache[j].m_distance1 = cp.m_distance1;
manifold.m_pointCache[j].m_combinedFriction = friction;
manifold.m_pointCache[j].m_combinedRestitution = restitution;
manifold.m_pointCache[j].m_appliedImpulse = cp.m_constraintRow[0].m_accumImpulse;
manifold.m_pointCache[j].m_lateralFrictionDir1.setValue(
cp.m_constraintRow[1].m_normal[0],
cp.m_constraintRow[1].m_normal[1],
cp.m_constraintRow[1].m_normal[2]);
manifold.m_pointCache[j].m_appliedImpulseLateral1 = cp.m_constraintRow[1].m_accumImpulse;
manifold.m_pointCache[j].m_lateralFrictionDir2.setValue(
cp.m_constraintRow[2].m_normal[0],
cp.m_constraintRow[2].m_normal[1],
cp.m_constraintRow[2].m_normal[2]);
manifold.m_pointCache[j].m_appliedImpulseLateral2 = cp.m_constraintRow[2].m_accumImpulse;
manifold.m_pointCache[j].m_lateralFrictionInitialized = true;
manifold.m_pointCache[j].m_lifeTime = cp.m_duration;
btTransform trA = manifold.m_body0->getWorldTransform();
btTransform trB = manifold.m_body1->getWorldTransform();
manifold.m_pointCache[j].m_positionWorldOnA = trA( manifold.m_pointCache[j].m_localPointA );
manifold.m_pointCache[j].m_positionWorldOnB = trB( manifold.m_pointCache[j].m_localPointB );
//btVector3 m_localPointA;
//btVector3 m_localPointB;
//btVector3 m_positionWorldOnB;
//m_positionWorldOnA is redundant information, see getPositionWorldOnA(), but for clarity
//btVector3 m_positionWorldOnA;
/*
pfxSetupContactConstraint(
cp.m_constraintRow[0],
cp.m_constraintRow[1],
cp.m_constraintRow[2],
cp.m_distance,
restitution,
friction,
pfxReadVector3(cp.m_constraintRow[0].m_normal),
pfxReadVector3(cp.m_localPointA),
pfxReadVector3(cp.m_localPointB),
stateA,
stateB,
solverBodyA,
solverBodyB,
param.separateBias,
param.timeStep
);
*/
}
contact.setCompositeFriction(friction);
}
SCE_PFX_POP_MARKER();
return SCE_PFX_OK;
}
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);
}
}