diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index 2185b342a..d60c01184 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -26,7 +26,7 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" #include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h" #include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h" - +#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h" #include "LinearMath/btAabbUtil2.h" #include "LinearMath/btQuickprof.h" @@ -908,6 +908,115 @@ void btCollisionWorld::convexSweepTest(const btConvexShape* castShape, const btT +struct btBridgedManifoldResult : public btManifoldResult +{ + + btCollisionWorld::ContactResultCallback& m_resultCallback; + + btBridgedManifoldResult( btCollisionObject* obj0,btCollisionObject* obj1,btCollisionWorld::ContactResultCallback& resultCallback ) + :btManifoldResult(obj0,obj1), + m_resultCallback(resultCallback) + { + } + + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) + { + bool isSwapped = m_manifoldPtr->getBody0() != m_body0; + btVector3 pointA = pointInWorld + normalOnBInWorld * depth; + btVector3 localA; + btVector3 localB; + if (isSwapped) + { + localA = m_rootTransB.invXform(pointA ); + localB = m_rootTransA.invXform(pointInWorld); + } else + { + localA = m_rootTransA.invXform(pointA ); + localB = m_rootTransB.invXform(pointInWorld); + } + + btManifoldPoint newPt(localA,localB,normalOnBInWorld,depth); + newPt.m_positionWorldOnA = pointA; + newPt.m_positionWorldOnB = pointInWorld; + + //BP mod, store contact triangles. + if (isSwapped) + { + newPt.m_partId0 = m_partId1; + newPt.m_partId1 = m_partId0; + newPt.m_index0 = m_index1; + newPt.m_index1 = m_index0; + } else + { + newPt.m_partId0 = m_partId0; + newPt.m_partId1 = m_partId1; + newPt.m_index0 = m_index0; + newPt.m_index1 = m_index1; + } + + //experimental feature info, for per-triangle material etc. + btCollisionObject* obj0 = isSwapped? m_body1 : m_body0; + btCollisionObject* obj1 = isSwapped? m_body0 : m_body1; + m_resultCallback.addSingleResult(newPt,obj0,newPt.m_partId0,newPt.m_index0,obj1,newPt.m_partId1,newPt.m_index1); + + } + +}; + + + +struct btSingleContactCallback : public btBroadphaseRayCallback +{ + + btCollisionObject* m_collisionObject; + btCollisionWorld* m_world; + btCollisionWorld::ContactResultCallback& m_resultCallback; + + + btSingleContactCallback(btCollisionObject* collisionObject, btCollisionWorld* world,btCollisionWorld::ContactResultCallback& resultCallback) + :m_collisionObject(collisionObject), + m_world(world), + m_resultCallback(resultCallback) + { + } + + virtual bool process(const btBroadphaseProxy* proxy) + { + btCollisionObject* collisionObject = (btCollisionObject*)proxy->m_clientObject; + if (collisionObject == m_collisionObject) + return true; + + //only perform raycast if filterMask matches + if(m_resultCallback.needsCollision(collisionObject->getBroadphaseHandle())) + { + btCollisionAlgorithm* algorithm = m_world->getDispatcher()->findAlgorithm(m_collisionObject,collisionObject); + if (algorithm) + { + btBridgedManifoldResult contactPointResult(m_collisionObject,collisionObject, m_resultCallback); + //discrete collision detection query + algorithm->processCollision(m_collisionObject,collisionObject, m_world->getDispatchInfo(),&contactPointResult); + + algorithm->~btCollisionAlgorithm(); + m_world->getDispatcher()->freeCollisionAlgorithm(algorithm); + } + } + return true; + } +}; + + +///contactTest performs a discrete collision test against all objects in the btCollisionWorld, and calls the resultCallback. +///it reports one or more contact points for every overlapping object (including the one with deepest penetration) +void btCollisionWorld::contactTest( btCollisionObject* colObj, ContactResultCallback& resultCallback) +{ + btVector3 aabbMin,aabbMax; + colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(),aabbMin,aabbMax); + btSingleContactCallback contactCB(colObj,this,resultCallback); + + m_broadphasePairCache->rayTest(colObj->getWorldTransform().getOrigin(),colObj->getWorldTransform().getOrigin(),contactCB,aabbMin,aabbMax); +} + + class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback { diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index f199a66cd..630b255f9 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -358,6 +358,34 @@ public: } }; + ///ContactResultCallback is used to report contact points + struct ContactResultCallback + { + short int m_collisionFilterGroup; + short int m_collisionFilterMask; + + ContactResultCallback() + :m_collisionFilterGroup(btBroadphaseProxy::DefaultFilter), + m_collisionFilterMask(btBroadphaseProxy::AllFilter) + { + } + + virtual ~ContactResultCallback() + { + } + + virtual bool needsCollision(btBroadphaseProxy* proxy0) const + { + bool collides = (proxy0->m_collisionFilterGroup & m_collisionFilterMask) != 0; + collides = collides && (m_collisionFilterGroup & proxy0->m_collisionFilterMask); + return collides; + } + + virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObject* colObj0,int partId0,int index0,const btCollisionObject* colObj1,int partId1,int index1) = 0; + }; + + + int getNumCollisionObjects() const { return int(m_collisionObjects.size()); @@ -367,10 +395,13 @@ public: /// This allows for several queries: first hit, all hits, any hit, dependent on the value returned by the callback. virtual void rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const; - // convexTest performs a swept convex cast on all objects in the btCollisionWorld, and calls the resultCallback - // This allows for several queries: first hit, all hits, any hit, dependent on the value return by the callback. + /// convexTest performs a swept convex cast on all objects in the btCollisionWorld, and calls the resultCallback + /// This allows for several queries: first hit, all hits, any hit, dependent on the value return by the callback. void convexSweepTest (const btConvexShape* castShape, const btTransform& from, const btTransform& to, ConvexResultCallback& resultCallback, btScalar allowedCcdPenetration = btScalar(0.)) const; + ///contactTest performs a discrete collision test between colObj against all objects in the btCollisionWorld, and calls the resultCallback. + ///it reports one or more contact points for every overlapping object (including the one with deepest penetration) + void contactTest(btCollisionObject* colObj, ContactResultCallback& resultCallback); /// rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest. /// In a future implementation, we consider moving the ray test as a virtual method in btCollisionShape. diff --git a/src/BulletCollision/CollisionDispatch/btManifoldResult.h b/src/BulletCollision/CollisionDispatch/btManifoldResult.h index 978033997..927e2bc4f 100644 --- a/src/BulletCollision/CollisionDispatch/btManifoldResult.h +++ b/src/BulletCollision/CollisionDispatch/btManifoldResult.h @@ -34,6 +34,8 @@ extern ContactAddedCallback gContactAddedCallback; ///btManifoldResult is a helper class to manage contact results. class btManifoldResult : public btDiscreteCollisionDetectorInterface::Result { +protected: + btPersistentManifold* m_manifoldPtr; //we need this for compounds