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