Merge pull request #24740 from OBKF/update-bullet-physics

Update Bullet physics to commit 126b676

1 files changed, 131 insertions, 173 deletions

diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp
index 4528758c37..b81fc97044 100644
--- a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp
+++ b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp
@@ -27,11 +27,9 @@ subject to the following restrictions:
 #ifdef TRI_COLLISION_PROFILING
 btClock g_q_tree_clock;
 
-
 float g_q_accum_tree_collision_time = 0;
 int g_q_count_traversing = 0;
 
-
 void bt_begin_gim02_q_tree_time()
 {
 	g_q_tree_clock.reset();
@@ -43,11 +41,10 @@ void bt_end_gim02_q_tree_time()
 	g_q_count_traversing++;
 }
 
-
 //! Gets the average time in miliseconds of tree collisions
 float btGImpactQuantizedBvh::getAverageTreeCollisionTime()
 {
-	if(g_q_count_traversing == 0) return 0;
+	if (g_q_count_traversing == 0) return 0;
 
 	float avgtime = g_q_accum_tree_collision_time;
 	avgtime /= (float)g_q_count_traversing;
@@ -56,99 +53,92 @@ float btGImpactQuantizedBvh::getAverageTreeCollisionTime()
 	g_q_count_traversing = 0;
 	return avgtime;
 
-//	float avgtime = g_q_count_traversing;
-//	g_q_count_traversing = 0;
-//	return avgtime;
-
+	//	float avgtime = g_q_count_traversing;
+	//	g_q_count_traversing = 0;
+	//	return avgtime;
 }
 
-#endif //TRI_COLLISION_PROFILING
+#endif  //TRI_COLLISION_PROFILING
 
 /////////////////////// btQuantizedBvhTree /////////////////////////////////
 
 void btQuantizedBvhTree::calc_quantization(
-	GIM_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin)
+	GIM_BVH_DATA_ARRAY& primitive_boxes, btScalar boundMargin)
 {
 	//calc globa box
 	btAABB global_bound;
 	global_bound.invalidate();
 
-	for (int i=0;i<primitive_boxes.size() ;i++ )
+	for (int i = 0; i < primitive_boxes.size(); i++)
 	{
 		global_bound.merge(primitive_boxes[i].m_bound);
 	}
 
 	bt_calc_quantization_parameters(
-		m_global_bound.m_min,m_global_bound.m_max,m_bvhQuantization,global_bound.m_min,global_bound.m_max,boundMargin);
-
+		m_global_bound.m_min, m_global_bound.m_max, m_bvhQuantization, global_bound.m_min, global_bound.m_max, boundMargin);
 }
 
-
-
 int btQuantizedBvhTree::_calc_splitting_axis(
-	GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex)
+	GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex)
 {
-
 	int i;
 
-	btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
-	btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.));
-	int numIndices = endIndex-startIndex;
+	btVector3 means(btScalar(0.), btScalar(0.), btScalar(0.));
+	btVector3 variance(btScalar(0.), btScalar(0.), btScalar(0.));
+	int numIndices = endIndex - startIndex;
 
-	for (i=startIndex;i<endIndex;i++)
+	for (i = startIndex; i < endIndex; i++)
 	{
-		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
-					 primitive_boxes[i].m_bound.m_min);
-		means+=center;
+		btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max +
+											primitive_boxes[i].m_bound.m_min);
+		means += center;
 	}
-	means *= (btScalar(1.)/(btScalar)numIndices);
+	means *= (btScalar(1.) / (btScalar)numIndices);
 
-	for (i=startIndex;i<endIndex;i++)
+	for (i = startIndex; i < endIndex; i++)
 	{
-		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
-					 primitive_boxes[i].m_bound.m_min);
-		btVector3 diff2 = center-means;
+		btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max +
+											primitive_boxes[i].m_bound.m_min);
+		btVector3 diff2 = center - means;
 		diff2 = diff2 * diff2;
 		variance += diff2;
 	}
-	variance *= (btScalar(1.)/	((btScalar)numIndices-1)	);
+	variance *= (btScalar(1.) / ((btScalar)numIndices - 1));
 
 	return variance.maxAxis();
 }
 
