MultiSource/Benchmarks/Bullet/btGImpactQuantizedBvh.cpp - third_party/llvm-test-suite - Git at Google

 /*! \file gim_box_set.h
 \author Francisco Len Nßjera
 */
 /*
 This source file is part of GIMPACT Library.

 For the latest info, see http://gimpact.sourceforge.net/

 Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
 email: projectileman@yahoo.com


 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
 Permission is granted to anyone to use this software for any purpose,
 including commercial applications, and to alter it and redistribute it freely,
 subject to the following restrictions:

 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
 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 "BulletCollision/Gimpact/btGImpactQuantizedBvh.h"
 #include "LinearMath/btQuickprof.h"

 #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();
 }

 void bt_end_gim02_q_tree_time()
 {
 	g_q_accum_tree_collision_time += g_q_tree_clock.getTimeMicroseconds();
 	g_q_count_traversing++;
 }


 //! Gets the average time in miliseconds of tree collisions
 float btGImpactQuantizedBvh::getAverageTreeCollisionTime()
 {
 	if(g_q_count_traversing == 0) return 0;

 	float avgtime = g_q_accum_tree_collision_time;
 	avgtime /= (float)g_q_count_traversing;

 	g_q_accum_tree_collision_time = 0;
 	g_q_count_traversing = 0;
 	return avgtime;

 //	float avgtime = g_q_count_traversing;
 //	g_q_count_traversing = 0;
 //	return avgtime;

 }

 #endif //TRI_COLLISION_PROFILING

 /////////////////////// btQuantizedBvhTree /////////////////////////////////

 void btQuantizedBvhTree::calc_quantization(
 	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++ )
 	{
 		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);

 }


 int btQuantizedBvhTree::_calc_splitting_axis(
 	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;

 	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;
 	}
 	means *= (btScalar(1.)/(btScalar)numIndices);

 	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;
 		diff2 = diff2 * diff2;
 		variance += diff2;
 	}
 	variance *= (btScalar(1.)/	((btScalar)numIndices-1)	);

 	return variance.maxAxis();
 }


 int btQuantizedBvhTree::_sort_and_calc_splitting_index(
 	GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,
 	int endIndex, int splitAxis)
 {
 	int i;
 	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 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);

 	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++)
 	{
 		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);
 			//swapLeafNodes(i,splitIndex);
 			splitIndex++;
 		}
 	}

 	//if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex
 	//otherwise the tree-building might fail due to stack-overflows in certain cases.
 	//unbalanced1 is unsafe: it can cause stack overflows
 	//bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1)));

 	//unbalanced2 should work too: always use center (perfect balanced trees)
 	//bool unbalanced2 = true;

 	//this should be safe too:
 	int rangeBalancedIndices = numIndices/3;
 	bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices)));

 	if (unbalanced)
 	{
 		splitIndex = startIndex+ (numIndices>>1);
 	}

 	btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex))));

 	return splitIndex;

 }


 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);

 	if ((endIndex-startIndex)==1)
 	{
 	    //We have a leaf node
 	    setNodeBound(curIndex,primitive_boxes[startIndex].m_bound);
 		m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data);

 		return;
 	}
 	//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);

 	splitIndex = _sort_and_calc_splitting_index(
 			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++)
 	{
 		node_bound.merge(primitive_boxes[i].m_bound);
 	}

 	setNodeBound(curIndex,node_bound);


 	//build left branch
 	_build_sub_tree(primitive_boxes, startIndex, splitIndex );


 	//build right branch
 	 _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)
 {
 	calc_quantization(primitive_boxes);
 	// initialize node count to 0
 	m_num_nodes = 0;
 	// allocate nodes
 	m_node_array.resize(primitive_boxes.size()*2);

 	_build_sub_tree(primitive_boxes, 0, primitive_boxes.size());
 }

 ////////////////////////////////////class btGImpactQuantizedBvh

 void btGImpactQuantizedBvh::refit()
 {
 	int nodecount = getNodeCount();
 	while(nodecount--)
 	{
 		if(isLeafNode(nodecount))
 		{
 			btAABB leafbox;
 			m_primitive_manager->get_primitive_box(getNodeData(nodecount),leafbox);
 			setNodeBound(nodecount,leafbox);
 		}
 		else
 		{
 			//const GIM_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount);
 			//get left bound
 			btAABB bound;
 			bound.invalidate();

 			btAABB temp_box;

 			int child_node = getLeftNode(nodecount);
 			if(child_node)
 			{
 				getNodeBound(child_node,temp_box);
 				bound.merge(temp_box);
 			}

 			child_node = getRightNode(nodecount);
 			if(child_node)
 			{
 				getNodeBound(child_node,temp_box);
 				bound.merge(temp_box);
 			}

 			setNodeBound(nodecount,bound);
 		}
 	}
 }

 //! this rebuild the entire set
 void btGImpactQuantizedBvh::buildSet()
 {
 	//obtain primitive boxes
 	GIM_BVH_DATA_ARRAY primitive_boxes;
 	primitive_boxes.resize(m_primitive_manager->get_primitive_count());

 	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_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
 {
 	int curIndex = 0;
 	int numNodes = getNodeCount();

 	//quantize box

 	unsigned short quantizedMin[3];
 	unsigned short quantizedMax[3];

 	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 isleafnode = isLeafNode(curIndex);

 		if (isleafnode && aabbOverlap)
 		{
 			collided_results.push_back(getNodeData(curIndex));
 		}

 		if (aabbOverlap || isleafnode)
 		{
 			//next subnode
 			curIndex++;
 		}
 		else
 		{
 			//skip node
 			curIndex+= getEscapeNodeIndex(curIndex);
 		}
 	}
 	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
 {
 	int curIndex = 0;
 	int numNodes = getNodeCount();

 	while (curIndex < numNodes)
 	{
 		btAABB bound;
 		getNodeBound(curIndex,bound);

 		//catch bugs in tree data

 		bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir);
 		bool isleafnode = isLeafNode(curIndex);

 		if (isleafnode && aabbOverlap)
 		{
 			collided_results.push_back(getNodeData( curIndex));
 		}

 		if (aabbOverlap || isleafnode)
 		{
 			//next subnode
 			curIndex++;
 		}
 		else
 		{
 			//skip node
 			curIndex+= getEscapeNodeIndex(curIndex);
 		}
 	}
 	if(collided_results.size()>0) return true;
 	return false;
 }


 SIMD_FORCE_INLINE bool _quantized_node_collision(
 	btGImpactQuantizedBvh * boxset0, 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);
 	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);

 }


 //stackless recursive collision routine
 static void _find_quantized_collision_pairs_recursive(
 	btGImpactQuantizedBvh * boxset0, 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(boxset0->isLeafNode(node0))
 	{
 		if(boxset1->isLeafNode(node1))
 		{
 			// collision result
 			collision_pairs->push_pair(
 				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);

 			//collide right recursive
 			_find_quantized_collision_pairs_recursive(
 								boxset0,boxset1,
 								collision_pairs,trans_cache_1to0,
 								node0,boxset1->getRightNode(node1),false);


 		}
 	}
 	else
 	{
 		if(boxset1->isLeafNode(node1))
 		{

 			//collide left recursive
 			_find_quantized_collision_pairs_recursive(
 								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);


 		}
 		else
 		{
 			//collide left0 left1


 			_find_quantized_collision_pairs_recursive(
 				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);


 			//collide right0 left1

 			_find_quantized_collision_pairs_recursive(
 				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);

 		}// else if node1 is not a leaf
 	}// else if node0 is not a leaf
 }


 void btGImpactQuantizedBvh::find_collision(btGImpactQuantizedBvh * boxset0, const btTransform & trans0,
 		btGImpactQuantizedBvh * boxset1, const btTransform & trans1,
 		btPairSet & collision_pairs)
 {

 	if(boxset0->getNodeCount()==0 || boxset1->getNodeCount()==0 ) return;

 	BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0;

 	trans_cache_1to0.calc_from_homogenic(trans0,trans1);

 #ifdef TRI_COLLISION_PROFILING
 	bt_begin_gim02_q_tree_time();
 #endif //TRI_COLLISION_PROFILING

 	_find_quantized_collision_pairs_recursive(
 		boxset0,boxset1,
 		&collision_pairs,trans_cache_1to0,0,0,true);
 #ifdef TRI_COLLISION_PROFILING
 	bt_end_gim02_q_tree_time();
 #endif //TRI_COLLISION_PROFILING

 }
	/*! \file gim_box_set.h
	\author Francisco Len Nßjera
	*/
	/*
	This source file is part of GIMPACT Library.

	For the latest info, see http://gimpact.sourceforge.net/

	Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
	email: projectileman@yahoo.com


	This software is provided 'as-is', without any express or implied warranty.
	In no event will the authors be held liable for any damages arising from the use of this software.
	Permission is granted to anyone to use this software for any purpose,
	including commercial applications, and to alter it and redistribute it freely,
	subject to the following restrictions:

	1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
	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 "BulletCollision/Gimpact/btGImpactQuantizedBvh.h"
	#include "LinearMath/btQuickprof.h"

	#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();
	}

	void bt_end_gim02_q_tree_time()
	{
	g_q_accum_tree_collision_time += g_q_tree_clock.getTimeMicroseconds();
	g_q_count_traversing++;
	}


	//! Gets the average time in miliseconds of tree collisions
	float btGImpactQuantizedBvh::getAverageTreeCollisionTime()
	{
	if(g_q_count_traversing == 0) return 0;

	float avgtime = g_q_accum_tree_collision_time;
	avgtime /= (float)g_q_count_traversing;

	g_q_accum_tree_collision_time = 0;
	g_q_count_traversing = 0;
	return avgtime;

	// float avgtime = g_q_count_traversing;
	// g_q_count_traversing = 0;
	// return avgtime;

	}

	#endif //TRI_COLLISION_PROFILING

	/////////////////////// btQuantizedBvhTree /////////////////////////////////

	void btQuantizedBvhTree::calc_quantization(
	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++ )
	{
	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);

	}



	int btQuantizedBvhTree::_calc_splitting_axis(
	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;

	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;
	}
	means *= (btScalar(1.)/(btScalar)numIndices);

	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;
	diff2 = diff2 * diff2;
	variance += diff2;
	}
	variance *= (btScalar(1.)/ ((btScalar)numIndices-1) );

	return variance.maxAxis();
	}


	int btQuantizedBvhTree::_sort_and_calc_splitting_index(
	GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,
	int endIndex, int splitAxis)
	{
	int i;
	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 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);

	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++)
	{
	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);
	//swapLeafNodes(i,splitIndex);
	splitIndex++;
	}
	}

	//if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex
	//otherwise the tree-building might fail due to stack-overflows in certain cases.
	//unbalanced1 is unsafe: it can cause stack overflows
	//bool unbalanced1 = ((splitIndex==startIndex) \|\| (splitIndex == (endIndex-1)));

	//unbalanced2 should work too: always use center (perfect balanced trees)
	//bool unbalanced2 = true;

	//this should be safe too:
	int rangeBalancedIndices = numIndices/3;
	bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) \|\| (splitIndex >=(endIndex-1-rangeBalancedIndices)));

	if (unbalanced)
	{
	splitIndex = startIndex+ (numIndices>>1);
	}

	btAssert(!((splitIndex==startIndex) \|\| (splitIndex == (endIndex))));

	return splitIndex;

	}


	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);

	if ((endIndex-startIndex)==1)
	{
	//We have a leaf node
	setNodeBound(curIndex,primitive_boxes[startIndex].m_bound);
	m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data);

	return;
	}
	//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);

	splitIndex = _sort_and_calc_splitting_index(
	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++)
	{
	node_bound.merge(primitive_boxes[i].m_bound);
	}

	setNodeBound(curIndex,node_bound);


	//build left branch
	_build_sub_tree(primitive_boxes, startIndex, splitIndex );


	//build right branch
	_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)
	{
	calc_quantization(primitive_boxes);
	// initialize node count to 0
	m_num_nodes = 0;
	// allocate nodes
	m_node_array.resize(primitive_boxes.size()*2);

	_build_sub_tree(primitive_boxes, 0, primitive_boxes.size());
	}

	////////////////////////////////////class btGImpactQuantizedBvh

	void btGImpactQuantizedBvh::refit()
	{
	int nodecount = getNodeCount();
	while(nodecount--)
	{
	if(isLeafNode(nodecount))
	{
	btAABB leafbox;
	m_primitive_manager->get_primitive_box(getNodeData(nodecount),leafbox);
	setNodeBound(nodecount,leafbox);
	}
	else
	{
	//const GIM_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount);
	//get left bound
	btAABB bound;
	bound.invalidate();

	btAABB temp_box;

	int child_node = getLeftNode(nodecount);
	if(child_node)
	{
	getNodeBound(child_node,temp_box);
	bound.merge(temp_box);
	}

	child_node = getRightNode(nodecount);
	if(child_node)
	{
	getNodeBound(child_node,temp_box);
	bound.merge(temp_box);
	}

	setNodeBound(nodecount,bound);
	}
	}
	}

	//! this rebuild the entire set
	void btGImpactQuantizedBvh::buildSet()
	{
	//obtain primitive boxes
	GIM_BVH_DATA_ARRAY primitive_boxes;
	primitive_boxes.resize(m_primitive_manager->get_primitive_count());

	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_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
	{
	int curIndex = 0;
	int numNodes = getNodeCount();

	//quantize box

	unsigned short quantizedMin[3];
	unsigned short quantizedMax[3];

	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 isleafnode = isLeafNode(curIndex);

	if (isleafnode && aabbOverlap)
	{
	collided_results.push_back(getNodeData(curIndex));
	}

	if (aabbOverlap \|\| isleafnode)
	{
	//next subnode
	curIndex++;
	}
	else
	{
	//skip node
	curIndex+= getEscapeNodeIndex(curIndex);
	}
	}
	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
	{
	int curIndex = 0;
	int numNodes = getNodeCount();

	while (curIndex < numNodes)
	{
	btAABB bound;
	getNodeBound(curIndex,bound);

	//catch bugs in tree data

	bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir);
	bool isleafnode = isLeafNode(curIndex);

	if (isleafnode && aabbOverlap)
	{
	collided_results.push_back(getNodeData( curIndex));
	}

	if (aabbOverlap \|\| isleafnode)
	{
	//next subnode
	curIndex++;
	}
	else
	{
	//skip node
	curIndex+= getEscapeNodeIndex(curIndex);
	}
	}
	if(collided_results.size()>0) return true;
	return false;
	}


	SIMD_FORCE_INLINE bool _quantized_node_collision(
	btGImpactQuantizedBvh * boxset0, 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);
	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);

	}


	//stackless recursive collision routine
	static void _find_quantized_collision_pairs_recursive(
	btGImpactQuantizedBvh * boxset0, 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(boxset0->isLeafNode(node0))
	{
	if(boxset1->isLeafNode(node1))
	{
	// collision result
	collision_pairs->push_pair(
	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);

	//collide right recursive
	_find_quantized_collision_pairs_recursive(
	boxset0,boxset1,
	collision_pairs,trans_cache_1to0,
	node0,boxset1->getRightNode(node1),false);


	}
	}
	else
	{
	if(boxset1->isLeafNode(node1))
	{

	//collide left recursive
	_find_quantized_collision_pairs_recursive(
	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);


	}
	else
	{
	//collide left0 left1



	_find_quantized_collision_pairs_recursive(
	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);


	//collide right0 left1

	_find_quantized_collision_pairs_recursive(
	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);

	}// else if node1 is not a leaf
	}// else if node0 is not a leaf
	}


	void btGImpactQuantizedBvh::find_collision(btGImpactQuantizedBvh * boxset0, const btTransform & trans0,
	btGImpactQuantizedBvh * boxset1, const btTransform & trans1,
	btPairSet & collision_pairs)
	{

	if(boxset0->getNodeCount()==0 \|\| boxset1->getNodeCount()==0 ) return;

	BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0;

	trans_cache_1to0.calc_from_homogenic(trans0,trans1);

	#ifdef TRI_COLLISION_PROFILING
	bt_begin_gim02_q_tree_time();
	#endif //TRI_COLLISION_PROFILING

	_find_quantized_collision_pairs_recursive(
	boxset0,boxset1,
	&collision_pairs,trans_cache_1to0,0,0,true);
	#ifdef TRI_COLLISION_PROFILING
	bt_end_gim02_q_tree_time();
	#endif //TRI_COLLISION_PROFILING

	}