//----------------------------------------------------------------------------
//  pfxUpdateRigidStates
//
/// Perform update rigid states in parallel using a task manager.
///
/// @param param        Information about rigid bodies
/// @param taskManager  Pointer to the thread task manager to use
///
/// @return SCE_PFX_OK if successful, otherwise, returns an error code.
//----------------------------------------------------------------------------
PfxInt32 pfxUpdateRigidStates(PfxUpdateRigidStatesParam &param, PfxTaskManager *taskManager)
{
	PfxInt32 ret = pfxCheckParamOfUpdateRigidStates(param);
	if(ret != SCE_PFX_OK) return ret;

	SCE_PFX_PUSH_MARKER("pfxUpdateRigidStates");

	PfxUInt32 maxBatchSize = param.numRigidBodies / (PfxUInt32)(taskManager->getNumTasks());
	PfxUInt32 iEnd = maxBatchSize, iStart = 0;
	int task = 0;
	taskManager->setTaskEntry((void*)pfxUpdateRigidStatesTaskEntry);

	for (task = 0; task < taskManager->getNumTasks() - 1; task++, iStart += maxBatchSize, iEnd += maxBatchSize)
	{
		taskManager->startTask(task, static_cast<void*>(&param), iStart, iEnd, 0, 0);
	}

	// send final task
	iEnd = param.numRigidBodies;
	taskManager->startTask(taskManager->getNumTasks() - 1, static_cast<void*>(&param), iStart, iEnd, 0, 0);

	// wait for tasks to complete
	PfxUInt32 data1, data2, data3, data4;
	for (PfxUInt32 i = 0; i < taskManager->getNumTasks(); i++)
		taskManager->waitTask(task, data1, data2, data3, data4);

	SCE_PFX_POP_MARKER();
	
	return SCE_PFX_OK;
}
PfxInt32 pfxUpdateRigidStates(PfxUpdateRigidStatesParam &param)
{
	PfxInt32 ret = pfxCheckParamOfUpdateRigidStates(param);
	if(ret != SCE_PFX_OK) return ret;

	SCE_PFX_PUSH_MARKER("pfxUpdateRigidStates");

	for(PfxUInt32 i=0;i<param.numRigidBodies;i++) {
		pfxIntegrate(param.states[i],param.bodies[i],param.timeStep);
	}

	SCE_PFX_POP_MARKER();

	return SCE_PFX_OK;
}
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;
}
Esempio n. 4
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;
}