-
 int btQuantizedBvhTree::_sort_and_calc_splitting_index(
-	GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,
+	GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex,
 	int endIndex, int splitAxis)
 {
 	int i;
-	int splitIndex =startIndex;
+	int splitIndex = startIndex;
 	int numIndices = endIndex - startIndex;
 
 	// average of centers
 	btScalar splitValue = 0.0f;
 
-	btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
-	for (i=startIndex;i<endIndex;i++)
+	btVector3 means(btScalar(0.), btScalar(0.), btScalar(0.));
+	for (i = startIndex; i < endIndex; i++)
 	{
-		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
-					 primitive_boxes[i].m_bound.m_min);
-		means+=center;
+		btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max +
+											primitive_boxes[i].m_bound.m_min);
+		means += center;
 	}
-	means *= (btScalar(1.)/(btScalar)numIndices);
+	means *= (btScalar(1.) / (btScalar)numIndices);
 
 	splitValue = means[splitAxis];
 
-
 	//sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
-	for (i=startIndex;i<endIndex;i++)
+	for (i = startIndex; i < endIndex; i++)
 	{
-		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
-					 primitive_boxes[i].m_bound.m_min);
+		btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max +
+											primitive_boxes[i].m_bound.m_min);
 		if (center[splitAxis] > splitValue)
 		{
 			//swap
-			primitive_boxes.swap(i,splitIndex);
+			primitive_boxes.swap(i, splitIndex);
 			//swapLeafNodes(i,splitIndex);
 			splitIndex++;
 		}
@@ -163,32 +153,30 @@ int btQuantizedBvhTree::_sort_and_calc_splitting_index(
 	//bool unbalanced2 = true;
 
 	//this should be safe too:
-	int rangeBalancedIndices = numIndices/3;
-	bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices)));
+	int rangeBalancedIndices = numIndices / 3;
+	bool unbalanced = ((splitIndex <= (startIndex + rangeBalancedIndices)) || (splitIndex >= (endIndex - 1 - rangeBalancedIndices)));
 
 	if (unbalanced)
 	{
-		splitIndex = startIndex+ (numIndices>>1);
+		splitIndex = startIndex + (numIndices >> 1);
 	}
 
-	btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex))));
+	btAssert(!((splitIndex == startIndex) || (splitIndex == (endIndex))));
 
 	return splitIndex;
-
 }
 
-
-void btQuantizedBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex)
+void btQuantizedBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex)
 {
 	int curIndex = m_num_nodes;
 	m_num_nodes++;
 
-	btAssert((endIndex-startIndex)>0);
+	btAssert((endIndex - startIndex) > 0);
 
-	if ((endIndex-startIndex)==1)
+	if ((endIndex - startIndex) == 1)
 	{
-	    //We have a leaf node
-	    setNodeBound(curIndex,primitive_boxes[startIndex].m_bound);
+		//We have a leaf node
+		setNodeBound(curIndex, primitive_boxes[startIndex].m_bound);
 		m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data);
 
 		return;
@@ -196,48 +184,43 @@ void btQuantizedBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, i
 	//calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
 
 	//split axis
-	int splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex);
+	int splitIndex = _calc_splitting_axis(primitive_boxes, startIndex, endIndex);
 
 	splitIndex = _sort_and_calc_splitting_index(
-			primitive_boxes,startIndex,endIndex,
-			splitIndex//split axis
-			);
-
+		primitive_boxes, startIndex, endIndex,
+		splitIndex  //split axis
+	);
 
 	//calc this node bounding box
 
 	btAABB node_bound;
 	node_bound.invalidate();
 
-	for (int i=startIndex;i<endIndex;i++)
+	for (int i = startIndex; i < endIndex; i++)
 	{
 		node_bound.merge(primitive_boxes[i].m_bound);
 	}
 
-	setNodeBound(curIndex,node_bound);
-
+	setNodeBound(curIndex, node_bound);
 
 	//build left branch
-	_build_sub_tree(primitive_boxes, startIndex, splitIndex );
-
+	_build_sub_tree(primitive_boxes, startIndex, splitIndex);
 
 	//build right branch
-	 _build_sub_tree(primitive_boxes, splitIndex ,endIndex);
+	_build_sub_tree(primitive_boxes, splitIndex, endIndex);
 
 	m_node_array[curIndex].setEscapeIndex(m_num_nodes - curIndex);
-
-
 }
 
 //! stackless build tree
 void btQuantizedBvhTree::build_tree(
-	GIM_BVH_DATA_ARRAY & primitive_boxes)
+	GIM_BVH_DATA_ARRAY& primitive_boxes)
 {
 	calc_quantization(primitive_boxes);
 	// initialize node count to 0
 	m_num_nodes = 0;
 	// allocate nodes
-	m_node_array.resize(primitive_boxes.size()*2);
+	m_node_array.resize(primitive_boxes.size() * 2);
 
 	_build_sub_tree(primitive_boxes, 0, primitive_boxes.size());
 }
