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