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