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