• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
1 /*! \file gim_box_set.h
2 \author Francisco Leon Najera
3 */
4 /*
5 This source file is part of GIMPACT Library.
6 
7 For the latest info, see http://gimpact.sourceforge.net/
8 
9 Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
10 email: projectileman@yahoo.com
11 
12 
13 This software is provided 'as-is', without any express or implied warranty.
14 In no event will the authors be held liable for any damages arising from the use of this software.
15 Permission is granted to anyone to use this software for any purpose,
16 including commercial applications, and to alter it and redistribute it freely,
17 subject to the following restrictions:
18 
19 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.
20 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
21 3. This notice may not be removed or altered from any source distribution.
22 */
23 #include "btGImpactBvh.h"
24 #include "LinearMath/btQuickprof.h"
25 
26 #ifdef TRI_COLLISION_PROFILING
27 
28 btClock g_tree_clock;
29 
30 float g_accum_tree_collision_time = 0;
31 int g_count_traversing = 0;
32 
33 
bt_begin_gim02_tree_time()34 void bt_begin_gim02_tree_time()
35 {
36 	g_tree_clock.reset();
37 }
38 
bt_end_gim02_tree_time()39 void bt_end_gim02_tree_time()
40 {
41 	g_accum_tree_collision_time += g_tree_clock.getTimeMicroseconds();
42 	g_count_traversing++;
43 }
44 
45 //! Gets the average time in miliseconds of tree collisions
getAverageTreeCollisionTime()46 float btGImpactBvh::getAverageTreeCollisionTime()
47 {
48 	if(g_count_traversing == 0) return 0;
49 
50 	float avgtime = g_accum_tree_collision_time;
51 	avgtime /= (float)g_count_traversing;
52 
53 	g_accum_tree_collision_time = 0;
54 	g_count_traversing = 0;
55 	return avgtime;
56 
57 //	float avgtime = g_count_traversing;
58 //	g_count_traversing = 0;
59 //	return avgtime;
60 
61 }
62 
63 #endif //TRI_COLLISION_PROFILING
64 
65 /////////////////////// btBvhTree /////////////////////////////////
66 
_calc_splitting_axis(GIM_BVH_DATA_ARRAY & primitive_boxes,int startIndex,int endIndex)67 int btBvhTree::_calc_splitting_axis(
68 	GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex)
69 {
70 
71 	int i;
72 
73 	btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
74 	btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.));
75 	int numIndices = endIndex-startIndex;
76 
77 	for (i=startIndex;i<endIndex;i++)
78 	{
79 		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
80 					 primitive_boxes[i].m_bound.m_min);
81 		means+=center;
82 	}
83 	means *= (btScalar(1.)/(btScalar)numIndices);
84 
85 	for (i=startIndex;i<endIndex;i++)
86 	{
87 		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
88 					 primitive_boxes[i].m_bound.m_min);
89 		btVector3 diff2 = center-means;
90 		diff2 = diff2 * diff2;
91 		variance += diff2;
92 	}
93 	variance *= (btScalar(1.)/	((btScalar)numIndices-1)	);
94 
95 	return variance.maxAxis();
96 }
97 
98 
_sort_and_calc_splitting_index(GIM_BVH_DATA_ARRAY & primitive_boxes,int startIndex,int endIndex,int splitAxis)99 int btBvhTree::_sort_and_calc_splitting_index(
100 	GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,
101 	int endIndex, int splitAxis)
102 {
103 	int i;
104 	int splitIndex =startIndex;
105 	int numIndices = endIndex - startIndex;
106 
107 	// average of centers
108 	btScalar splitValue = 0.0f;
109 
110 	btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
111 	for (i=startIndex;i<endIndex;i++)
112 	{
113 		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
114 					 primitive_boxes[i].m_bound.m_min);
115 		means+=center;
116 	}
117 	means *= (btScalar(1.)/(btScalar)numIndices);
118 
119 	splitValue = means[splitAxis];
120 
121 
122 	//sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
123 	for (i=startIndex;i<endIndex;i++)
124 	{
125 		btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
126 					 primitive_boxes[i].m_bound.m_min);
127 		if (center[splitAxis] > splitValue)
128 		{
129 			//swap
130 			primitive_boxes.swap(i,splitIndex);
131 			//swapLeafNodes(i,splitIndex);
132 			splitIndex++;
133 		}
134 	}
135 
136 	//if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex
137 	//otherwise the tree-building might fail due to stack-overflows in certain cases.
138 	//unbalanced1 is unsafe: it can cause stack overflows
139 	//bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1)));
140 
141 	//unbalanced2 should work too: always use center (perfect balanced trees)
142 	//bool unbalanced2 = true;
143 
144 	//this should be safe too:
145 	int rangeBalancedIndices = numIndices/3;
146 	bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices)));
147 
148 	if (unbalanced)
149 	{
150 		splitIndex = startIndex+ (numIndices>>1);
151 	}
152 
153 	btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex))));
154 
155 	return splitIndex;
156 
157 }
158 
159 
_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes,int startIndex,int endIndex)160 void btBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex)
161 {
162 	int curIndex = m_num_nodes;
163 	m_num_nodes++;
164 
165 	btAssert((endIndex-startIndex)>0);
166 
167 	if ((endIndex-startIndex)==1)
168 	{
169 	    //We have a leaf node
170 	    setNodeBound(curIndex,primitive_boxes[startIndex].m_bound);
171 		m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data);
172 
173 		return;
174 	}
175 	//calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
176 
177 	//split axis
178 	int splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex);
179 
180 	splitIndex = _sort_and_calc_splitting_index(
181 			primitive_boxes,startIndex,endIndex,
182 			splitIndex//split axis
183 			);
184 
185 
186 	//calc this node bounding box
187 
188 	btAABB node_bound;
189 	node_bound.invalidate();
190 
191 	for (int i=startIndex;i<endIndex;i++)
192 	{
193 		node_bound.merge(primitive_boxes[i].m_bound);
194 	}
195 
196 	setNodeBound(curIndex,node_bound);
197 
198 
199 	//build left branch
200 	_build_sub_tree(primitive_boxes, startIndex, splitIndex );
201 
202 
203 	//build right branch
204 	 _build_sub_tree(primitive_boxes, splitIndex ,endIndex);
205 
206 	m_node_array[curIndex].setEscapeIndex(m_num_nodes - curIndex);
207 
208 
209 }
210 
211 //! stackless build tree
build_tree(GIM_BVH_DATA_ARRAY & primitive_boxes)212 void btBvhTree::build_tree(
213 	GIM_BVH_DATA_ARRAY & primitive_boxes)
214 {
215 	// initialize node count to 0
216 	m_num_nodes = 0;
217 	// allocate nodes
218 	m_node_array.resize(primitive_boxes.size()*2);
219 
220 	_build_sub_tree(primitive_boxes, 0, primitive_boxes.size());
221 }
222 
223 ////////////////////////////////////class btGImpactBvh
224 
refit()225 void btGImpactBvh::refit()
226 {
227 	int nodecount = getNodeCount();
228 	while(nodecount--)
229 	{
230 		if(isLeafNode(nodecount))
231 		{
232 			btAABB leafbox;
233 			m_primitive_manager->get_primitive_box(getNodeData(nodecount),leafbox);
234 			setNodeBound(nodecount,leafbox);
235 		}
236 		else
237 		{
238 			//const GIM_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount);
239 			//get left bound
240 			btAABB bound;
241 			bound.invalidate();
242 
243 			btAABB temp_box;
244 
245 			int child_node = getLeftNode(nodecount);
246 			if(child_node)
247 			{
248 				getNodeBound(child_node,temp_box);
249 				bound.merge(temp_box);
250 			}
251 
252 			child_node = getRightNode(nodecount);
253 			if(child_node)
254 			{
255 				getNodeBound(child_node,temp_box);
256 				bound.merge(temp_box);
257 			}
258 
259 			setNodeBound(nodecount,bound);
260 		}
261 	}
262 }
263 
264 //! this rebuild the entire set
buildSet()265 void btGImpactBvh::buildSet()
266 {
267 	//obtain primitive boxes
268 	GIM_BVH_DATA_ARRAY primitive_boxes;
269 	primitive_boxes.resize(m_primitive_manager->get_primitive_count());
270 
271 	for (int i = 0;i<primitive_boxes.size() ;i++ )
272 	{
273 		 m_primitive_manager->get_primitive_box(i,primitive_boxes[i].m_bound);
274 		 primitive_boxes[i].m_data = i;
275 	}
276 
277 	m_box_tree.build_tree(primitive_boxes);
278 }
279 
280 //! returns the indices of the primitives in the m_primitive_manager
boxQuery(const btAABB & box,btAlignedObjectArray<int> & collided_results) const281 bool btGImpactBvh::boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const
282 {
283 	int curIndex = 0;
284 	int numNodes = getNodeCount();
285 
286 	while (curIndex < numNodes)
287 	{
288 		btAABB bound;
289 		getNodeBound(curIndex,bound);
290 
291 		//catch bugs in tree data
292 
293 		bool aabbOverlap = bound.has_collision(box);
294 		bool isleafnode = isLeafNode(curIndex);
295 
296 		if (isleafnode && aabbOverlap)
297 		{
298 			collided_results.push_back(getNodeData(curIndex));
299 		}
300 
301 		if (aabbOverlap || isleafnode)
302 		{
303 			//next subnode
304 			curIndex++;
305 		}
306 		else
307 		{
308 			//skip node
309 			curIndex+= getEscapeNodeIndex(curIndex);
310 		}
311 	}
312 	if(collided_results.size()>0) return true;
313 	return false;
314 }
315 
316 
317 
318 //! returns the indices of the primitives in the m_primitive_manager
rayQuery(const btVector3 & ray_dir,const btVector3 & ray_origin,btAlignedObjectArray<int> & collided_results) const319 bool btGImpactBvh::rayQuery(
320 	const btVector3 & ray_dir,const btVector3 & ray_origin ,
321 	btAlignedObjectArray<int> & collided_results) const
322 {
323 	int curIndex = 0;
324 	int numNodes = getNodeCount();
325 
326 	while (curIndex < numNodes)
327 	{
328 		btAABB bound;
329 		getNodeBound(curIndex,bound);
330 
331 		//catch bugs in tree data
332 
333 		bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir);
334 		bool isleafnode = isLeafNode(curIndex);
335 
336 		if (isleafnode && aabbOverlap)
337 		{
338 			collided_results.push_back(getNodeData( curIndex));
339 		}
340 
341 		if (aabbOverlap || isleafnode)
342 		{
343 			//next subnode
344 			curIndex++;
345 		}
346 		else
347 		{
348 			//skip node
349 			curIndex+= getEscapeNodeIndex(curIndex);
350 		}
351 	}
352 	if(collided_results.size()>0) return true;
353 	return false;
354 }
355 
356 
_node_collision(btGImpactBvh * boxset0,btGImpactBvh * boxset1,const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,int node0,int node1,bool complete_primitive_tests)357 SIMD_FORCE_INLINE bool _node_collision(
358 	btGImpactBvh * boxset0, btGImpactBvh * boxset1,
359 	const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
360 	int node0 ,int node1, bool complete_primitive_tests)
361 {
362 	btAABB box0;
363 	boxset0->getNodeBound(node0,box0);
364 	btAABB box1;
365 	boxset1->getNodeBound(node1,box1);
366 
367 	return box0.overlapping_trans_cache(box1,trans_cache_1to0,complete_primitive_tests );
368 //	box1.appy_transform_trans_cache(trans_cache_1to0);
369 //	return box0.has_collision(box1);
370 
371 }
372 
373 
374 //stackless recursive collision routine
_find_collision_pairs_recursive(btGImpactBvh * boxset0,btGImpactBvh * boxset1,btPairSet * collision_pairs,const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,int node0,int node1,bool complete_primitive_tests)375 static void _find_collision_pairs_recursive(
376 	btGImpactBvh * boxset0, btGImpactBvh * boxset1,
377 	btPairSet * collision_pairs,
378 	const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
379 	int node0, int node1, bool complete_primitive_tests)
380 {
381 
382 
383 
384 	if( _node_collision(
385 		boxset0,boxset1,trans_cache_1to0,
386 		node0,node1,complete_primitive_tests) ==false) return;//avoid colliding internal nodes
387 
388 	if(boxset0->isLeafNode(node0))
389 	{
390 		if(boxset1->isLeafNode(node1))
391 		{
392 			// collision result
393 			collision_pairs->push_pair(
394 				boxset0->getNodeData(node0),boxset1->getNodeData(node1));
395 			return;
396 		}
397 		else
398 		{
399 
400 			//collide left recursive
401 
402 			_find_collision_pairs_recursive(
403 								boxset0,boxset1,
404 								collision_pairs,trans_cache_1to0,
405 								node0,boxset1->getLeftNode(node1),false);
406 
407 			//collide right recursive
408 			_find_collision_pairs_recursive(
409 								boxset0,boxset1,
410 								collision_pairs,trans_cache_1to0,
411 								node0,boxset1->getRightNode(node1),false);
412 
413 
414 		}
415 	}
416 	else
417 	{
418 		if(boxset1->isLeafNode(node1))
419 		{
420 
421 			//collide left recursive
422 			_find_collision_pairs_recursive(
423 								boxset0,boxset1,
424 								collision_pairs,trans_cache_1to0,
425 								boxset0->getLeftNode(node0),node1,false);
426 
427 
428 			//collide right recursive
429 
430 			_find_collision_pairs_recursive(
431 								boxset0,boxset1,
432 								collision_pairs,trans_cache_1to0,
433 								boxset0->getRightNode(node0),node1,false);
434 
435 
436 		}
437 		else
438 		{
439 			//collide left0 left1
440 
441 
442 
443 			_find_collision_pairs_recursive(
444 				boxset0,boxset1,
445 				collision_pairs,trans_cache_1to0,
446 				boxset0->getLeftNode(node0),boxset1->getLeftNode(node1),false);
447 
448 			//collide left0 right1
449 
450 			_find_collision_pairs_recursive(
451 				boxset0,boxset1,
452 				collision_pairs,trans_cache_1to0,
453 				boxset0->getLeftNode(node0),boxset1->getRightNode(node1),false);
454 
455 
456 			//collide right0 left1
457 
458 			_find_collision_pairs_recursive(
459 				boxset0,boxset1,
460 				collision_pairs,trans_cache_1to0,
461 				boxset0->getRightNode(node0),boxset1->getLeftNode(node1),false);
462 
463 			//collide right0 right1
464 
465 			_find_collision_pairs_recursive(
466 				boxset0,boxset1,
467 				collision_pairs,trans_cache_1to0,
468 				boxset0->getRightNode(node0),boxset1->getRightNode(node1),false);
469 
470 		}// else if node1 is not a leaf
471 	}// else if node0 is not a leaf
472 }
473 
474 
find_collision(btGImpactBvh * boxset0,const btTransform & trans0,btGImpactBvh * boxset1,const btTransform & trans1,btPairSet & collision_pairs)475 void btGImpactBvh::find_collision(btGImpactBvh * boxset0, const btTransform & trans0,
476 		btGImpactBvh * boxset1, const btTransform & trans1,
477 		btPairSet & collision_pairs)
478 {
479 
480 	if(boxset0->getNodeCount()==0 || boxset1->getNodeCount()==0 ) return;
481 
482 	BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0;
483 
484 	trans_cache_1to0.calc_from_homogenic(trans0,trans1);
485 
486 #ifdef TRI_COLLISION_PROFILING
487 	bt_begin_gim02_tree_time();
488 #endif //TRI_COLLISION_PROFILING
489 
490 	_find_collision_pairs_recursive(
491 		boxset0,boxset1,
492 		&collision_pairs,trans_cache_1to0,0,0,true);
493 #ifdef TRI_COLLISION_PROFILING
494 	bt_end_gim02_tree_time();
495 #endif //TRI_COLLISION_PROFILING
496 
497 }
498 
499