Bullet Collision Detection & Physics Library
btGImpactBvh.cpp
Go to the documentation of this file.
1
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#include "btGImpactBvh.h"
25
26#ifdef TRI_COLLISION_PROFILING
27
28btClock g_tree_clock;
29
30float g_accum_tree_collision_time = 0;
31int g_count_traversing = 0;
32
33
34void bt_begin_gim02_tree_time()
35{
36 g_tree_clock.reset();
37}
38
39void bt_end_gim02_tree_time()
40{
41 g_accum_tree_collision_time += g_tree_clock.getTimeMicroseconds();
42 g_count_traversing++;
43}
44
46float 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
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
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
160void 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
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
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
226{
227 int nodecount = getNodeCount();
228 while(nodecount--)
229 {
230 if(isLeafNode(nodecount))
231 {
232 btAABB 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
266{
267 //obtain primitive boxes
268 GIM_BVH_DATA_ARRAY primitive_boxes;
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
281bool 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
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
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
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
403 boxset0,boxset1,
404 collision_pairs,trans_cache_1to0,
405 node0,boxset1->getLeftNode(node1),false);
406
407 //collide right 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
423 boxset0,boxset1,
424 collision_pairs,trans_cache_1to0,
425 boxset0->getLeftNode(node0),node1,false);
426
427
428 //collide right recursive
429
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
444 boxset0,boxset1,
445 collision_pairs,trans_cache_1to0,
446 boxset0->getLeftNode(node0),boxset1->getLeftNode(node1),false);
447
448 //collide left0 right1
449
451 boxset0,boxset1,
452 collision_pairs,trans_cache_1to0,
453 boxset0->getLeftNode(node0),boxset1->getRightNode(node1),false);
454
455
456 //collide right0 left1
457
459 boxset0,boxset1,
460 collision_pairs,trans_cache_1to0,
461 boxset0->getRightNode(node0),boxset1->getLeftNode(node1),false);
462
463 //collide right0 right1
464
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
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
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
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)
bool _node_collision(btGImpactBvh *boxset0, btGImpactBvh *boxset1, const BT_BOX_BOX_TRANSFORM_CACHE &trans_cache_1to0, int node0, int node1, bool complete_primitive_tests)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
#define SIMD_FORCE_INLINE
Definition: btScalar.h:81
#define btAssert(x)
Definition: btScalar.h:131
Class for transforming a model1 to the space of model0.
void calc_from_homogenic(const btTransform &trans0, const btTransform &trans1)
Calc the transformation relative 1 to 0. Inverts matrics by transposing.
Axis aligned box.
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
bool collide_ray(const btVector3 &vorigin, const btVector3 &vdir) const
Finds the Ray intersection parameter.
void merge(const btAABB &box)
Merges a Box.
bool has_collision(const btAABB &other) const
void invalidate()
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
int size() const
return the number of elements in the array
void resize(int newsize, const T &fillData=T())
void swap(int index0, int index1)
void push_back(const T &_Val)
void _build_sub_tree(GIM_BVH_DATA_ARRAY &primitive_boxes, int startIndex, int endIndex)
int _calc_splitting_axis(GIM_BVH_DATA_ARRAY &primitive_boxes, int startIndex, int endIndex)
GIM_BVH_TREE_NODE_ARRAY m_node_array
Definition: btGImpactBvh.h:70
int m_num_nodes
Definition: btGImpactBvh.h:69
void setNodeBound(int nodeindex, const btAABB &bound)
Definition: btGImpactBvh.h:117
int _sort_and_calc_splitting_index(GIM_BVH_DATA_ARRAY &primitive_boxes, int startIndex, int endIndex, int splitAxis)
void build_tree(GIM_BVH_DATA_ARRAY &primitive_boxes)
prototype functions for box tree management
The btClock is a portable basic clock that measures accurate time in seconds, use for profiling.
Definition: btQuickprof.h:25
void reset()
Resets the initial reference time.
unsigned long long int getTimeMicroseconds()
Returns the time in us since the last call to reset or since the Clock was created.
Structure for containing Boxes.
Definition: btGImpactBvh.h:174
void buildSet()
this rebuild the entire set
int getRightNode(int nodeindex) const
Definition: btGImpactBvh.h:288
int getNodeCount() const
node count
Definition: btGImpactBvh.h:256
bool isLeafNode(int nodeindex) const
tells if the node is a leaf
Definition: btGImpactBvh.h:262
int getLeftNode(int nodeindex) const
Definition: btGImpactBvh.h:283
bool boxQuery(const btAABB &box, btAlignedObjectArray< int > &collided_results) const
returns the indices of the primitives in the m_primitive_manager
void getNodeBound(int nodeindex, btAABB &bound) const
Definition: btGImpactBvh.h:272
int getEscapeNodeIndex(int nodeindex) const
Definition: btGImpactBvh.h:293
int getNodeData(int nodeindex) const
Definition: btGImpactBvh.h:267
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
btBvhTree m_box_tree
Definition: btGImpactBvh.h:176
btPrimitiveManagerBase * m_primitive_manager
Definition: btGImpactBvh.h:177
void setNodeBound(int nodeindex, const btAABB &bound)
Definition: btGImpactBvh.h:277
static void find_collision(btGImpactBvh *boxset1, const btTransform &trans1, btGImpactBvh *boxset2, const btTransform &trans2, btPairSet &collision_pairs)
A pairset array.
Definition: btGImpactBvh.h:36
void push_pair(int index1, int index2)
Definition: btGImpactBvh.h:42
virtual int get_primitive_count() const =0
virtual void get_primitive_box(int prim_index, btAABB &primbox) const =0
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:84
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:487