@@ -247,13 +230,13 @@ void btQuantizedBvhTree::build_tree(
 void btGImpactQuantizedBvh::refit()
 {
 	int nodecount = getNodeCount();
-	while(nodecount--)
+	while (nodecount--)
 	{
-		if(isLeafNode(nodecount))
+		if (isLeafNode(nodecount))
 		{
 			btAABB leafbox;
-			m_primitive_manager->get_primitive_box(getNodeData(nodecount),leafbox);
-			setNodeBound(nodecount,leafbox);
+			m_primitive_manager->get_primitive_box(getNodeData(nodecount), leafbox);
+			setNodeBound(nodecount, leafbox);
 		}
 		else
 		{
@@ -265,20 +248,20 @@ void btGImpactQuantizedBvh::refit()
 			btAABB temp_box;
 
 			int child_node = getLeftNode(nodecount);
-			if(child_node)
+			if (child_node)
 			{
-				getNodeBound(child_node,temp_box);
+				getNodeBound(child_node, temp_box);
 				bound.merge(temp_box);
 			}
 
 			child_node = getRightNode(nodecount);
-			if(child_node)
+			if (child_node)
 			{
-				getNodeBound(child_node,temp_box);
+				getNodeBound(child_node, temp_box);
 				bound.merge(temp_box);
 			}
 
-			setNodeBound(nodecount,bound);
+			setNodeBound(nodecount, bound);
 		}
 	}
 }
@@ -290,17 +273,17 @@ void btGImpactQuantizedBvh::buildSet()
 	GIM_BVH_DATA_ARRAY primitive_boxes;
 	primitive_boxes.resize(m_primitive_manager->get_primitive_count());
 
-	for (int i = 0;i<primitive_boxes.size() ;i++ )
+	for (int i = 0; i < primitive_boxes.size(); i++)
 	{
-		 m_primitive_manager->get_primitive_box(i,primitive_boxes[i].m_bound);
-		 primitive_boxes[i].m_data = i;
+		m_primitive_manager->get_primitive_box(i, primitive_boxes[i].m_bound);
+		primitive_boxes[i].m_data = i;
 	}
 
 	m_box_tree.build_tree(primitive_boxes);
 }
 
 //! returns the indices of the primitives in the m_primitive_manager
