Bullet Collision Detection & Physics Library
btGImpactQuantizedBvh.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 
24 #include "btGImpactQuantizedBvh.h"
25 #include "LinearMath/btQuickprof.h"
26 
27 #ifdef TRI_COLLISION_PROFILING
28 btClock g_q_tree_clock;
29 
30 
31 float g_q_accum_tree_collision_time = 0;
32 int g_q_count_traversing = 0;
33 
34 
35 void bt_begin_gim02_q_tree_time()
36 {
37  g_q_tree_clock.reset();
38 }
39 
40 void 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 
48 float 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 
181 void 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 
201  splitIndex = _sort_and_calc_splitting_index(
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;
291  primitive_boxes.resize(m_primitive_manager->get_primitive_count());
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 
303 bool 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 
int getRightNode(int nodeindex) const
void build_tree(GIM_BVH_DATA_ARRAY &primitive_boxes)
prototype functions for box tree management
void push_back(const T &_Val)
void calc_quantization(GIM_BVH_DATA_ARRAY &primitive_boxes, btScalar boundMargin=btScalar(1.0))
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
btVector3 m_max
int getNodeData(int nodeindex) const
virtual int get_primitive_count() const =0
#define btAssert(x)
Definition: btScalar.h:131
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
The btClock is a portable basic clock that measures accurate time in seconds, use for profiling...
Definition: btQuickprof.h:24
#define SIMD_FORCE_INLINE
Definition: btScalar.h:81
void reset()
Resets the initial reference time.
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
btVector3 m_min
void merge(const btAABB &box)
Merges a Box.
btQuantizedBvhTree m_box_tree
void swap(int index0, int index1)
bool isLeafNode(int nodeindex) const
tells if the node is a leaf
A pairset array.
Definition: btGImpactBvh.h:35
void push_pair(int index1, int index2)
Definition: btGImpactBvh.h:42
int size() const
return the number of elements in the array
void setNodeBound(int nodeindex, const btAABB &bound)
int getLeftNode(int nodeindex) const
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)
int getNodeCount() const
node count
unsigned long long int getTimeMicroseconds()
Returns the time in us since the last call to reset or since the Clock was created.
virtual void get_primitive_box(int prim_index, btAABB &primbox) const =0
Axis aligned box.
Class for transforming a model1 to the space of model0.
bool collide_ray(const btVector3 &vorigin, const btVector3 &vdir) const
Finds the Ray intersection parameter.
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
void bt_calc_quantization_parameters(btVector3 &outMinBound, btVector3 &outMaxBound, btVector3 &bvhQuantization, const btVector3 &srcMinBound, const btVector3 &srcMaxBound, btScalar quantizationMargin)
void resize(int newsize, const T &fillData=T())
btPrimitiveManagerBase * m_primitive_manager
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)
void invalidate()
bool boxQuery(const btAABB &box, btAlignedObjectArray< int > &collided_results) const
returns the indices of the primitives in the m_primitive_manager
int getEscapeNodeIndex(int nodeindex) const
int _sort_and_calc_splitting_index(GIM_BVH_DATA_ARRAY &primitive_boxes, int startIndex, int endIndex, int splitAxis)
void quantizePoint(unsigned short *quantizedpoint, const btVector3 &point) const
static void find_collision(const btGImpactQuantizedBvh *boxset1, const btTransform &trans1, const btGImpactQuantizedBvh *boxset2, const btTransform &trans2, btPairSet &collision_pairs)
void getNodeBound(int nodeindex, btAABB &bound) const
Structure for containing Boxes.
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
void calc_from_homogenic(const btTransform &trans0, const btTransform &trans1)
Calc the transformation relative 1 to 0. Inverts matrics by transposing.
void buildSet()
this rebuild the entire set
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
void _build_sub_tree(GIM_BVH_DATA_ARRAY &primitive_boxes, int startIndex, int endIndex)