Bullet Collision Detection & Physics Library: btGImpactBvh.cpp Source File

Bullet Collision Detection & Physics Library

btGImpactBvh.cpp
Go to the documentation of this file.
1
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 void bt_begin_gim02_tree_time()
34 {
35 g_tree_clock.reset();
36 }
37
38 void bt_end_gim02_tree_time()
39 {
40 g_accum_tree_collision_time += g_tree_clock.getTimeMicroseconds();
41 g_count_traversing++;
42 }
43
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
64
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
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
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
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
212
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
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
269 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
305 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
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
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
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 }
btGImpactBvh::refit
void refit()
Definition: btGImpactBvh.cpp:213
btAABB::overlapping_trans_cache
bool overlapping_trans_cache(const btAABB &box, const BT_BOX_BOX_TRANSFORM_CACHE &transcache, bool fulltest) const
transcache is the transformation cache from box to this AABB
Definition: btBoxCollision.h:502
btBvhTree::setNodeBound
void setNodeBound(int nodeindex, const btAABB &bound)
Definition: btGImpactBvh.h:114
btGImpactBvh::buildSet
void buildSet()
this rebuild the entire set
Definition: btGImpactBvh.cpp:253
btGImpactBvh::m_box_tree
btBvhTree m_box_tree
Definition: btGImpactBvh.h:170
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btClock::getTimeMicroseconds
unsigned long long int getTimeMicroseconds()
Returns the time in us since the last call to reset or since the Clock was created.
Definition: btQuickprof.cpp:171
btBvhTree::_calc_splitting_axis
int _calc_splitting_axis(GIM_BVH_DATA_ARRAY &primitive_boxes, int startIndex, int endIndex)
Definition: btGImpactBvh.cpp:65
_node_collision
bool _node_collision(btGImpactBvh *boxset0, btGImpactBvh *boxset1, const BT_BOX_BOX_TRANSFORM_CACHE &trans_cache_1to0, int node0, int node1, bool complete_primitive_tests)
Definition: btGImpactBvh.cpp:342
btBvhTree::m_node_array
GIM_BVH_TREE_NODE_ARRAY m_node_array
Definition: btGImpactBvh.h:65
btGImpactBvh::getLeftNode
int getLeftNode(int nodeindex) const
Definition: btGImpactBvh.h:275
btGImpactBvh::getEscapeNodeIndex
int getEscapeNodeIndex(int nodeindex) const
Definition: btGImpactBvh.h:285
btVector3::maxAxis
int maxAxis() const
Return the axis with the largest value Note return values are 0,1,2 for x, y, or z.
Definition: btVector3.h:477
btGImpactBvh::getRightNode
int getRightNode(int nodeindex) const
Definition: btGImpactBvh.h:280
btGImpactBvh::m_primitive_manager
btPrimitiveManagerBase * m_primitive_manager
Definition: btGImpactBvh.h:171
btPairSet
A pairset array.
Definition: btGImpactBvh.h:35
btBvhTree::m_num_nodes
int m_num_nodes
Definition: btGImpactBvh.h:64
btGImpactBvh::setNodeBound
void setNodeBound(int nodeindex, const btAABB &bound)
Definition: btGImpactBvh.h:270
btAABB
Axis aligned box.
Definition: btBoxCollision.h:216
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btGImpactBvh::boxQuery
bool boxQuery(const btAABB &box, btAlignedObjectArray< int > &collided_results) const
returns the indices of the primitives in the m_primitive_manager
Definition: btGImpactBvh.cpp:269
btAlignedObjectArray::swap
void swap(int index0, int index1)
Definition: btAlignedObjectArray.h:382
btGImpactBvh::getNodeCount
int getNodeCount() const
node count
Definition: btGImpactBvh.h:249
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:203
btGImpactBvh::find_collision
static void find_collision(btGImpactBvh *boxset1, const btTransform &trans1, btGImpactBvh *boxset2, const btTransform &trans2, btPairSet &collision_pairs)
Definition: btGImpactBvh.cpp:444
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btAABB::invalidate
void invalidate()
Definition: btBoxCollision.h:273
btGImpactBvh::getNodeBound
void getNodeBound(int nodeindex, btAABB &bound) const
Definition: btGImpactBvh.h:265
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btBvhTree::build_tree
void build_tree(GIM_BVH_DATA_ARRAY &primitive_boxes)
prototype functions for box tree management
Definition: btGImpactBvh.cpp:200
btGImpactBvh.h
btAlignedObjectArray< int >
SIMD_FORCE_INLINE
#define SIMD_FORCE_INLINE
Definition: btScalar.h:98
btClock::reset
void reset()
Resets the initial reference time.
Definition: btQuickprof.cpp:112
BT_BOX_BOX_TRANSFORM_CACHE::calc_from_homogenic
void calc_from_homogenic(const btTransform &trans0, const btTransform &trans1)
Calc the transformation relative 1 to 0. Inverts matrics by transposing.
Definition: btBoxCollision.h:182
btGImpactBvh::getNodeData
int getNodeData(int nodeindex) const
Definition: btGImpactBvh.h:260
btAABB::merge
void merge(const btAABB &box)
Merges a Box.
Definition: btBoxCollision.h:374
btClock
The btClock is a portable basic clock that measures accurate time in seconds, use for profiling.
Definition: btQuickprof.h:23
_find_collision_pairs_recursive
static void _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)
Definition: btGImpactBvh.cpp:358
BT_BOX_BOX_TRANSFORM_CACHE
Class for transforming a model1 to the space of model0.
Definition: btBoxCollision.h:153
btGImpactBvh::isLeafNode
bool isLeafNode(int nodeindex) const
tells if the node is a leaf
Definition: btGImpactBvh.h:255
btQuickprof.h
btAABB::collide_ray
bool collide_ray(const btVector3 &vorigin, const btVector3 &vdir) const
Finds the Ray intersection parameter.
Definition: btBoxCollision.h:436
btGImpactBvh
Structure for containing Boxes.
Definition: btGImpactBvh.h:168
btGImpactBvh::rayQuery
bool rayQuery(const btVector3 &ray_dir, const btVector3 &ray_origin, btAlignedObjectArray< int > &collided_results) const
returns the indices of the primitives in the m_primitive_manager
Definition: btGImpactBvh.cpp:305
btBvhTree::_build_sub_tree
void _build_sub_tree(GIM_BVH_DATA_ARRAY &primitive_boxes, int startIndex, int endIndex)
Definition: btGImpactBvh.cpp:153
btAlignedObjectArray::m_data
T * m_data
Definition: btAlignedObjectArray.h:51
btBvhTree::_sort_and_calc_splitting_index
int _sort_and_calc_splitting_index(GIM_BVH_DATA_ARRAY &primitive_boxes, int startIndex, int endIndex, int splitAxis)
Definition: btGImpactBvh.cpp:95
GIM_BVH_DATA_ARRAY
Definition: btGImpactBvh.h:53
btPrimitiveManagerBase::get_primitive_box
virtual void get_primitive_box(int prim_index, btAABB &primbox) const =0
btAlignedObjectArray::push_back
void push_back(const T &_Val)
Definition: btAlignedObjectArray.h:257
btPrimitiveManagerBase::get_primitive_count
virtual int get_primitive_count() const =0
btAABB::has_collision
bool has_collision(const btAABB &other) const
Definition: btBoxCollision.h:417
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:142
btPairSet::push_pair
void push_pair(int index1, int index2)
Definition: btGImpactBvh.h:41