//----------------------------------------------------------------------------
// 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 ¶m, 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*>(¶m), iStart, iEnd, 0, 0);
}
// send final task
iEnd = param.numRigidBodies;
taskManager->startTask(taskManager->getNumTasks() - 1, static_cast<void*>(¶m), 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 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;
}
PfxInt32 pfxUpdateRigidStates(PfxUpdateRigidStatesParam ¶m)
{
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 ¶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;
}