-bool btGImpactQuantizedBvh::boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const
+bool btGImpactQuantizedBvh::boxQuery(const btAABB& box, btAlignedObjectArray<int>& collided_results) const
 {
 	int curIndex = 0;
 	int numNodes = getNodeCount();
@@ -310,16 +293,14 @@ bool btGImpactQuantizedBvh::boxQuery(const btAABB & box, btAlignedObjectArray<in
 	unsigned short quantizedMin[3];
 	unsigned short quantizedMax[3];
 
-	m_box_tree.quantizePoint(quantizedMin,box.m_min);
-	m_box_tree.quantizePoint(quantizedMax,box.m_max);
-
+	m_box_tree.quantizePoint(quantizedMin, box.m_min);
+	m_box_tree.quantizePoint(quantizedMax, box.m_max);
 
 	while (curIndex < numNodes)
 	{
-
 		//catch bugs in tree data
 
-		bool aabbOverlap = m_box_tree.testQuantizedBoxOverlapp(curIndex, quantizedMin,quantizedMax);
+		bool aabbOverlap = m_box_tree.testQuantizedBoxOverlapp(curIndex, quantizedMin, quantizedMax);
 		bool isleafnode = isLeafNode(curIndex);
 
 		if (isleafnode && aabbOverlap)
@@ -335,19 +316,17 @@ bool btGImpactQuantizedBvh::boxQuery(const btAABB & box, btAlignedObjectArray<in
 		else
 		{
 			//skip node
-			curIndex+= getEscapeNodeIndex(curIndex);
+			curIndex += getEscapeNodeIndex(curIndex);
 		}
 	}
-	if(collided_results.size()>0) return true;
+	if (collided_results.size() > 0) return true;
 	return false;
 }
 
-
-
 //! returns the indices of the primitives in the m_primitive_manager
 bool btGImpactQuantizedBvh::rayQuery(
-	const btVector3 & ray_dir,const btVector3 & ray_origin ,
-	btAlignedObjectArray<int> & collided_results) const
+	const btVector3& ray_dir, const btVector3& ray_origin,
+	btAlignedObjectArray<int>& collided_results) const
 {
 	int curIndex = 0;
 	int numNodes = getNodeCount();
@@ -355,16 +334,16 @@ bool btGImpactQuantizedBvh::rayQuery(
 	while (curIndex < numNodes)
 	{
 		btAABB bound;
-		getNodeBound(curIndex,bound);
+		getNodeBound(curIndex, bound);
 
 		//catch bugs in tree data
 
-		bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir);
+		bool aabbOverlap = bound.collide_ray(ray_origin, ray_dir);
 		bool isleafnode = isLeafNode(curIndex);
 
 		if (isleafnode && aabbOverlap)
 		{
-			collided_results.push_back(getNodeData( curIndex));
+			collided_results.push_back(getNodeData(curIndex));
 		}
 
 		if (aabbOverlap || isleafnode)
@@ -375,154 +354,133 @@ bool btGImpactQuantizedBvh::rayQuery(
 		else
 		{
 			//skip node
-			curIndex+= getEscapeNodeIndex(curIndex);
+			curIndex += getEscapeNodeIndex(curIndex);
 		}
 	}
-	if(collided_results.size()>0) return true;
+	if (collided_results.size() > 0) return true;
 	return false;
 }
 
-
 SIMD_FORCE_INLINE bool _quantized_node_collision(
-	const btGImpactQuantizedBvh * boxset0, const btGImpactQuantizedBvh * boxset1,
-	const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
-	int node0 ,int node1, bool complete_primitive_tests)
+	const btGImpactQuantizedBvh* boxset0, const btGImpactQuantizedBvh* boxset1,
+	const BT_BOX_BOX_TRANSFORM_CACHE& trans_cache_1to0,
+	int node0, int node1, bool complete_primitive_tests)
 {
 	btAABB box0;
-	boxset0->getNodeBound(node0,box0);
+	boxset0->getNodeBound(node0, box0);
 	btAABB box1;
-	boxset1->getNodeBound(node1,box1);
-
-	return box0.overlapping_trans_cache(box1,trans_cache_1to0,complete_primitive_tests );
-//	box1.appy_transform_trans_cache(trans_cache_1to0);
-//	return box0.has_collision(box1);
+	boxset1->getNodeBound(node1, box1);
 
+	return box0.overlapping_trans_cache(box1, trans_cache_1to0, complete_primitive_tests);
+	//	box1.appy_transform_trans_cache(trans_cache_1to0);
+	//	return box0.has_collision(box1);
 }
 
-
 //stackless recursive collision routine
 static void _find_quantized_collision_pairs_recursive(
-	const btGImpactQuantizedBvh * boxset0, const btGImpactQuantizedBvh * boxset1,
-	btPairSet * collision_pairs,
-	const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
+	const btGImpactQuantizedBvh* boxset0, const btGImpactQuantizedBvh* boxset1,
+	btPairSet* collision_pairs,
+	const BT_BOX_BOX_TRANSFORM_CACHE& trans_cache_1to0,
 	int node0, int node1, bool complete_primitive_tests)
 {
+	if (_quantized_node_collision(
+			boxset0, boxset1, trans_cache_1to0,
+			node0, node1, complete_primitive_tests) == false) return;  //avoid colliding internal nodes
 
-
-
-	if( _quantized_node_collision(
-		boxset0,boxset1,trans_cache_1to0,
-		node0,node1,complete_primitive_tests) ==false) return;//avoid colliding internal nodes
-
-	if(boxset0->isLeafNode(node0))
+	if (boxset0->isLeafNode(node0))
 	{
-		if(boxset1->isLeafNode(node1))
+		if (boxset1->isLeafNode(node1))
 		{
 			// collision result
 			collision_pairs->push_pair(
-				boxset0->getNodeData(node0),boxset1->getNodeData(node1));
+				boxset0->getNodeData(node0), boxset1->getNodeData(node1));
 			return;
 		}
 		else
 		{
-
 			//collide left recursive
 
 			_find_quantized_collision_pairs_recursive(
-								boxset0,boxset1,
-								collision_pairs,trans_cache_1to0,
-								node0,boxset1->getLeftNode(node1),false);
+				boxset0, boxset1,
+				collision_pairs, trans_cache_1to0,
+				node0, boxset1->getLeftNode(node1), false);
 
 			//collide right recursive
 			_find_quantized_collision_pairs_recursive(
-								boxset0,boxset1,
-								collision_pairs,trans_cache_1to0,
-								node0,boxset1->getRightNode(node1),false);
-
-
+				boxset0, boxset1,
+				collision_pairs, trans_cache_1to0,
+				node0, boxset1->getRightNode(node1), false);
 		}
 	}
 	else
 	{
-		if(boxset1->isLeafNode(node1))
+		if (boxset1->isLeafNode(node1))
 		{
-
 			//collide left recursive
 			_find_quantized_collision_pairs_recursive(
-								boxset0,boxset1,
-								collision_pairs,trans_cache_1to0,
-								boxset0->getLeftNode(node0),node1,false);
-
+				boxset0, boxset1,
+				collision_pairs, trans_cache_1to0,
+				boxset0->getLeftNode(node0), node1, false);
 
 			//collide right recursive
 
 			_find_quantized_collision_pairs_recursive(
-								boxset0,boxset1,
-								collision_pairs,trans_cache_1to0,
-								boxset0->getRightNode(node0),node1,false);
-
-
+				boxset0, boxset1,
+				collision_pairs, trans_cache_1to0,
+				boxset0->getRightNode(node0), node1, false);
 		}
 		else
 		{
 			//collide left0 left1
 
-
-
 			_find_quantized_collision_pairs_recursive(
-				boxset0,boxset1,
-				collision_pairs,trans_cache_1to0,
-				boxset0->getLeftNode(node0),boxset1->getLeftNode(node1),false);
+				boxset0, boxset1,
+				collision_pairs, trans_cache_1to0,
+				boxset0->getLeftNode(node0), boxset1->getLeftNode(node1), false);
 
 			//collide left0 right1
 
 			_find_quantized_collision_pairs_recursive(
-				boxset0,boxset1,
-				collision_pairs,trans_cache_1to0,
-				boxset0->getLeftNode(node0),boxset1->getRightNode(node1),false);
-
+				boxset0, boxset1,
+				collision_pairs, trans_cache_1to0,
+				boxset0->getLeftNode(node0), boxset1->getRightNode(node1), false);
 
 			//collide right0 left1
 
 			_find_quantized_collision_pairs_recursive(
-				boxset0,boxset1,
-				collision_pairs,trans_cache_1to0,
-				boxset0->getRightNode(node0),boxset1->getLeftNode(node1),false);
+				boxset0, boxset1,
+				collision_pairs, trans_cache_1to0,
+				boxset0->getRightNode(node0), boxset1->getLeftNode(node1), false);
 
 			//collide right0 right1
 
 			_find_quantized_collision_pairs_recursive(
-				boxset0,boxset1,
-				collision_pairs,trans_cache_1to0,
-				boxset0->getRightNode(node0),boxset1->getRightNode(node1),false);
+				boxset0, boxset1,
+				collision_pairs, trans_cache_1to0,
+				boxset0->getRightNode(node0), boxset1->getRightNode(node1), false);
 
