void addCallTime( float time )
{
#if BT_THREADSAFE
// user provided mutex lock func might be instrumented with profiling, so
// we can't call it or we risk infinite recursion
btMutexLockInternal( &mMutex );
#endif
mTimeThisFrame += time;
mCallCount++;
#if BT_THREADSAFE
btMutexUnlock( &mMutex );
#endif
}
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;
}
}
}
}
}
}
