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

96
src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp
View File

@@ -1,6 +1,7 @@
#include "btInternalEdgeUtility.h"
#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
#include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h"
#include "BulletCollision/CollisionShapes/btTriangleShape.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
@@ -9,7 +10,6 @@
//#define DEBUG_INTERNAL_EDGE
#ifdef DEBUG_INTERNAL_EDGE
#include <stdio.h>
#endif //DEBUG_INTERNAL_EDGE
@@ -456,8 +456,14 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject*
if (colObj0->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE)
return;
btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)colObj0->getRootCollisionShape();
btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap();
btBvhTriangleMeshShape* trimesh = 0;
if( colObj0->getRootCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE )
trimesh = ((btScaledBvhTriangleMeshShape*)colObj0->getRootCollisionShape())->getChildShape();
else
trimesh = (btBvhTriangleMeshShape*)colObj0->getRootCollisionShape();
btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap();
if (!triangleInfoMapPtr)
return;
@@ -501,14 +507,63 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject*
btVector3 localContactNormalOnB = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB;
localContactNormalOnB.normalize();//is this necessary?
if ((info->m_edgeV0V1Angle)< SIMD_2_PI)
// Get closest edge
int bestedge=-1;
float disttobestedge=BT_LARGE_FLOAT;
//
// Edge 0 -> 1
if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
{
btVector3 nearest;
btNearestPointInLineSegment( cp.m_localPointB, v0, v1, nearest );
float len=(contact-nearest).length();
//
if( len < disttobestedge )
{
bestedge=0;
disttobestedge=len;
}
}
// Edge 1 -> 2
if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
{
btVector3 nearest;
btNearestPointInLineSegment( cp.m_localPointB, v1, v2, nearest );
float len=(contact-nearest).length();
//
if( len < disttobestedge )
{
bestedge=1;
disttobestedge=len;
}
}
// Edge 2 -> 0
if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
{
btVector3 nearest;
btNearestPointInLineSegment( cp.m_localPointB, v2, v0, nearest );
float len=(contact-nearest).length();
//
if( len < disttobestedge )
{
bestedge=2;
disttobestedge=len;
}
}
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btVector3 upfix=tri_normal * btVector3(0.1f,0.1f,0.1f);
btDebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red );
#endif
//if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
{
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
#endif
btScalar len = (contact-nearest).length();
if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
//if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
if( bestedge==0 )
{
btVector3 edge(v0-v1);
isNearEdge = true;
@@ -577,7 +632,11 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject*
btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,green);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
if ((info->m_edgeV1V2Angle)< SIMD_2_PI)
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr * v1 + upfix, tr * v2 + upfix , green );
#endif
//if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
{
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black);
@@ -586,7 +645,8 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject*
btScalar len = (contact-nearest).length();
if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
//if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
if( bestedge==1 )
{
isNearEdge = true;
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
@@ -658,8 +718,11 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject*
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,blue);
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
btDebugDrawLine(tr * v2 + upfix, tr * v0 + upfix , blue );
#endif
if ((info->m_edgeV2V0Angle)< SIMD_2_PI)
//if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold)
{
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
@@ -667,7 +730,8 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject*
#endif //BT_INTERNAL_EDGE_DEBUG_DRAW
btScalar len = (contact-nearest).length();
if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
//if(len<triangleInfoMapPtr->m_edgeDistanceThreshold)
if( bestedge==2 )
{
isNearEdge = true;
#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW
@@ -759,11 +823,17 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject*
cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis()*tri_normal;
} else
{
btVector3 newNormal = tri_normal *frontFacing;
//if the tri_normal is pointing opposite direction as the current local contact normal, skip it
btScalar d = newNormal.dot(localContactNormalOnB) ;
if (d< 0)
{
return;
}
//modify the normal to be the triangle normal (or backfacing normal)
cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis() *(tri_normal *frontFacing);
cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis() *newNormal;
}
// Reproject collision point along normal.
cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1;
cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB);

2
src/BulletCollision/CollisionShapes/btTriangleInfoMap.h
View File

@@ -61,6 +61,7 @@ struct btTriangleInfoMap : public btInternalTriangleInfoMap
btScalar m_planarEpsilon; ///used to determine if a triangle edge is planar with zero angle
btScalar m_equalVertexThreshold; ///used to compute connectivity: if the distance between two vertices is smaller than m_equalVertexThreshold, they are considered to be 'shared'
btScalar m_edgeDistanceThreshold; ///used to determine edge contacts: if the closest distance between a contact point and an edge is smaller than this distance threshold it is considered to "hit the edge"
btScalar m_maxEdgeAngleThreshold; //ignore edges that connect triangles at an angle larger than this m_maxEdgeAngleThreshold
btScalar m_zeroAreaThreshold; ///used to determine if a triangle is degenerate (length squared of cross product of 2 triangle edges < threshold)
@@ -71,6 +72,7 @@ struct btTriangleInfoMap : public btInternalTriangleInfoMap
m_equalVertexThreshold = btScalar(0.0001)*btScalar(0.0001);
m_edgeDistanceThreshold = btScalar(0.1);
m_zeroAreaThreshold = btScalar(0.0001)*btScalar(0.0001);
m_maxEdgeAngleThreshold = SIMD_2_PI;
}
virtual ~btTriangleInfoMap() {}

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");
}
}
}
}
}
}
}

2
src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h
View File

@@ -62,6 +62,8 @@ protected:
virtual void integrateTransforms(btScalar timeStep);
virtual void addSpeculativeContacts(btScalar timeStep);
virtual void calculateSimulationIslands();
virtual void solveConstraints(btContactSolverInfo& solverInfo);