-		}// else if node1 is not a leaf
-	}// else if node0 is not a leaf
+		}  // else if node1 is not a leaf
+	}      // else if node0 is not a leaf
 }
 
-
-void btGImpactQuantizedBvh::find_collision(const btGImpactQuantizedBvh * boxset0, const btTransform & trans0,
-		const btGImpactQuantizedBvh * boxset1, const btTransform & trans1,
-		btPairSet & collision_pairs)
+void btGImpactQuantizedBvh::find_collision(const btGImpactQuantizedBvh* boxset0, const btTransform& trans0,
+										   const btGImpactQuantizedBvh* boxset1, const btTransform& trans1,
+										   btPairSet& collision_pairs)
 {
-
-	if(boxset0->getNodeCount()==0 || boxset1->getNodeCount()==0 ) return;
+	if (boxset0->getNodeCount() == 0 || boxset1->getNodeCount() == 0) return;
 
 	BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0;
 
-	trans_cache_1to0.calc_from_homogenic(trans0,trans1);
+	trans_cache_1to0.calc_from_homogenic(trans0, trans1);
 
 #ifdef TRI_COLLISION_PROFILING
 	bt_begin_gim02_q_tree_time();
-#endif //TRI_COLLISION_PROFILING
+#endif  //TRI_COLLISION_PROFILING
 
 	_find_quantized_collision_pairs_recursive(
-		boxset0,boxset1,
-		&collision_pairs,trans_cache_1to0,0,0,true);
+		boxset0, boxset1,
+		&collision_pairs, trans_cache_1to0, 0, 0, true);
 #ifdef TRI_COLLISION_PROFILING
 	bt_end_gim02_q_tree_time();
-#endif //TRI_COLLISION_PROFILING
-
+#endif  //TRI_COLLISION_PROFILING
 }
-
-