Example #1
0
PfxInt32 BulletWriteWarmstartContactConstraints(PfxSetupContactConstraintsParam &param)
{
//	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;
}
Example #2
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;
}
Example #4
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);
	}
}
PfxInt32 pfxDetectCollision(PfxDetectCollisionParam &param)
{
	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;
}
Example #6
0
PfxInt32 BulletSetupContactConstraints(PfxSetupContactConstraintsParam &param)
{
//	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;
}
Example #7
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
	{
		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);
	}
}