Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Implemented btDiscreteDynamicsWorld::addSpeculativeContacts, using conservative advancement to find contact point ahead of time

Browse Source 
make Extras/ConvexDecomposition thread safe, Issue 501
some improvements to the btInternalEdgeUtility, patch from Issue 501
This commit is contained in:
erwin.coumans
2011-04-07 06:23:34 +00:00
parent 49630e9c77
commit a4e8213ede
8 changed files with 304 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files

124
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
View File

@@ -46,6 +46,13 @@ subject to the following restrictions:
#include "LinearMath/btSerializer.h"
#if 0
btAlignedObjectArray<btVector3> debugContacts;
btAlignedObjectArray<btVector3> debugNormals;
int startHit=2;
int firstHit=startHit;
#endif
btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
@@ -314,9 +321,13 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
dispatchInfo.m_stepCount = 0;
dispatchInfo.m_debugDraw = getDebugDrawer();
///perform collision detection
performDiscreteCollisionDetection();
addSpeculativeContacts(timeStep);
calculateSimulationIslands();
@@ -745,12 +756,13 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands()
class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
{
public:
btCollisionObject* m_me;
btScalar m_allowedPenetration;
btOverlappingPairCache* m_pairCache;
btDispatcher* m_dispatcher;
public:
btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :
btCollisionWorld::ClosestConvexResultCallback(fromA,toA),
@@ -828,7 +840,6 @@ public:
///internal debugging variable. this value shouldn't be too high
int gNumClampedCcdMotions=0;
//#include "stdio.h"
void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
{
BT_PROFILE("integrateTransforms");
@@ -875,6 +886,115 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
}
}
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");
}
}
}
}
}
}
}