From f2c9588969e06e5f57d85f60d0c75adad7b49b89 Mon Sep 17 00:00:00 2001 From: ejcoumans Date: Thu, 29 Nov 2007 21:24:51 +0000 Subject: [PATCH] Added raycast against trianglemesh. Will be extended to object cast soon. Thanks John Rowe (JMC) --- Demos/AllBulletDemos/DemoEntries.cpp | 3 + .../CollisionDispatch/btCollisionWorld.cpp | 25 +--- .../btBvhTriangleMeshShape.cpp | 61 +++++++- .../CollisionShapes/btBvhTriangleMeshShape.h | 2 + .../CollisionShapes/btOptimizedBvh.cpp | 130 +++++++++++++++++- .../CollisionShapes/btOptimizedBvh.h | 4 +- .../btRaycastCallback.cpp | 2 - src/LinearMath/btAabbUtil2.h | 36 +++++ 8 files changed, 238 insertions(+), 25 deletions(-) diff --git a/Demos/AllBulletDemos/DemoEntries.cpp b/Demos/AllBulletDemos/DemoEntries.cpp index 6d4a549d3..f59d292bb 100644 --- a/Demos/AllBulletDemos/DemoEntries.cpp +++ b/Demos/AllBulletDemos/DemoEntries.cpp @@ -19,6 +19,7 @@ subject to the following restrictions: #include "../BspDemo/BspDemo.h" #include "../BasicDemo/BasicDemo.h" #include "../ConcaveDemo/ConcaveDemo.h" +//#include "../ConcaveRaycastDemo/ConcaveRaycastDemo.h" #include "../ConvexDecompositionDemo/ConvexDecompositionDemo.h" #include "../RagdollDemo/RagdollDemo.h" #include "../GimpactTestDemo/GimpactTestDemo.h" @@ -96,6 +97,7 @@ btDemoEntry g_demoEntries[] = {"RagdollDemo",RagdollDemo::Create}, {"CcdPhysicsDemo", CcdPhysicsDemo::Create}, {"ConcaveDemo",ConcaveDemo::Create}, +// {"ConcaveRaycastDemo",ConcaveRaycastDemo::Create}, {"ConvexDecomposition",ConvexDecompositionDemo::Create}, {"BasicDemo", BasicDemo::Create}, {"BspDemo", BspDemo::Create}, @@ -108,3 +110,4 @@ btDemoEntry g_demoEntries[] = {0, 0} }; + diff --git a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index f0d567eb8..435ffaa84 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -20,7 +20,7 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btConvexShape.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" //for raycasting -#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h" //for raycasting +#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" //for raycasting #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" #include "BulletCollision/CollisionShapes/btCompoundShape.h" #include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h" @@ -254,19 +254,14 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt } else { - if (collisionShape->isConcave()) - { - - btTriangleMeshShape* triangleMesh = (btTriangleMeshShape*)collisionShape; - + { + btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape; btTransform worldTocollisionObject = colObjWorldTransform.inverse(); - btVector3 rayFromLocal = worldTocollisionObject * rayFromTrans.getOrigin(); btVector3 rayToLocal = worldTocollisionObject * rayToTrans.getOrigin(); //ConvexCast::CastResult - struct BridgeTriangleRaycastCallback : public btTriangleRaycastCallback { btCollisionWorld::RayResultCallback* m_resultCallback; @@ -297,23 +292,13 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt bool normalInWorldSpace = false; return m_resultCallback->AddSingleResult(rayResult,normalInWorldSpace); - - } }; - - BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh); + BridgeTriangleRaycastCallback rcb(rayFromLocal,rayToLocal,&resultCallback,collisionObject,triangleMesh); rcb.m_hitFraction = resultCallback.m_closestHitFraction; - - btVector3 rayAabbMinLocal = rayFromLocal; - rayAabbMinLocal.setMin(rayToLocal); - btVector3 rayAabbMaxLocal = rayFromLocal; - rayAabbMaxLocal.setMax(rayToLocal); - - triangleMesh->processAllTriangles(&rcb,rayAabbMinLocal,rayAabbMaxLocal); - + triangleMesh->performRaycast(&rcb,rayFromLocal,rayToLocal); } else { //todo: use AABB tree or other BVH acceleration structure! diff --git a/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp b/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp index a390cc73b..83f1ccf58 100644 --- a/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp +++ b/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp @@ -18,7 +18,6 @@ subject to the following restrictions: #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" #include "BulletCollision/CollisionShapes/btOptimizedBvh.h" - ///Bvh Concave triangle mesh is a static-triangle mesh shape with Bounding Volume Hierarchy optimization. ///Uses an interface to access the triangles to allow for sharing graphics/physics triangles. btBvhTriangleMeshShape::btBvhTriangleMeshShape(btStridingMeshInterface* meshInterface, bool useQuantizedAabbCompression, bool buildBvh) @@ -92,6 +91,66 @@ btBvhTriangleMeshShape::~btBvhTriangleMeshShape() } } +void btBvhTriangleMeshShape::performRaycast (btTriangleRaycastCallback* callback, const btVector3& raySource, const btVector3& rayTarget) +{ + struct MyNodeOverlapCallback : public btNodeOverlapCallback + { + btStridingMeshInterface* m_meshInterface; + btTriangleRaycastCallback* m_callback; + + MyNodeOverlapCallback(btTriangleRaycastCallback* callback,btStridingMeshInterface* meshInterface) + :m_meshInterface(meshInterface), + m_callback(callback) + { + } + + virtual void processNode(int nodeSubPart, int nodeTriangleIndex) + { + btVector3 m_triangle[3]; + const unsigned char *vertexbase; + int numverts; + PHY_ScalarType type; + int stride; + const unsigned char *indexbase; + int indexstride; + int numfaces; + PHY_ScalarType indicestype; + + m_meshInterface->getLockedReadOnlyVertexIndexBase( + &vertexbase, + numverts, + type, + stride, + &indexbase, + indexstride, + numfaces, + indicestype, + nodeSubPart); + + int* gfxbase = (int*)(indexbase+nodeTriangleIndex*indexstride); + btAssert(indicestype==PHY_INTEGER||indicestype==PHY_SHORT); + + const btVector3& meshScaling = m_meshInterface->getScaling(); + for (int j=2;j>=0;j--) + { + int graphicsindex = indicestype==PHY_SHORT?((short*)gfxbase)[j]:gfxbase[j]; + + btScalar* graphicsbase = (btScalar*)(vertexbase+graphicsindex*stride); + + m_triangle[j] = btVector3(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); + } + + /* Perform ray vs. triangle collision here */ + m_callback->processTriangle(m_triangle,nodeSubPart,nodeTriangleIndex); + m_meshInterface->unLockReadOnlyVertexBase(nodeSubPart); + } + }; + + MyNodeOverlapCallback myNodeCallback(callback,m_meshInterface); + + m_bvh->reportRayOverlappingNodex(&myNodeCallback,raySource,rayTarget); +} + //perform bvh tree traversal and report overlapping triangles to 'callback' void btBvhTriangleMeshShape::processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const { diff --git a/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h b/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h index 95c73b244..a73642f5f 100644 --- a/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h +++ b/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h @@ -19,6 +19,7 @@ subject to the following restrictions: #include "btTriangleMeshShape.h" #include "btOptimizedBvh.h" #include "LinearMath/btAlignedAllocator.h" +#include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" ///Bvh Concave triangle mesh is a static-triangle mesh shape with Bounding Volume Hierarchy optimization. ///Uses an interface to access the triangles to allow for sharing graphics/physics triangles. @@ -51,6 +52,7 @@ public: */ + void performRaycast (btTriangleRaycastCallback* callback, const btVector3& raySource, const btVector3& rayTarget); virtual void processAllTriangles(btTriangleCallback* callback,const btVector3& aabbMin,const btVector3& aabbMax) const; diff --git a/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp b/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp index bd264f76f..1417bcbcc 100644 --- a/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp +++ b/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp @@ -12,6 +12,7 @@ subject to the following restrictions: 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ + #include "btOptimizedBvh.h" #include "btStridingMeshInterface.h" #include "LinearMath/btAabbUtil2.h" @@ -740,7 +741,117 @@ void btOptimizedBvh::walkRecursiveQuantizedTreeAgainstQueryAabb(const btQuantize +void btOptimizedBvh::walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, int startNodeIndex,int endNodeIndex) const +{ + btAssert(m_useQuantization); + + int curIndex = startNodeIndex; + int walkIterations = 0; + int subTreeSize = endNodeIndex - startNodeIndex; + const btQuantizedBvhNode* rootNode = &m_quantizedContiguousNodes[startNodeIndex]; + int escapeIndex; + + bool isLeafNode; + //PCK: unsigned instead of bool + unsigned BoxBoxOverlap = 0; + unsigned RayBoxOverlap = 0; + + btScalar lambda_max = 1.0; +#define RAYAABB2 +#ifdef RAYAABB2 + btVector3 rayFrom = raySource; + btVector3 rayDirection = (rayTarget-raySource); + rayDirection.normalize (); + lambda_max = rayDirection.dot(rayTarget-raySource); + rayDirection[0] = btScalar(1.0) / rayDirection[0]; + rayDirection[1] = btScalar(1.0) / rayDirection[1]; + rayDirection[2] = btScalar(1.0) / rayDirection[2]; + unsigned int sign[3] = { rayDirection[0] < 0.0, rayDirection[1] < 0.0, rayDirection[2] < 0.0}; +#endif + + /* Quick pruning by quantized box */ + btVector3 rayAabbMin = raySource; + btVector3 rayAabbMax = raySource; + rayAabbMin.setMin(rayTarget); + rayAabbMax.setMax(rayTarget); + + unsigned short int quantizedQueryAabbMin[3]; + unsigned short int quantizedQueryAabbMax[3]; + quantizeWithClamp(quantizedQueryAabbMin,rayAabbMin); + quantizeWithClamp(quantizedQueryAabbMax,rayAabbMax); + + while (curIndex < endNodeIndex) + { + +//#define VISUALLY_ANALYZE_BVH 1 +#ifdef VISUALLY_ANALYZE_BVH + //some code snippet to debugDraw aabb, to visually analyze bvh structure + static int drawPatch = 0; + //need some global access to a debugDrawer + extern btIDebugDraw* debugDrawerPtr; + if (curIndex==drawPatch) + { + btVector3 aabbMin,aabbMax; + aabbMin = unQuantize(rootNode->m_quantizedAabbMin); + aabbMax = unQuantize(rootNode->m_quantizedAabbMax); + btVector3 color(1,0,0); + debugDrawerPtr->drawAabb(aabbMin,aabbMax,color); + } +#endif//VISUALLY_ANALYZE_BVH + + //catch bugs in tree data + assert (walkIterations < subTreeSize); + + walkIterations++; + //PCK: unsigned instead of bool + // only interested if this is closer than any previous hit + btScalar param = 1.0; + RayBoxOverlap = 0; + BoxBoxOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode->m_quantizedAabbMin,rootNode->m_quantizedAabbMax); + isLeafNode = rootNode->isLeafNode(); + if (BoxBoxOverlap) + { + btVector3 bounds[2]; + bounds[0] = unQuantize(rootNode->m_quantizedAabbMin); + bounds[1] = unQuantize(rootNode->m_quantizedAabbMax); + btVector3 normal; +#if 0 + bool ra2 = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0, lambda_max); + bool ra = btRayAabb (raySource, rayTarget, bounds[0], bounds[1], param, normal); + if (ra2 != ra) + { + printf("functions don't match\n"); + } +#endif +#ifdef RAYAABB2 + RayBoxOverlap = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0, lambda_max); +#else + RayBoxOverlap = btRayAabb(raySource, rayTarget, bounds[0], bounds[1], param, normal); +#endif + } + + if (isLeafNode && RayBoxOverlap) + { + nodeCallback->processNode(rootNode->getPartId(),rootNode->getTriangleIndex()); + } + + //PCK: unsigned instead of bool + if ((RayBoxOverlap != 0) || isLeafNode) + { + rootNode++; + curIndex++; + } else + { + escapeIndex = rootNode->getEscapeIndex(); + rootNode += escapeIndex; + curIndex += escapeIndex; + } + } + if (maxIterations < walkIterations) + maxIterations = walkIterations; + +} void btOptimizedBvh::walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const { @@ -830,6 +941,21 @@ void btOptimizedBvh::walkStacklessQuantizedTreeCacheFriendly(btNodeOverlapCallba } +void btOptimizedBvh::reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const +{ + bool fast_path = m_useQuantization && m_traversalMode == TRAVERSAL_STACKLESS; + if (fast_path) + { + walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, 0, m_curNodeIndex); + } else { + /* Otherwise fallback to AABB overlap test */ + btVector3 aabbMin = raySource; + btVector3 aabbMax = raySource; + aabbMin.setMin(rayTarget); + aabbMax.setMax(rayTarget); + reportAabbOverlappingNodex(nodeCallback,aabbMin,aabbMax); + } +} void btOptimizedBvh::reportSphereOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const @@ -1186,6 +1312,8 @@ m_bvhAabbMin(self.m_bvhAabbMin), m_bvhAabbMax(self.m_bvhAabbMax), m_bvhQuantization(self.m_bvhQuantization) { - + + } + diff --git a/src/BulletCollision/CollisionShapes/btOptimizedBvh.h b/src/BulletCollision/CollisionShapes/btOptimizedBvh.h index 6f9ec2d83..6ac30f0ee 100644 --- a/src/BulletCollision/CollisionShapes/btOptimizedBvh.h +++ b/src/BulletCollision/CollisionShapes/btOptimizedBvh.h @@ -286,6 +286,7 @@ protected: void walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; + void walkStacklessQuantizedTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, int startNodeIndex,int endNodeIndex) const; void walkStacklessQuantizedTree(btNodeOverlapCallback* nodeCallback,unsigned short int* quantizedQueryAabbMin,unsigned short int* quantizedQueryAabbMax,int startNodeIndex,int endNodeIndex) const; ///tree traversal designed for small-memory processors like PS3 SPU @@ -329,7 +330,7 @@ public: void build(btStridingMeshInterface* triangles,bool useQuantizedAabbCompression, const btVector3& bvhAabbMin, const btVector3& bvhAabbMax); void reportAabbOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; - + void reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const; void reportSphereOverlappingNodex(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const; SIMD_FORCE_INLINE void quantizeWithClamp(unsigned short* out, const btVector3& point) const @@ -401,3 +402,4 @@ private: #endif //OPTIMIZED_BVH_H + diff --git a/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp b/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp index 31b914677..b802ae39d 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp @@ -29,8 +29,6 @@ btTriangleRaycastCallback::btTriangleRaycastCallback(const btVector3& from,const void btTriangleRaycastCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex) { - - const btVector3 &vert0=triangle[0]; const btVector3 &vert1=triangle[1]; const btVector3 &vert2=triangle[2]; diff --git a/src/LinearMath/btAabbUtil2.h b/src/LinearMath/btAabbUtil2.h index 9b320961b..3491ef2d1 100644 --- a/src/LinearMath/btAabbUtil2.h +++ b/src/LinearMath/btAabbUtil2.h @@ -65,6 +65,41 @@ SIMD_FORCE_INLINE int btOutcode(const btVector3& p,const btVector3& halfExtent) } +SIMD_FORCE_INLINE bool btRayAabb2(const btVector3& rayFrom, + const btVector3& rayInvDirection, + const unsigned int raySign[3], + const btVector3 bounds[2], + btScalar& tmin, + btScalar lambda_min, + btScalar lambda_max) +{ + btScalar tmax, tymin, tymax, tzmin, tzmax; + tmin = (bounds[raySign[0]][0] - rayFrom[0]) * rayInvDirection[0]; + tmax = (bounds[1-raySign[0]][0] - rayFrom[0]) * rayInvDirection[0]; + tymin = (bounds[raySign[1]][1] - rayFrom[1]) * rayInvDirection[1]; + tymax = (bounds[1-raySign[1]][1] - rayFrom[1]) * rayInvDirection[1]; + + if ( (tmin > tymax) || (tymin > tmax) ) + return false; + + if (tymin > tmin) + tmin = tymin; + + if (tymax < tmax) + tmax = tymax; + + tzmin = (bounds[raySign[2]][2] - rayFrom[2]) * rayInvDirection[2]; + tzmax = (bounds[1-raySign[2]][2] - rayFrom[2]) * rayInvDirection[2]; + + if ( (tmin > tzmax) || (tzmin > tmax) ) + return false; + if (tzmin > tmin) + tmin = tzmin; + if (tzmax < tmax) + tmax = tzmax; + return ( (tmin < lambda_max) && (tmax > lambda_min) ); +} + SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom, const btVector3& rayTo, const btVector3& aabbMin, @@ -123,3 +158,4 @@ SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom, #endif +