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); } }
const PfxContactManifold &physics_get_contact(int id) { return contacts[pfxGetConstraintId(pairs[id])]; }
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; }