Enable rayTest btDbvt (dynamic aabb-tree) acceleration for btCompoundShape

Thanks to NaN/core13, see Issue 25

Demos/ConvexDecompositionDemo/ConvexDecompositionDemo.cpp

@@ -455,7 +455,7 @@ void ConvexDecompositionDemo::initPhysics(const char* filename)
 		
 		if (1)
 		{
-			btCompoundShape* compound = new btCompoundShape(false);
+			btCompoundShape* compound = new btCompoundShape();
 			m_collisionShapes.push_back (compound);
 
 			btTransform trans;

src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp

@@ -28,11 +28,15 @@ subject to the following restrictions:
 #include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
 #include "BulletCollision/BroadphaseCollision/btBroadphaseInterface.h"
+#include "BulletCollision/BroadphaseCollision/btDbvt.h"
 #include "LinearMath/btAabbUtil2.h"
 #include "LinearMath/btQuickprof.h"
 #include "LinearMath/btStackAlloc.h"
 #include "LinearMath/btSerializer.h"
 
+//#define DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
+
+
 //#define USE_BRUTEFORCE_RAYBROADPHASE 1
 //RECALCULATE_AABB is slower, but benefit is that you don't need to call 'stepSimulation'  or 'updateAabbs' before using a rayTest
 //#define RECALCULATE_AABB_RAYCAST 1
@@ -420,51 +424,113 @@ void	btCollisionWorld::rayTestSingle(const btTransform& rayFromTrans,const btTra
 			}
 		} else {
 			//			BT_PROFILE("rayTestCompound");
-			///@todo: use AABB tree or other BVH acceleration structure, see btDbvt
 			if (collisionShape->isCompound())
 			{
-				const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
+				struct LocalInfoAdder2 : public RayResultCallback
-				int i=0;
-				for (i=0;i<compoundShape->getNumChildShapes();i++)
 				{
-					btTransform childTrans = compoundShape->getChildTransform(i);
+					RayResultCallback* m_userCallback;
-					const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
+					int m_i;
-					btTransform childWorldTrans = colObjWorldTransform * childTrans;
+					
-					// replace collision shape so that callback can determine the triangle
+					LocalInfoAdder2 (int i, RayResultCallback *user)
-					btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape();
+						: m_userCallback(user), m_i(i)
-					collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
+					{ 
-                    struct LocalInfoAdder2 : public RayResultCallback {
+						m_closestHitFraction = m_userCallback->m_closestHitFraction;
-						RayResultCallback* m_userCallback;
+					}
-						int m_i;
+					
-                        LocalInfoAdder2 (int i, RayResultCallback *user)
+					virtual btScalar addSingleResult (btCollisionWorld::LocalRayResult &r, bool b)
-							: m_userCallback(user),
+					{
-							m_i(i)
+						btCollisionWorld::LocalShapeInfo shapeInfo;
-						{ 
+						shapeInfo.m_shapePart = -1;
-							m_closestHitFraction = m_userCallback->m_closestHitFraction;
+						shapeInfo.m_triangleIndex = m_i;
-						}
+						if (r.m_localShapeInfo == NULL)
-						virtual btScalar addSingleResult (btCollisionWorld::LocalRayResult &r, bool b)
+							r.m_localShapeInfo = &shapeInfo;
-                            {
-                                    btCollisionWorld::LocalShapeInfo	shapeInfo;
-                                    shapeInfo.m_shapePart = -1;
-                                    shapeInfo.m_triangleIndex = m_i;
-                                    if (r.m_localShapeInfo == NULL)
-                                        r.m_localShapeInfo = &shapeInfo;
 
-									const btScalar result = m_userCallback->addSingleResult(r, b);
+						const btScalar result = m_userCallback->addSingleResult(r, b);
-									m_closestHitFraction = m_userCallback->m_closestHitFraction;
+						m_closestHitFraction = m_userCallback->m_closestHitFraction;
-									return result;
+						return result;
-                            }
+					}
-                    };
+				};
+				
+				struct RayTester : btDbvt::ICollide
+				{
+					btCollisionObject* m_collisionObject;
+					const btCompoundShape* m_compoundShape;
+					const btTransform& m_colObjWorldTransform;
+					const btTransform& m_rayFromTrans;
+					const btTransform& m_rayToTrans;
+					RayResultCallback& m_resultCallback;
+					
+					RayTester(btCollisionObject* collisionObject,
+							const btCompoundShape* compoundShape,
+							const btTransform& colObjWorldTransform,
+							const btTransform& rayFromTrans,
+							const btTransform& rayToTrans,
+							RayResultCallback& resultCallback):
+						m_collisionObject(collisionObject),
+						m_compoundShape(compoundShape),
+						m_colObjWorldTransform(colObjWorldTransform),
+						m_rayFromTrans(rayFromTrans),
+						m_rayToTrans(rayToTrans),
+						m_resultCallback(resultCallback)
+					{
+						
+					}
+					
+					void Process(int i)
+					{
+						const btCollisionShape* childCollisionShape = m_compoundShape->getChildShape(i);
+						const btTransform& childTrans = m_compoundShape->getChildTransform(i);
+						btTransform childWorldTrans = m_colObjWorldTransform * childTrans;
+						
+						// replace collision shape so that callback can determine the triangle
+						btCollisionShape* saveCollisionShape = m_collisionObject->getCollisionShape();
+						m_collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
 
-                    LocalInfoAdder2 my_cb(i, &resultCallback);
+						LocalInfoAdder2 my_cb(i, &m_resultCallback);
 
-					rayTestSingle(rayFromTrans,rayToTrans,
+						rayTestSingle(
-						collisionObject,
+							m_rayFromTrans,
-						childCollisionShape,
+							m_rayToTrans,
-						childWorldTrans,
+							m_collisionObject,
-						my_cb);
+							childCollisionShape,
-					// restore
+							childWorldTrans,
-					collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
+							my_cb);
+						
+						// restore
+						m_collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
+					}
+					
+					void Process(const btDbvtNode* leaf)
+					{
+						Process((int)leaf->data);
+					}
+				};
+				
+				const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
+				const btDbvt* dbvt = compoundShape->getDynamicAabbTree();
+
+
+				RayTester rayCB(
+					collisionObject,
+					compoundShape,
+					colObjWorldTransform,
+					rayFromTrans,
+					rayToTrans,
+					resultCallback);
+#ifndef	DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
+				if (dbvt)
+				{
+					btVector3 localRayFrom = colObjWorldTransform.inverseTimes(rayFromTrans).getOrigin();
+					btVector3 localRayTo = colObjWorldTransform.inverseTimes(rayToTrans).getOrigin();
+					btDbvt::rayTest(dbvt->m_root, localRayFrom , localRayTo, rayCB);
+				}
+				else
+#endif //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
+				{
+					for (int i = 0, n = compoundShape->getNumChildShapes(); i < n; ++i)
+					{
+						rayCB.Process(i);
+					}	
 				}
 			}
 		}

src/BulletCollision/CollisionShapes/btCompoundShape.h

@@ -143,8 +143,12 @@ public:
 		return "Compound";
 	}
 
-
+	const btDbvt*	getDynamicAabbTree() const
-	btDbvt*							getDynamicAabbTree()
+	{
+		return m_dynamicAabbTree;
+	}
+	
+	btDbvt*	getDynamicAabbTree()
 	{
 		return m_dynamicAabbTree;
 	}