Changeset 2430 for code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp

Timestamp:

Dec 13, 2008, 11:45:51 PM (17 years ago)

Author:

rgrieder

Message:

Updated to Bullet 2.73 (first part).

File:

• code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp (modified) (16 diffs)

code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp

@@ -19 +19 @@
 #include "LinearMath/btIDebugDraw.h"
 
-
-btQuantizedBvh::btQuantizedBvh() : m_useQuantization(false), 
+#define RAYAABB2
+
+btQuantizedBvh::btQuantizedBvh() : 
+                                        m_bulletVersion(BT_BULLET_VERSION),
+                                        m_useQuantization(false), 
                                         //m_traversalMode(TRAVERSAL_STACKLESS_CACHE_FRIENDLY)
                                         m_traversalMode(TRAVERSAL_STACKLESS)
                                         //m_traversalMode(TRAVERSAL_RECURSIVE)
                                         ,m_subtreeHeaderCount(0) //PCK: add this line
-{ 
-
+{
+        m_bvhAabbMin.setValue(-SIMD_INFINITY,-SIMD_INFINITY,-SIMD_INFINITY);
+        m_bvhAabbMax.setValue(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY);
 }
 
@@ -120 +124 @@
         int curIndex = m_curNodeIndex;
 
-        assert(numIndices>0);
+        btAssert(numIndices>0);
 
         if (numIndices==1)
@@ -141 +145 @@
         int internalNodeIndex = m_curNodeIndex;
         
-        setInternalNodeAabbMax(m_curNodeIndex,m_bvhAabbMin);
-        setInternalNodeAabbMin(m_curNodeIndex,m_bvhAabbMax);
+        //set the min aabb to 'inf' or a max value, and set the max aabb to a -inf/minimum value.
+        //the aabb will be expanded during buildTree/mergeInternalNodeAabb with actual node values
+        setInternalNodeAabbMin(m_curNodeIndex,m_bvhAabbMax);//can't use btVector3(SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY)) because of quantization
+        setInternalNodeAabbMax(m_curNodeIndex,m_bvhAabbMin);//can't use btVector3(-SIMD_INFINITY,-SIMD_INFINITY,-SIMD_INFINITY)) because of quantization
+        
         
         for (i=startIndex;i<endIndex;i++)
@@ -178 +185 @@
                         updateSubtreeHeaders(leftChildNodexIndex,rightChildNodexIndex);
                 }
+        } else
+        {
+
         }
 
@@ -339 +349 @@
 int maxIterations = 0;
 
