void btDiscreteDynamicsWorld::createPredictiveContactsInternal( btRigidBody** bodies, int numBodies, btScalar timeStep) { btTransform predictedTrans; for ( int i=0;i<numBodies;i++) { btRigidBody* body = bodies[i]; body->setHitFraction(1.f); if (body->isActive() && (!body->isStaticOrKinematicObject())) { body->predictIntegratedTransform(timeStep, predictedTrans); btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2(); if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion) { BT_PROFILE("predictive convexSweepTest"); if (body->getCollisionShape()->isConvex()) { gNumClampedCcdMotions++; #ifdef PREDICTIVE_CONTACT_USE_STATIC_ONLY class StaticOnlyCallback : public btClosestNotMeConvexResultCallback { public: StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher) { } virtual bool needsCollision(btBroadphaseProxy* proxy0) const { btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject; if (!otherObj->isStaticOrKinematicObject()) return false; return btClosestNotMeConvexResultCallback::needsCollision(proxy0); } }; StaticOnlyCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); #else btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); #endif //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); sweepResults.m_allowedPenetration=getDispatchInfo().m_allowedCcdPenetration; sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup; sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; btTransform modifiedPredictedTrans = predictedTrans; modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis()); convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults); if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f)) { btVector3 distVec = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin())*sweepResults.m_closestHitFraction; btScalar distance = distVec.dot(-sweepResults.m_hitNormalWorld); btPersistentManifold* manifold = m_dispatcher1->getNewManifold(body,sweepResults.m_hitCollisionObject); btMutexLock( &m_predictiveManifoldsMutex ); m_predictiveManifolds.push_back(manifold); btMutexUnlock( &m_predictiveManifoldsMutex ); btVector3 worldPointB = body->getWorldTransform().getOrigin()+distVec; btVector3 localPointB = sweepResults.m_hitCollisionObject->getWorldTransform().inverse()*worldPointB; btManifoldPoint newPoint(btVector3(0,0,0), localPointB,sweepResults.m_hitNormalWorld,distance); bool isPredictive = true; int index = manifold->addManifoldPoint(newPoint, isPredictive); btManifoldPoint& pt = manifold->getContactPoint(index); pt.m_combinedRestitution = 0; pt.m_combinedFriction = gCalculateCombinedFrictionCallback(body,sweepResults.m_hitCollisionObject); pt.m_positionWorldOnA = body->getWorldTransform().getOrigin(); pt.m_positionWorldOnB = worldPointB; } } } } } }
void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) { BT_PROFILE("integrateTransforms"); btTransform predictedTrans; for ( int i=0;i<m_nonStaticRigidBodies.size();i++) { btRigidBody* body = m_nonStaticRigidBodies[i]; body->setHitFraction(1.f); if (body->isActive() && (!body->isStaticOrKinematicObject())) { body->predictIntegratedTransform(timeStep, predictedTrans); btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2(); if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion) { BT_PROFILE("CCD motion clamping"); if (body->getCollisionShape()->isConvex()) { gNumClampedCcdMotions++; #ifdef USE_STATIC_ONLY class StaticOnlyCallback : public btClosestNotMeConvexResultCallback { public: StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher) { } virtual bool needsCollision(btBroadphaseProxy* proxy0) const { btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject; if (!otherObj->isStaticOrKinematicObject()) return false; return btClosestNotMeConvexResultCallback::needsCollision(proxy0); } }; StaticOnlyCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); #else btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); #endif //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); sweepResults.m_allowedPenetration=getDispatchInfo().m_allowedCcdPenetration; sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup; sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; btTransform modifiedPredictedTrans = predictedTrans; modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis()); convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults); if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f)) { //printf("clamped integration to hit fraction = %f\n",fraction); body->setHitFraction(sweepResults.m_closestHitFraction); body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans); body->setHitFraction(0.f); body->proceedToTransform( predictedTrans); #if 0 btVector3 linVel = body->getLinearVelocity(); btScalar maxSpeed = body->getCcdMotionThreshold()/getSolverInfo().m_timeStep; btScalar maxSpeedSqr = maxSpeed*maxSpeed; if (linVel.length2()>maxSpeedSqr) { linVel.normalize(); linVel*= maxSpeed; body->setLinearVelocity(linVel); btScalar ms2 = body->getLinearVelocity().length2(); body->predictIntegratedTransform(timeStep, predictedTrans); btScalar sm2 = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2(); btScalar smt = body->getCcdSquareMotionThreshold(); printf("sm2=%f\n",sm2); } #else //response between two dynamic objects without friction, assuming 0 penetration depth btScalar appliedImpulse = 0.f; btScalar depth = 0.f; appliedImpulse = resolveSingleCollision(body,sweepResults.m_hitCollisionObject,sweepResults.m_hitPointWorld,sweepResults.m_hitNormalWorld,getSolverInfo(), depth); #endif continue; } } } body->proceedToTransform( predictedTrans); } } }