btCollisionWorld::~btCollisionWorld()
{
m_stackAlloc->destroy();
delete m_stackAlloc;
//clean up remaining objects
int i;
for (i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* collisionObject= m_collisionObjects[i];
btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
if (bp)
{
//
// only clear the cached algorithms
//
getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
getBroadphase()->destroyProxy(bp,m_dispatcher1);
}
}
if (m_ownsDispatcher)
delete m_dispatcher1;
if (m_ownsBroadphasePairCache)
delete m_broadphasePairCache;
}
void btCollisionWorld::removeCollisionObject(btCollisionObject* collisionObject)
{
//bool removeFromBroadphase = false;
{
btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
if (bp)
{
//
// only clear the cached algorithms
//
getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
getBroadphase()->destroyProxy(bp,m_dispatcher1);
collisionObject->setBroadphaseHandle(0);
}
}
//swapremove
m_collisionObjects.remove(collisionObject);
}
void DynamicsWorld::reset()
{
getBroadphase()->resetPool(getDispatcher());
m_nonStaticRigidBodies.resize(0);
m_collisionObjects.resize(0);
track = 0;
}
void btCollisionWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)
{
//check that the object isn't already added
btAssert( m_collisionObjects.findLinearSearch(collisionObject) == m_collisionObjects.size());
m_collisionObjects.push_back(collisionObject);
//calculate new AABB
btTransform trans = collisionObject->getWorldTransform();
btVector3 minAabb;
btVector3 maxAabb;
collisionObject->getCollisionShape()->getAabb(trans,minAabb,maxAabb);
int type = collisionObject->getCollisionShape()->getShapeType();
collisionObject->setBroadphaseHandle( getBroadphase()->createProxy(
minAabb,
maxAabb,
type,
collisionObject,
collisionFilterGroup,
collisionFilterMask
)) ;
}
void World::reset()
{
getBroadphase()->resetPool(getDispatcher());
m_nonStaticRigidBodies.resize(0);
m_collisionObjects.resize(0);
//rayTestProc = 0;
}
void ForkLiftDemo::resetForklift()
{
gVehicleSteering = 0.f;
gBreakingForce = defaultBreakingForce;
gEngineForce = 0.f;
m_carChassis->setCenterOfMassTransform(btTransform::getIdentity());
m_carChassis->setLinearVelocity(btVector3(0,0,0));
m_carChassis->setAngularVelocity(btVector3(0,0,0));
m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(m_carChassis->getBroadphaseHandle(),getDynamicsWorld()->getDispatcher());
if (m_vehicle)
{
m_vehicle->resetSuspension();
for (int i=0;i<m_vehicle->getNumWheels();i++)
{
//synchronize the wheels with the (interpolated) chassis worldtransform
m_vehicle->updateWheelTransform(i,true);
}
}
btTransform liftTrans;
liftTrans.setIdentity();
liftTrans.setOrigin(m_liftStartPos);
m_liftBody->activate();
m_liftBody->setCenterOfMassTransform(liftTrans);
m_liftBody->setLinearVelocity(btVector3(0,0,0));
m_liftBody->setAngularVelocity(btVector3(0,0,0));
btTransform forkTrans;
forkTrans.setIdentity();
forkTrans.setOrigin(m_forkStartPos);
m_forkBody->activate();
m_forkBody->setCenterOfMassTransform(forkTrans);
m_forkBody->setLinearVelocity(btVector3(0,0,0));
m_forkBody->setAngularVelocity(btVector3(0,0,0));
// m_liftHinge->setLimit(-LIFT_EPS, LIFT_EPS);
m_liftHinge->setLimit(0.0f, 0.0f);
m_liftHinge->enableAngularMotor(false, 0, 0);
m_forkSlider->setLowerLinLimit(0.1f);
m_forkSlider->setUpperLinLimit(0.1f);
m_forkSlider->setPoweredLinMotor(false);
btTransform loadTrans;
loadTrans.setIdentity();
loadTrans.setOrigin(m_loadStartPos);
m_loadBody->activate();
m_loadBody->setCenterOfMassTransform(loadTrans);
m_loadBody->setLinearVelocity(btVector3(0,0,0));
m_loadBody->setAngularVelocity(btVector3(0,0,0));
}
btCollisionWorld::~btCollisionWorld()
{
//clean up remaining objects
int i;
for (i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* collisionObject= m_collisionObjects[i];
btBroadphaseProxy* bp = collisionObject->getBroadphaseHandle();
if (bp)
{
//
// only clear the cached algorithms
//
getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(bp,m_dispatcher1);
getBroadphase()->destroyProxy(bp,m_dispatcher1);
collisionObject->setBroadphaseHandle(0);
}
}
}
void btSimpleDynamicsWorld::updateAabbs()
{
btTransform predictedTrans;
for ( int i=0;i<m_collisionObjects.size();i++)
{
btCollisionObject* colObj = m_collisionObjects[i];
btRigidBody* body = btRigidBody::upcast(colObj);
if (body)
{
if (body->isActive() && (!body->isStaticObject()))
{
btVector3 minAabb,maxAabb;
colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
btBroadphaseInterface* bp = getBroadphase();
bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb, m_dispatcher1);
}
}
}
}
//#include "stdio.h"
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 (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
{
BT_PROFILE("CCD motion clamping");
if (body->getCollisionShape()->isConvex())
{
gNumClampedCcdMotions++;
btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask;
convexSweepTest(&tmpSphere,body->getWorldTransform(),predictedTrans,sweepResults);
if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
{
body->setHitFraction(sweepResults.m_closestHitFraction);
body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
body->setHitFraction(0.f);
// printf("clamped integration to hit fraction = %f\n",fraction);
}
}
}
body->proceedToTransform( predictedTrans);
}
}
}
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::addSpeculativeContacts(btScalar timeStep)
{
BT_PROFILE("addSpeculativeContacts");
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 (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
{
BT_PROFILE("search speculative contacts");
if (body->getCollisionShape()->isConvex())
{
gNumClampedCcdMotions++;
btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask;
btTransform modifiedPredictedTrans;
modifiedPredictedTrans = predictedTrans;
modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis());
convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults);
if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
{
btBroadphaseProxy* proxy0 = body->getBroadphaseHandle();
btBroadphaseProxy* proxy1 = sweepResults.m_hitCollisionObject->getBroadphaseHandle();
btBroadphasePair* pair = sweepResults.m_pairCache->findPair(proxy0,proxy1);
if (pair)
{
if (pair->m_algorithm)
{
btManifoldArray contacts;
pair->m_algorithm->getAllContactManifolds(contacts);
if (contacts.size())
{
btManifoldResult result(body,sweepResults.m_hitCollisionObject);
result.setPersistentManifold(contacts[0]);
btVector3 vec = (modifiedPredictedTrans.getOrigin()-body->getWorldTransform().getOrigin());
vec*=sweepResults.m_closestHitFraction;
btScalar lenSqr = vec.length2();
btScalar depth = 0.f;
btVector3 pointWorld = sweepResults.m_hitPointWorld;
if (lenSqr>SIMD_EPSILON)
{
depth = btSqrt(lenSqr);
pointWorld -= vec;
vec /= depth;
}
if (contacts[0]->getBody0()==body)
{
result.addContactPoint(sweepResults.m_hitNormalWorld,pointWorld,depth);
#if 0
debugContacts.push_back(sweepResults.m_hitPointWorld);//sweepResults.m_hitPointWorld);
debugNormals.push_back(sweepResults.m_hitNormalWorld);
#endif
} else
{
//swapped
result.addContactPoint(-sweepResults.m_hitNormalWorld,pointWorld,depth);
//sweepResults.m_hitPointWorld,depth);
#if 0
if (1)//firstHit==1)
{
firstHit=0;
debugNormals.push_back(sweepResults.m_hitNormalWorld);
debugContacts.push_back(pointWorld);//sweepResults.m_hitPointWorld);
debugNormals.push_back(sweepResults.m_hitNormalWorld);
debugContacts.push_back(sweepResults.m_hitPointWorld);
}
firstHit--;
#endif
}
}
} else
{
//no algorithm, use dispatcher to create one
}
} else
{
//add an overlapping pair
//printf("pair missing\n");
}
}
}
}
}
}
}
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);
}
}
}
