Index: code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp
===================================================================
--- code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp	(revision 1963)
+++ code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp	(revision 1972)
@@ -19,5 +19,4 @@
 #include "LinearMath/btIDebugDraw.h"
 
-#define RAYAABB2
 
 btQuantizedBvh::btQuantizedBvh() : m_useQuantization(false), 
@@ -26,7 +25,6 @@
 					//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);
+{ 
+
 }
 
@@ -122,5 +120,5 @@
 	int curIndex = m_curNodeIndex;
 
-	btAssert(numIndices>0);
+	assert(numIndices>0);
 
 	if (numIndices==1)
@@ -143,9 +141,6 @@
 	int internalNodeIndex = m_curNodeIndex;
 	
-	//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
-	
+	setInternalNodeAabbMax(m_curNodeIndex,m_bvhAabbMin);
+	setInternalNodeAabbMin(m_curNodeIndex,m_bvhAabbMax);
 	
 	for (i=startIndex;i<endIndex;i++)
@@ -183,7 +178,4 @@
 			updateSubtreeHeaders(leftChildNodexIndex,rightChildNodexIndex);
 		}
-	} else
-	{
-
 	}
 
@@ -347,5 +339,4 @@
 int maxIterations = 0;
 
-
 void	btQuantizedBvh::walkStacklessTree(btNodeOverlapCallback* nodeCallback,const btVector3& aabbMin,const btVector3& aabbMax) const
 {
@@ -362,5 +353,5 @@
 	{
 		//catch bugs in tree data
-		btAssert (walkIterations < m_curNodeIndex);
+		assert (walkIterations < m_curNodeIndex);
 
 		walkIterations++;
@@ -444,95 +435,4 @@
 
 
-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;
-
-}
-
 
 
@@ -555,5 +455,5 @@
 
 	btScalar lambda_max = 1.0;
-
+#define RAYAABB2
 #ifdef RAYAABB2
 	btVector3 rayFrom = raySource;
@@ -603,5 +503,5 @@
 
 		//catch bugs in tree data
-		btAssert (walkIterations < subTreeSize);
+		assert (walkIterations < subTreeSize);
 
 		walkIterations++;
@@ -634,7 +534,5 @@
 			///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);
@@ -700,5 +598,5 @@
 
 		//catch bugs in tree data
-		btAssert (walkIterations < subTreeSize);
+		assert (walkIterations < subTreeSize);
 
 		walkIterations++;
@@ -755,5 +653,16 @@
 void	btQuantizedBvh::reportRayOverlappingNodex (btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget) const
 {
-	reportBoxCastOverlappingNodex(nodeCallback,raySource,rayTarget,btVector3(0,0,0),btVector3(0,0,0));
+	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);
+	}
 }
 
@@ -761,17 +670,11 @@
 void	btQuantizedBvh::reportBoxCastOverlappingNodex(btNodeOverlapCallback* nodeCallback, const btVector3& raySource, const btVector3& rayTarget, const btVector3& aabbMin,const btVector3& aabbMax) const
 {
-	//always use stackless
-
-	if (m_useQuantization)
+	bool fast_path = m_useQuantization && m_traversalMode == TRAVERSAL_STACKLESS;
+	if (fast_path)
 	{
 		walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, m_curNodeIndex);
-	}
-	else
-	{
-		walkStacklessTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, m_curNodeIndex);
-	}
-	/*
-	{
-		//recursive traversal
+	} else {
+		/* Slow path:
+		   Construct the bounding box for the entire box cast and send that down the tree */
 		btVector3 qaabbMin = raySource;
 		btVector3 qaabbMax = raySource;
@@ -782,6 +685,4 @@
 		reportAabbOverlappingNodex(nodeCallback,qaabbMin,qaabbMax);
 	}
-	*/
-
 }
 
@@ -843,5 +744,5 @@
 bool btQuantizedBvh::serialize(void *o_alignedDataBuffer, unsigned /*i_dataBufferSize */, bool i_swapEndian)
 {
-	btAssert(m_subtreeHeaderCount == m_SubtreeHeaders.size());
+	assert(m_subtreeHeaderCount == m_SubtreeHeaders.size());
 	m_subtreeHeaderCount = m_SubtreeHeaders.size();
 
