Foam::tmp<Foam::scalarField> Foam::lduMatrix::residual ( const scalarField& psi, const scalarField& source, const FieldField<Field, scalar>& interfaceBouCoeffs, const lduInterfaceFieldPtrsList& interfaces, const direction cmpt ) const { tmp<scalarField> trA(new scalarField(psi.size())); residual(trA.ref(), psi, source, interfaceBouCoeffs, interfaces, cmpt); return trA; }
void detectCollisionSpherePlane(RTB3CollisionWorld* world, int colA, int shapeIndexA, int colB, int shapeIndexB, plContactCache* contactCache) { const b3Transform& trA = world->m_collidableTransforms[colA]; const b3Vector3& sphereALocalPos = world->m_childShapes[shapeIndexA].m_childPosition; b3Vector3 spherePosAWorld = trA(sphereALocalPos); int planeFaceIndex = world->m_childShapes[shapeIndexB].m_shapeIndex; b3Vector3 planeNormal = world->m_planeFaces[planeFaceIndex].m_plane; b3Scalar planeConstant = planeNormal[3]; planeNormal[3] = 0.f; ComputeClosestPointsPlaneSphere(planeNormal, planeConstant, spherePosAWorld, world->m_childShapes[shapeIndexA].m_radius, contactCache); }
B3_FORCE_INLINE void detectCollisionSphereSphere(RTB3CollisionWorld* world, int colA, int shapeIndexA, int colB, int shapeIndexB, plContactCache* contactCache) { const b3Scalar radiusA = world->m_childShapes[shapeIndexA].m_radius; const b3Scalar radiusB = world->m_childShapes[shapeIndexB].m_radius; const b3Transform& trA = world->m_collidableTransforms[colA]; const b3Vector3& sphereALocalPos = world->m_childShapes[shapeIndexA].m_childPosition; b3Vector3 spherePosAWorld = trA(sphereALocalPos); //b3Vector3 spherePosAWorld = b3QuatRotate( world->m_collidableOrientations[colA], sphereALocalPos ) + (world->m_collidablePositions[colA]); const b3Transform& trB = world->m_collidableTransforms[colB]; const b3Vector3& sphereBLocalPos = world->m_childShapes[shapeIndexB].m_childPosition; b3Vector3 spherePosBWorld = trB(sphereBLocalPos); //b3Vector3 spherePosBWorld = b3QuatRotate( world->m_collidableOrientations[colB], sphereBLocalPos ) + (world->m_collidablePositions[colB]); ComputeClosestPointsSphereSphere(radiusA, spherePosAWorld, radiusB, spherePosBWorld, contactCache); }
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; }