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

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

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
34 void bt_begin_gim02_tree_time()
35 {
36 g_tree_clock.reset();
37 }
38
39 void bt_end_gim02_tree_time()
40 {
41 g_accum_tree_collision_time += g_tree_clock.getTimeMicroseconds();
42 g_count_traversing++;
43 }
44
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
66
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
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
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
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
224
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
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
281 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
319 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
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
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
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
btGImpactBvh.h
btGImpactBvh::find_collision
static void find_collision(btGImpactBvh *boxset1, const btTransform &trans1, btGImpactBvh *boxset2, const btTransform &trans2, btPairSet &collision_pairs)
Definition: btGImpactBvh.cpp:475
btAlignedObjectArray::push_back
void push_back(const T &_Val)
Definition: btAlignedObjectArray.h:274
btAlignedObjectArray< int >
btPrimitiveManagerBase::get_primitive_count
virtual int get_primitive_count() const =0
btAssert
#define btAssert(x)
Definition: btScalar.h:113
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:521
btGImpactBvh::refit
void refit()
Definition: btGImpactBvh.cpp:225
btClock::getTimeMicroseconds
unsigned long int getTimeMicroseconds()
Returns the time in us since the last call to reset or since the Clock was created.
Definition: btQuickprof.cpp:187
btClock
The btClock is a portable basic clock that measures accurate time in seconds, use for profiling...
Definition: btQuickprof.h:35
SIMD_FORCE_INLINE
#define SIMD_FORCE_INLINE
Definition: btScalar.h:63
btBvhTree::build_tree
void build_tree(GIM_BVH_DATA_ARRAY &primitive_boxes)
prototype functions for box tree management
Definition: btGImpactBvh.cpp:212
btClock::reset
void reset()
Resets the initial reference time.
Definition: btQuickprof.cpp:109
btAABB::merge
void merge(const btAABB &box)
Merges a Box.
Definition: btBoxCollision.h:391
_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:357
btBvhTree::_build_sub_tree
void _build_sub_tree(GIM_BVH_DATA_ARRAY &primitive_boxes, int startIndex, int endIndex)
Definition: btGImpactBvh.cpp:160
btAlignedObjectArray::swap
void swap(int index0, int index1)
Definition: btAlignedObjectArray.h:405
btPairSet
A pairset array.
Definition: btGImpactBvh.h:59
btPairSet::push_pair
void push_pair(int index1, int index2)
Definition: btGImpactBvh.h:66
btBvhTree::_calc_splitting_axis
int _calc_splitting_axis(GIM_BVH_DATA_ARRAY &primitive_boxes, int startIndex, int endIndex)
Definition: btGImpactBvh.cpp:67
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:99
btGImpactBvh::getNodeBound
void getNodeBound(int nodeindex, btAABB &bound) const
Definition: btGImpactBvh.h:349
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:155
btPrimitiveManagerBase::get_primitive_box
virtual void get_primitive_box(int prim_index, btAABB &primbox) const =0
btAABB
Axis aligned box.
Definition: btBoxCollision.h:229
BT_BOX_BOX_TRANSFORM_CACHE
Class for transforming a model1 to the space of model0.
Definition: btBoxCollision.h:161
btGImpactBvh::isLeafNode
bool isLeafNode(int nodeindex) const
tells if the node is a leaf
Definition: btGImpactBvh.h:339
btGImpactBvh::setNodeBound
void setNodeBound(int nodeindex, const btAABB &bound)
Definition: btGImpactBvh.h:354
btGImpactBvh::m_box_tree
btBvhTree m_box_tree
Definition: btGImpactBvh.h:253
btAABB::collide_ray
bool collide_ray(const btVector3 &vorigin, const btVector3 &vdir) const
Finds the Ray intersection parameter.
Definition: btBoxCollision.h:454
btGImpactBvh::buildSet
void buildSet()
this rebuild the entire set
Definition: btGImpactBvh.cpp:265
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btGImpactBvh::getEscapeNodeIndex
int getEscapeNodeIndex(int nodeindex) const
Definition: btGImpactBvh.h:370
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btGImpactBvh::m_primitive_manager
btPrimitiveManagerBase * m_primitive_manager
Definition: btGImpactBvh.h:254
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:281
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:218
btQuickprof.h
btGImpactBvh::getNodeData
int getNodeData(int nodeindex) const
Definition: btGImpactBvh.h:344
btAABB::invalidate
void invalidate()
Definition: btBoxCollision.h:289
btGImpactBvh
Structure for containing Boxes.
Definition: btGImpactBvh.h:250
btGImpactBvh::getNodeCount
int getNodeCount() const
node count
Definition: btGImpactBvh.h:333
GIM_BVH_DATA_ARRAY
Definition: btGImpactBvh.h:130
btGImpactBvh::getLeftNode
int getLeftNode(int nodeindex) const
Definition: btGImpactBvh.h:360
btGImpactBvh::getRightNode
int getRightNode(int nodeindex) const
Definition: btGImpactBvh.h:365
_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:375
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:319
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278
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:194
btAABB::has_collision
bool has_collision(const btAABB &other) const
Definition: btBoxCollision.h:435
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:475