void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
{
BT_PROFILE("internalSingleStepSimulation");
if(0 != m_internalPreTickCallback) {
(*m_internalPreTickCallback)(this, timeStep);
}
///apply gravity, predict motion
predictUnconstraintMotion(timeStep);
btDispatcherInfo& dispatchInfo = getDispatchInfo();
dispatchInfo.m_timeStep = timeStep;
dispatchInfo.m_stepCount = 0;
dispatchInfo.m_debugDraw = getDebugDrawer();
///perform collision detection
performDiscreteCollisionDetection();
if (getDispatchInfo().m_useContinuous)
addSpeculativeContacts(timeStep);
calculateSimulationIslands();
getSolverInfo().m_timeStep = timeStep;
///solve contact and other joint constraints
solveConstraints(getSolverInfo());
///CallbackTriggers();
///integrate transforms
integrateTransforms(timeStep);
///update vehicle simulation
updateActions(timeStep);
updateActivationState( timeStep );
if(0 != m_internalTickCallback) {
(*m_internalTickCallback)(this, timeStep);
}
}
void btContinuousDynamicsWorld::calculateTimeOfImpacts(btScalar timeStep)
{
///these should be 'temporal' aabbs!
updateTemporalAabbs(timeStep);
///'toi' is the global smallest time of impact. However, we just calculate the time of impact for each object individually.
///so we handle the case moving versus static properly, and we cheat for moving versus moving
btScalar toi = 1.f;
btDispatcherInfo& dispatchInfo = getDispatchInfo();
dispatchInfo.m_timeStep = timeStep;
dispatchInfo.m_timeOfImpact = 1.f;
dispatchInfo.m_stepCount = 0;
dispatchInfo.m_dispatchFunc = btDispatcherInfo::DISPATCH_CONTINUOUS;
///calculate time of impact for overlapping pairs
btDispatcher* dispatcher = getDispatcher();
if (dispatcher)
dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1);
toi = dispatchInfo.m_timeOfImpact;
dispatchInfo.m_dispatchFunc = btDispatcherInfo::DISPATCH_DISCRETE;
}
void btGpuDemoDynamicsWorld::solveConstraintsCPU2(btContactSolverInfo& solverInfo)
{
BT_PROFILE("solveConstraints");
grabData();
createBatches2();
btCudaPartProps partProps;
{
BT_PROFILE("set constraint data CPU");
partProps.m_mass = 1.0f;
partProps.m_diameter = m_objRad * 2.0f;
partProps.m_restCoeff = 1.0f;
btGpu_clearAccumulationOfLambdaDt(m_hLambdaDtBox, m_totalNumConstraints, m_maxVtxPerObj * 2);
if(!m_useBulletNarrowphase)
{
btGpu_setConstraintData(m_hIds, m_totalNumConstraints - m_numNonContactConstraints, m_numObj + 1, m_hPos, m_hRot,m_hShapeBuffer, m_hShapeIds,
partProps, m_hContact);
}
}
btCudaBoxProps boxProps;
boxProps.minX = m_worldMin[0];
boxProps.maxX = m_worldMax[0];
boxProps.minY = m_worldMin[1];
boxProps.maxY = m_worldMax[1];
{
BT_PROFILE("btCuda_collisionBatchResolutionBox CPU");
int nIter=getSolverInfo().m_numIterations;
btDispatcherInfo& dispatchInfo = getDispatchInfo();
btScalar timeStep = dispatchInfo.m_timeStep;
for(int i=0;i<nIter;i++){
btGpu_collisionWithWallBox(m_hPos, m_hVel, m_hRot, m_hAngVel,m_hShapeBuffer, m_hShapeIds,
m_hInvMass, partProps, boxProps, m_numObj + 1, timeStep);
int* pBatchIds = m_hBatchIds;
for(int iBatch=0;iBatch < m_maxBatches;iBatch++)
{
int numContConstraints = m_numInBatches[iBatch];
if(!numContConstraints)
{
break;
}
btGpu_collisionBatchResolutionBox( m_hIds, pBatchIds, numContConstraints, m_numObj + 1,
m_hPos, m_hVel,
m_hRot, m_hAngVel,
m_hLambdaDtBox,
m_hContact,
m_hInvMass,
partProps, iBatch, timeStep);
pBatchIds += numContConstraints;
}
}
}
writebackData();
m_numSimStep++;
}
void btCollisionWorld::performDiscreteCollisionDetection()
{
btDispatcherInfo& dispatchInfo = getDispatchInfo();
BEGIN_PROFILE("perform Broadphase Collision Detection");
//update aabb (of all moved objects)
btVector3 aabbMin,aabbMax;
for (int i=0;i<m_collisionObjects.size();i++)
{
m_collisionObjects[i]->getCollisionShape()->getAabb(m_collisionObjects[i]->getWorldTransform(),aabbMin,aabbMax);
m_broadphasePairCache->setAabb(m_collisionObjects[i]->getBroadphaseHandle(),aabbMin,aabbMax);
}
m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1);
END_PROFILE("perform Broadphase Collision Detection");
BEGIN_PROFILE("performDiscreteCollisionDetection");
btDispatcher* dispatcher = getDispatcher();
if (dispatcher)
dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1);
END_PROFILE("performDiscreteCollisionDetection");
}
void btBulletPhysicsEffectsWorld::integrateTransforms(btScalar timeStep)
{
///integrate transforms
#ifdef USE_PE_GATHER_SCATTER_SPURS_TASK
if (getDispatchInfo().m_enableSPU)
{
BT_PROFILE("integrateTransformsSPU");
int numRemainingObjects = m_nonStaticRigidBodies.size();
int batchSize = PARALLEL_BATCH_SIZE;
int startIndex = 0;
while (numRemainingObjects>0)
{
int currentBatchSize = numRemainingObjects > batchSize? batchSize : numRemainingObjects;
//issue and flush is likely to be called every frame, move the construction and deletion out of the inner loop (at construction/init etc)
m_PEGatherScatterProcess->issueTask(
CMD_SAMPLE_INTEGRATE_BODIES,
&m_nonStaticRigidBodies[0],
0,
0,
startIndex,
currentBatchSize);
numRemainingObjects -= currentBatchSize;
startIndex += currentBatchSize;
}
m_PEGatherScatterProcess->flush();
} else
#endif //USE_PE_GATHER_SCATTER_SPURS_TASK
{
// BT_PROFILE("integrateTransforms");
btDiscreteDynamicsWorld::integrateTransforms(timeStep);
}
}
void btContinuousDynamicsWorld::internalSingleStepSimulation( btScalar timeStep)
{
startProfiling(timeStep);
if(0 != m_internalPreTickCallback) {
(*m_internalPreTickCallback)(this, timeStep);
}
///update aabbs information
updateAabbs();
//static int frame=0;
// printf("frame %d\n",frame++);
///apply gravity, predict motion
predictUnconstraintMotion(timeStep);
btDispatcherInfo& dispatchInfo = getDispatchInfo();
dispatchInfo.m_timeStep = timeStep;
dispatchInfo.m_stepCount = 0;
dispatchInfo.m_debugDraw = getDebugDrawer();
///perform collision detection
performDiscreteCollisionDetection();
calculateSimulationIslands();
getSolverInfo().m_timeStep = timeStep;
///solve contact and other joint constraints
solveConstraints(getSolverInfo());
///CallbackTriggers();
calculateTimeOfImpacts(timeStep);
btScalar toi = dispatchInfo.m_timeOfImpact;
// if (toi < 1.f)
// printf("toi = %f\n",toi);
if (toi < 0.f)
kdPrintf("toi = %f\n",toi);
///integrate transforms
integrateTransforms(timeStep * toi);
///update vehicle simulation
updateActions(timeStep);
updateActivationState( timeStep );
if(0 != m_internalTickCallback) {
(*m_internalTickCallback)(this, timeStep);
}
}
///contactTest performs a discrete collision test between two collision objects and calls the resultCallback if overlap if detected.
///it reports one or more contact points (including the one with deepest penetration)
void btCollisionWorld::contactPairTest(btCollisionObject* colObjA, btCollisionObject* colObjB, ContactResultCallback& resultCallback)
{
btCollisionAlgorithm* algorithm = getDispatcher()->findAlgorithm(colObjA,colObjB);
if (algorithm)
{
btBridgedManifoldResult contactPointResult(colObjA,colObjB, resultCallback);
//discrete collision detection query
algorithm->processCollision(colObjA,colObjB, getDispatchInfo(),&contactPointResult);
algorithm->~btCollisionAlgorithm();
getDispatcher()->freeCollisionAlgorithm(algorithm);
}
}
void btBulletPhysicsEffectsWorld::predictUnconstraintMotion(btScalar timeStep)
{
#ifdef USE_PE_GATHER_SCATTER_SPURS_TASK
if (getDispatchInfo().m_enableSPU)
{
BT_PROFILE("predictUnconstraintMotionSPU");
int numRemainingObjects = m_nonStaticRigidBodies.size();
int batchSize = PARALLEL_BATCH_SIZE;
int startIndex=0;
while (numRemainingObjects>0)
{
int currentBatchSize = numRemainingObjects > batchSize? batchSize : numRemainingObjects;
//issue and flush is likely to be called every frame, move the construction and deletion out of the inner loop (at construction/init etc)
m_PEGatherScatterProcess->issueTask(
CMD_SAMPLE_PREDICT_MOTION_BODIES,
&m_nonStaticRigidBodies[0],
0,
0,
startIndex,
currentBatchSize);
numRemainingObjects -= currentBatchSize;
startIndex += currentBatchSize;
}
m_PEGatherScatterProcess->flush();
}
else
#endif //USE_PE_GATHER_SCATTER_SPURS_TASK
{
btDiscreteDynamicsWorld::predictUnconstraintMotion( timeStep);
}
for ( int i=0;i<m_nonStaticRigidBodies.size();i++)
{
btRigidBody* body = m_nonStaticRigidBodies[i];
body->setHitFraction(1.f);
if (body->isActive() && (!body->isStaticOrKinematicObject()))
{
syncRigidBodyState(body);
}
}
}
int btSimpleDynamicsWorld::stepSimulation( btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep, btScalar fixedSubSteps)
{
(void)fixedTimeStep;
(void)maxSubSteps;
(void)fixedSubSteps;
///apply gravity, predict motion
predictUnconstraintMotion(timeStep);
btDispatcherInfo& dispatchInfo = getDispatchInfo();
dispatchInfo.m_timeStep = timeStep;
dispatchInfo.m_stepCount = 0;
dispatchInfo.m_debugDraw = getDebugDrawer();
///perform collision detection
performDiscreteCollisionDetection();
///solve contact constraints
int numManifolds = m_dispatcher1->getNumManifolds();
if (numManifolds)
{
btPersistentManifold** manifoldPtr = ((btCollisionDispatcher*)m_dispatcher1)->getInternalManifoldPointer();
btContactSolverInfo infoGlobal;
infoGlobal.m_timeStep = timeStep;
m_constraintSolver->prepareSolve(0, numManifolds);
m_constraintSolver->solveGroup(&getCollisionObjectArray()[0], getNumCollisionObjects(), manifoldPtr, numManifolds,0,0, infoGlobal, m_debugDrawer, m_dispatcher1);
m_constraintSolver->allSolved(infoGlobal, m_debugDrawer);
}
///integrate transforms
integrateTransforms(timeStep);
updateAabbs();
synchronizeMotionStates();
clearForces();
return 1;
}
void btCollisionWorld::updateSingleAabb(btCollisionObject* colObj)
{
btVector3 minAabb,maxAabb;
colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
//need to increase the aabb for contact thresholds
btVector3 contactThreshold(gContactBreakingThreshold,gContactBreakingThreshold,gContactBreakingThreshold);
minAabb -= contactThreshold;
maxAabb += contactThreshold;
if(getDispatchInfo().m_convexMaxDistanceUseCPT)
{
btVector3 minAabb2,maxAabb2;
colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2);
minAabb2 -= contactThreshold;
maxAabb2 += contactThreshold;
minAabb.setMin(minAabb2);
maxAabb.setMax(maxAabb2);
}
btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache;
//moving objects should be moderately sized, probably something wrong if not
if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12)))
{
bp->setAabb(colObj->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
} else
{
//something went wrong, investigate
//this assert is unwanted in 3D modelers (danger of loosing work)
colObj->setActivationState(DISABLE_SIMULATION);
static bool reportMe = true;
if (reportMe && m_debugDrawer)
{
reportMe = false;
m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation");
m_debugDrawer->reportErrorWarning("If you can reproduce this, please email [email protected]\n");
m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n");
m_debugDrawer->reportErrorWarning("Thanks.\n");
}
}
}
void btCollisionWorld::performDiscreteCollisionDetection()
{
BT_PROFILE("performDiscreteCollisionDetection");
btDispatcherInfo& dispatchInfo = getDispatchInfo();
updateAabbs();
{
BT_PROFILE("calculateOverlappingPairs");
m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1);
}
btDispatcher* dispatcher = getDispatcher();
{
BT_PROFILE("dispatchAllCollisionPairs");
if (dispatcher)
dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1);
}
}
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
//don't apply the collision response right now, it will happen next frame
//if you really need to, you can uncomment next 3 lines. Note that is uses zero restitution.
//btScalar appliedImpulse = 0.f;
//btScalar depth = 0.f;
//appliedImpulse = resolveSingleCollision(body,(btCollisionObject*)sweepResults.m_hitCollisionObject,sweepResults.m_hitPointWorld,sweepResults.m_hitNormalWorld,getSolverInfo(), depth);
#endif
continue;
}
}
}
body->proceedToTransform( predictedTrans);
}
}
///this should probably be switched on by default, but it is not well tested yet
if (m_applySpeculativeContactRestitution)
{
BT_PROFILE("apply speculative contact restitution");
for (int i=0;i<m_predictiveManifolds.size();i++)
{
btPersistentManifold* manifold = m_predictiveManifolds[i];
btRigidBody* body0 = btRigidBody::upcast((btCollisionObject*)manifold->getBody0());
btRigidBody* body1 = btRigidBody::upcast((btCollisionObject*)manifold->getBody1());
for (int p=0;p<manifold->getNumContacts();p++)
{
const btManifoldPoint& pt = manifold->getContactPoint(p);
btScalar combinedRestitution = btManifoldResult::calculateCombinedRestitution(body0, body1);
if (combinedRestitution>0 && pt.m_appliedImpulse != 0.f)
//if (pt.getDistance()>0 && combinedRestitution>0 && pt.m_appliedImpulse != 0.f)
{
btVector3 imp = -pt.m_normalWorldOnB * pt.m_appliedImpulse* combinedRestitution;
const btVector3& pos1 = pt.getPositionWorldOnA();
const btVector3& pos2 = pt.getPositionWorldOnB();
btVector3 rel_pos0 = pos1 - body0->getWorldTransform().getOrigin();
btVector3 rel_pos1 = pos2 - body1->getWorldTransform().getOrigin();
if (body0)
body0->applyImpulse(imp,rel_pos0);
if (body1)
body1->applyImpulse(-imp,rel_pos1);
}
}
}
}
}
void btDiscreteDynamicsWorld::createPredictiveContacts(btScalar timeStep)
{
BT_PROFILE("createPredictiveContacts");
{
BT_PROFILE("release predictive contact manifolds");
for (int i=0;i<m_predictiveManifolds.size();i++)
{
btPersistentManifold* manifold = m_predictiveManifolds[i];
this->m_dispatcher1->releaseManifold(manifold);
}
m_predictiveManifolds.clear();
}
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("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);
m_predictiveManifolds.push_back(manifold);
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 = btManifoldResult::calculateCombinedFriction(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);
}
}
}
void btGpuDemoDynamicsWorld::solveConstraints2(btContactSolverInfo& solverInfo)
{
//#ifdef BT_USE_CUDA
#if 1
BT_PROFILE("solveConstraints");
grabData();
createBatches2();
copyDataToGPU();
btCudaPartProps partProps;
setConstraintData(partProps);
btCudaBoxProps boxProps;
boxProps.minX = m_worldMin[0];
boxProps.maxX = m_worldMax[0];
boxProps.minY = m_worldMin[1];
boxProps.maxY = m_worldMax[1];
btVector3 hParams[2];
hParams[0] = m_worldMin;
hParams[1] = m_worldMax;
btGpuDemo2dOCLWrap::copyArrayToDevice(btGpuDemo2dOCLWrap::m_dParams, hParams, sizeof(float) * 4 * 2);
{
BT_PROFILE("btCuda_collisionBatchResolutionBox");
int nIter=getSolverInfo().m_numIterations;
btDispatcherInfo& dispatchInfo = getDispatchInfo();
btScalar timeStep = dispatchInfo.m_timeStep;
btGpuDemo2dOCLWrap::setKernelArg(GPUDEMO2D_KERNEL_COLLISION_WITH_WALL, 1, sizeof(cl_mem), (void*)&btGpuDemo2dOCLWrap::m_dcPos);
btGpuDemo2dOCLWrap::setKernelArg(GPUDEMO2D_KERNEL_COLLISION_WITH_WALL, 2, sizeof(cl_mem), (void*)&btGpuDemo2dOCLWrap::m_dcVel);
btGpuDemo2dOCLWrap::setKernelArg(GPUDEMO2D_KERNEL_COLLISION_WITH_WALL, 3, sizeof(cl_mem), (void*)&btGpuDemo2dOCLWrap::m_dcRot);
btGpuDemo2dOCLWrap::setKernelArg(GPUDEMO2D_KERNEL_COLLISION_WITH_WALL, 4, sizeof(cl_mem), (void*)&btGpuDemo2dOCLWrap::m_dcAngVel);
btGpuDemo2dOCLWrap::setKernelArg(GPUDEMO2D_KERNEL_COLLISION_WITH_WALL, 9, sizeof(float), (void*)&timeStep);
btGpuDemo2dOCLWrap::setKernelArg(GPUDEMO2D_KERNEL_SOLVE_CONSTRAINTS, 3, sizeof(cl_mem), (void*)&btGpuDemo2dOCLWrap::m_dcPos);
btGpuDemo2dOCLWrap::setKernelArg(GPUDEMO2D_KERNEL_SOLVE_CONSTRAINTS, 4, sizeof(cl_mem), (void*)&btGpuDemo2dOCLWrap::m_dcVel);
btGpuDemo2dOCLWrap::setKernelArg(GPUDEMO2D_KERNEL_SOLVE_CONSTRAINTS, 5, sizeof(cl_mem), (void*)&btGpuDemo2dOCLWrap::m_dcRot);
btGpuDemo2dOCLWrap::setKernelArg(GPUDEMO2D_KERNEL_SOLVE_CONSTRAINTS, 6, sizeof(cl_mem), (void*)&btGpuDemo2dOCLWrap::m_dcAngVel);
btGpuDemo2dOCLWrap::setKernelArg(GPUDEMO2D_KERNEL_SOLVE_CONSTRAINTS, 12, sizeof(float), (void*)&timeStep);
for(int i=0;i<nIter;i++)
{
// btCuda_collisionWithWallBox(m_dcPos, m_dcVel, m_dcRot, m_dcAngVel,m_dShapeBuffer, m_dShapeIds, m_dInvMass,
// partProps, boxProps, m_numObj + 1, timeStep);
btGpuDemo2dOCLWrap::runKernelWithWorkgroupSize(GPUDEMO2D_KERNEL_COLLISION_WITH_WALL, m_numObj + 1);
// int* pBatchIds = m_dBatchIds;
int batchOffs = 0;
for(int iBatch=0;iBatch < m_maxBatches;iBatch++)
{
int numConstraints = m_numInBatches[iBatch];
/*
btCuda_collisionBatchResolutionBox( m_dIds, pBatchIds, numConstraints, m_numObj + 1,
m_dcPos, m_dcVel,
m_dcRot, m_dcAngVel,
m_dLambdaDtBox,
m_dContact, m_dInvMass,
partProps, iBatch, timeStep);
*/
btGpuDemo2dOCLWrap::setKernelArg(GPUDEMO2D_KERNEL_SOLVE_CONSTRAINTS, 10, sizeof(int), (void*)&iBatch);
btGpuDemo2dOCLWrap::setKernelArg(GPUDEMO2D_KERNEL_SOLVE_CONSTRAINTS, 11, sizeof(int), (void*)&batchOffs);
btGpuDemo2dOCLWrap::runKernelWithWorkgroupSize(GPUDEMO2D_KERNEL_SOLVE_CONSTRAINTS, numConstraints);
// pBatchIds += numConstraints;
batchOffs += numConstraints;
}
}
}
copyDataFromGPU();
writebackData();
m_numSimStep++;
#endif //BT_USE_CUDA
}