+
 void    btQuantizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const
 {
@@ -353 +364 @@
         {
                 //catch bugs in tree data
-                assert (walkIterations < m_curNodeIndex);
+                btAssert (walkIterations < m_curNodeIndex);
 
                 walkIterations++;
@@ -435 +446 @@
 
 
+void    btQuantizedBvh::walkStacklessTreeAgainstRay(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin, const btVector3& aabbMax, int startNodeIndex,int endNodeIndex) const
+{
+        btAssert(!m_useQuantization);
+
+        const btOptimizedBvhNode* rootNode = &m_contiguousNodes[0];
+        int escapeIndex, curIndex = 0;
+        int walkIterations = 0;
+        bool isLeafNode;
+        //PCK: unsigned instead of bool
+        unsigned aabbOverlap=0;
+        unsigned rayBoxOverlap=0;
+        btScalar lambda_max = 1.0;
+        
+                /* Quick pruning by quantized box */
+        btVector3 rayAabbMin = raySource;
+        btVector3 rayAabbMax = raySource;
+        rayAabbMin.setMin(rayTarget);
+        rayAabbMax.setMax(rayTarget);
+
+        /* Add box cast extents to bounding box */
+        rayAabbMin += aabbMin;
+        rayAabbMax += aabbMax;
+
+#ifdef RAYAABB2
+        btVector3 rayFrom = raySource;
+        btVector3 rayDir = (rayTarget-raySource);
+        rayDir.normalize ();
+        lambda_max = rayDir.dot(rayTarget-raySource);
+        ///what about division by zero? --> just set rayDirection[i] to 1.0
+        btVector3 rayDirectionInverse;
+        rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
+        rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
+        rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
+        unsigned int sign[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0};
+#endif
+
+        btVector3 bounds[2];
+
+        while (curIndex < m_curNodeIndex)
+        {
+                btScalar param = 1.0;
+                //catch bugs in tree data
+                btAssert (walkIterations < m_curNodeIndex);
+
+                walkIterations++;
+
+                bounds[0] = rootNode->m_aabbMinOrg;
+                bounds[1] = rootNode->m_aabbMaxOrg;
+                /* Add box cast extents */
+                bounds[0] += aabbMin;
+                bounds[1] += aabbMax;
+
+                aabbOverlap = TestAabbAgainstAabb2(rayAabbMin,rayAabbMax,rootNode->m_aabbMinOrg,rootNode->m_aabbMaxOrg);
+                //perhaps profile if it is worth doing the aabbOverlap test first
+
+#ifdef RAYAABB2
+                        ///careful with this check: need to check division by zero (above) and fix the unQuantize method
+                        ///thanks Joerg/hiker for the reproduction case!
+                        ///http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1858
+                rayBoxOverlap = aabbOverlap ? btRayAabb2 (raySource, rayDirectionInverse, sign, bounds, param, 0.0f, lambda_max) : false;
+
+#else
+                btVector3 normal;
+                rayBoxOverlap = btRayAabb(raySource, rayTarget,bounds[0],bounds[1],param, normal);
+#endif
+
+                isLeafNode = rootNode->m_escapeIndex == -1;
+                
+                //PCK: unsigned instead of bool
+                if (isLeafNode && (rayBoxOverlap != 0))
+                {
+                        nodeCallback->processNode(rootNode->m_subPart,rootNode->m_triangleIndex);
+                } 
+                
+                //PCK: unsigned instead of bool
+                if ((rayBoxOverlap != 0) || isLeafNode)
+                {
+                        rootNode++;
+                        curIndex++;
+                } else
+                {
+                        escapeIndex = rootNode->m_escapeIndex;
+                        rootNode += escapeIndex;
+                        curIndex += escapeIndex;
+                }
+        }
+        if (maxIterations < walkIterations)
+                maxIterations = walkIterations;
+
+}
+
 
 
@@ -455 +557 @@
 
         btScalar lambda_max = 1.0;
-#define RAYAABB2
+
 #ifdef RAYAABB2
         btVector3 rayFrom = raySource;
@@ -503 +605 @@
 
                 //catch bugs in tree data
-                assert (walkIterations < subTreeSize);
+                btAssert (walkIterations < subTreeSize);
 
                 walkIterations++;
@@ -534 +636 @@
                         ///http://www.bulletphysics.com/Bullet/phpBB3/viewtopic.php?f=9&t=1858
 
+                        //BT_PROFILE("btRayAabb2");
                         rayBoxOverlap = btRayAabb2 (raySource, rayDirection, sign, bounds, param, 0.0f, lambda_max);
+                        
 #else
                         rayBoxOverlap = true;//btRayAabb(raySource, rayTarget, bounds[0], bounds[1], param, normal);
@@ -598 +702 @@
 
                 //catch bugs in tree data
-                assert (walkIterations < subTreeSize);
+                btAssert (walkIterations < subTreeSize);
 
                 walkIterations++;
@@ -653 +757 @@
 void    btQuantizedBvh::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, btVector3(0, 0, 0), btVector3(0, 0, 0), 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);
-        }
+        reportBoxCastOverlappingNodex(nodeCallback,raySource,rayTarget,btVector3(0,0,0),btVector3(0,0,0));
 }
 
@@ -670 +763 @@
 void    btQuantizedBvh::reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const
 {
-        bool fast_path = m_useQuantization && m_traversalMode == TRAVERSAL_STACKLESS;
-        if (fast_path)
+        //always use stackless
+
+        if (m_useQuantization)
         {
                 walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, m_curNodeIndex);
-        } else {
-                /* Slow path:
-                   Construct the bounding box for the entire box cast and send that down the tree */
+        }
+        else
+        {
+                walkStacklessTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, m_curNodeIndex);
+        }
+        /*
+        {
+                //recursive traversal
                 btVector3 qaabbMin = raySource;
                 btVector3 qaabbMax = raySource;
@@ -685 +784 @@
                 reportAabbOverlappingNodex(nodeCallback,qaabbMin,qaabbMax);
         }
+        */
+
 }
 
@@ -744 +845 @@
 bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBufferSize */, bool i_swapEndian)
 {
-        assert(m_subtreeHeaderCount == m_SubtreeHeaders.size());
+        btAssert(m_subtreeHeaderCount == m_SubtreeHeaders.size());
         m_subtreeHeaderCount = m_SubtreeHeaders.size();
 
@@ -1038 +1139 @@
 m_bvhAabbMin(self.m_bvhAabbMin),
 m_bvhAabbMax(self.m_bvhAabbMax),
-m_bvhQuantization(self.m_bvhQuantization)
-{
-
-
-}
-
-
-
+m_bvhQuantization(self.m_bvhQuantization),
+m_bulletVersion(BT_BULLET_VERSION)
+{
+
+}
+
+
+