Blender  V3.3
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 float g_q_accum_tree_collision_time = 0;
31 int g_q_count_traversing = 0;
32 
33 void bt_begin_gim02_q_tree_time()
34 {
35  g_q_tree_clock.reset();
36 }
37 
38 void bt_end_gim02_q_tree_time()
39 {
40  g_q_accum_tree_collision_time += g_q_tree_clock.getTimeMicroseconds();
41  g_q_count_traversing++;
42 }
43 
45 float btGImpactQuantizedBvh::getAverageTreeCollisionTime()
46 {
47  if (g_q_count_traversing == 0) return 0;
48 
49  float avgtime = g_q_accum_tree_collision_time;
50  avgtime /= (float)g_q_count_traversing;
51 
52  g_q_accum_tree_collision_time = 0;
53  g_q_count_traversing = 0;
54  return avgtime;
55 
56  // float avgtime = g_q_count_traversing;
57  // g_q_count_traversing = 0;
58  // return avgtime;
59 }
60 
61 #endif //TRI_COLLISION_PROFILING
62 
64 
66  GIM_BVH_DATA_ARRAY& primitive_boxes, btScalar boundMargin)
67 {
68  //calc globa box
69  btAABB global_bound;
70  global_bound.invalidate();
71 
72  for (int i = 0; i < primitive_boxes.size(); i++)
73  {
74  global_bound.merge(primitive_boxes[i].m_bound);
75  }
76 
78  m_global_bound.m_min, m_global_bound.m_max, m_bvhQuantization, global_bound.m_min, global_bound.m_max, boundMargin);
79 }
80 
82  GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex)
83 {
84  int i;
85 
86  btVector3 means(btScalar(0.), btScalar(0.), btScalar(0.));
87  btVector3 variance(btScalar(0.), btScalar(0.), btScalar(0.));
88  int numIndices = endIndex - startIndex;
89 
90  for (i = startIndex; i < endIndex; i++)
91  {
92  btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max +
93  primitive_boxes[i].m_bound.m_min);
94  means += center;
95  }
96  means *= (btScalar(1.) / (btScalar)numIndices);
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  btVector3 diff2 = center - means;
103  diff2 = diff2 * diff2;
104  variance += diff2;
105  }
106  variance *= (btScalar(1.) / ((btScalar)numIndices - 1));
107 
108  return variance.maxAxis();
109 }
110 
112  GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex,
113  int endIndex, int splitAxis)
114 {
115  int i;
116  int splitIndex = startIndex;
117  int numIndices = endIndex - startIndex;
118 
119  // average of centers
120  btScalar splitValue = 0.0f;
121 
122  btVector3 means(btScalar(0.), btScalar(0.), btScalar(0.));
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  means += center;
128  }
129  means *= (btScalar(1.) / (btScalar)numIndices);
130 
131  splitValue = means[splitAxis];
132 
133  //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
134  for (i = startIndex; i < endIndex; i++)
135  {
136  btVector3 center = btScalar(0.5) * (primitive_boxes[i].m_bound.m_max +
137  primitive_boxes[i].m_bound.m_min);
138  if (center[splitAxis] > splitValue)
139  {
140  //swap
141  primitive_boxes.swap(i, splitIndex);
142  //swapLeafNodes(i,splitIndex);
143  splitIndex++;
144  }
145  }
146 
147  //if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex
148  //otherwise the tree-building might fail due to stack-overflows in certain cases.
149  //unbalanced1 is unsafe: it can cause stack overflows
150  //bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1)));
151 
152  //unbalanced2 should work too: always use center (perfect balanced trees)
153  //bool unbalanced2 = true;
154 
155  //this should be safe too:
156  int rangeBalancedIndices = numIndices / 3;
157  bool unbalanced = ((splitIndex <= (startIndex + rangeBalancedIndices)) || (splitIndex >= (endIndex - 1 - rangeBalancedIndices)));
158 
159  if (unbalanced)
160  {
161  splitIndex = startIndex + (numIndices >> 1);
162  }
163 
164  btAssert(!((splitIndex == startIndex) || (splitIndex == (endIndex))));
165 
166  return splitIndex;
167 }
168 
169 void btQuantizedBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY& primitive_boxes, int startIndex, int endIndex)
170 {
171  int curIndex = m_num_nodes;
172  m_num_nodes++;
173 
174  btAssert((endIndex - startIndex) > 0);
175 
176  if ((endIndex - startIndex) == 1)
177  {
178  //We have a leaf node
179  setNodeBound(curIndex, primitive_boxes[startIndex].m_bound);
180  m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data);
181 
182  return;
183  }
184  //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
185 
186  //split axis
187  int splitIndex = _calc_splitting_axis(primitive_boxes, startIndex, endIndex);
188 
189  splitIndex = _sort_and_calc_splitting_index(
190  primitive_boxes, startIndex, endIndex,
191  splitIndex //split axis
192  );
193 
194  //calc this node bounding box
195 
196  btAABB node_bound;
197  node_bound.invalidate();
198 
199  for (int i = startIndex; i < endIndex; i++)
200  {
201  node_bound.merge(primitive_boxes[i].m_bound);
202  }
203 
204  setNodeBound(curIndex, node_bound);
205 
206  //build left branch
207  _build_sub_tree(primitive_boxes, startIndex, splitIndex);
208 
209  //build right branch
210  _build_sub_tree(primitive_boxes, splitIndex, endIndex);
211 
212  m_node_array[curIndex].setEscapeIndex(m_num_nodes - curIndex);
213 }
214 
217  GIM_BVH_DATA_ARRAY& primitive_boxes)
218 {
219  calc_quantization(primitive_boxes);
220  // initialize node count to 0
221  m_num_nodes = 0;
222  // allocate nodes
223  m_node_array.resize(primitive_boxes.size() * 2);
224 
225  _build_sub_tree(primitive_boxes, 0, primitive_boxes.size());
226 }
227 
229 
231 {
232  int nodecount = getNodeCount();
233  while (nodecount--)
234  {
235  if (isLeafNode(nodecount))
236  {
237  btAABB leafbox;
238  m_primitive_manager->get_primitive_box(getNodeData(nodecount), leafbox);
239  setNodeBound(nodecount, leafbox);
240  }
241  else
242  {
243  //const GIM_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount);
244  //get left bound
245  btAABB bound;
246  bound.invalidate();
247 
248  btAABB temp_box;
249 
250  int child_node = getLeftNode(nodecount);
251  if (child_node)
252  {
253  getNodeBound(child_node, temp_box);
254  bound.merge(temp_box);
255  }
256 
257  child_node = getRightNode(nodecount);
258  if (child_node)
259  {
260  getNodeBound(child_node, temp_box);
261  bound.merge(temp_box);
262  }
263 
264  setNodeBound(nodecount, bound);
265  }
266  }
267 }
268 
271 {
272  //obtain primitive boxes
273  GIM_BVH_DATA_ARRAY primitive_boxes;
274  primitive_boxes.resize(m_primitive_manager->get_primitive_count());
275 
276  for (int i = 0; i < primitive_boxes.size(); i++)
277  {
278  m_primitive_manager->get_primitive_box(i, primitive_boxes[i].m_bound);
279  primitive_boxes[i].m_data = i;
280  }
281 
282  m_box_tree.build_tree(primitive_boxes);
283 }
284 
286 bool btGImpactQuantizedBvh::boxQuery(const btAABB& box, btAlignedObjectArray<int>& collided_results) const
287 {
288  int curIndex = 0;
289  int numNodes = getNodeCount();
290 
291  //quantize box
292 
293  unsigned short quantizedMin[3];
294  unsigned short quantizedMax[3];
295 
296  m_box_tree.quantizePoint(quantizedMin, box.m_min);
297  m_box_tree.quantizePoint(quantizedMax, box.m_max);
298 
299  while (curIndex < numNodes)
300  {
301  //catch bugs in tree data
302 
303  bool aabbOverlap = m_box_tree.testQuantizedBoxOverlapp(curIndex, quantizedMin, quantizedMax);
304  bool isleafnode = isLeafNode(curIndex);
305 
306  if (isleafnode && aabbOverlap)
307  {
308  collided_results.push_back(getNodeData(curIndex));
309  }
310 
311  if (aabbOverlap || isleafnode)
312  {
313  //next subnode
314  curIndex++;
315  }
316  else
317  {
318  //skip node
319  curIndex += getEscapeNodeIndex(curIndex);
320  }
321  }
322  if (collided_results.size() > 0) return true;
323  return false;
324 }
325 
328  const btVector3& ray_dir, const btVector3& ray_origin,
329  btAlignedObjectArray<int>& collided_results) const
330 {
331  int curIndex = 0;
332  int numNodes = getNodeCount();
333 
334  while (curIndex < numNodes)
335  {
336  btAABB bound;
337  getNodeBound(curIndex, bound);
338 
339  //catch bugs in tree data
340 
341  bool aabbOverlap = bound.collide_ray(ray_origin, ray_dir);
342  bool isleafnode = isLeafNode(curIndex);
343 
344  if (isleafnode && aabbOverlap)
345  {
346  collided_results.push_back(getNodeData(curIndex));
347  }
348 
349  if (aabbOverlap || isleafnode)
350  {
351  //next subnode
352  curIndex++;
353  }
354  else
355  {
356  //skip node
357  curIndex += getEscapeNodeIndex(curIndex);
358  }
359  }
360  if (collided_results.size() > 0) return true;
361  return false;
362 }
363 
365  const btGImpactQuantizedBvh* boxset0, const btGImpactQuantizedBvh* boxset1,
366  const BT_BOX_BOX_TRANSFORM_CACHE& trans_cache_1to0,
367  int node0, int node1, bool complete_primitive_tests)
368 {
369  btAABB box0;
370  boxset0->getNodeBound(node0, box0);
371  btAABB box1;
372  boxset1->getNodeBound(node1, box1);
373 
374  return box0.overlapping_trans_cache(box1, trans_cache_1to0, complete_primitive_tests);
375  // box1.appy_transform_trans_cache(trans_cache_1to0);
376  // return box0.has_collision(box1);
377 }
378 
379 //stackless recursive collision routine
381  const btGImpactQuantizedBvh* boxset0, const btGImpactQuantizedBvh* boxset1,
382  btPairSet* collision_pairs,
383  const BT_BOX_BOX_TRANSFORM_CACHE& trans_cache_1to0,
384  int node0, int node1, bool complete_primitive_tests)
385 {
387  boxset0, boxset1, trans_cache_1to0,
388  node0, node1, complete_primitive_tests) == false) return; //avoid colliding internal nodes
389 
390  if (boxset0->isLeafNode(node0))
391  {
392  if (boxset1->isLeafNode(node1))
393  {
394  // collision result
395  collision_pairs->push_pair(
396  boxset0->getNodeData(node0), boxset1->getNodeData(node1));
397  return;
398  }
399  else
400  {
401  //collide left recursive
402 
404  boxset0, boxset1,
405  collision_pairs, trans_cache_1to0,
406  node0, boxset1->getLeftNode(node1), false);
407 
408  //collide right recursive
410  boxset0, boxset1,
411  collision_pairs, trans_cache_1to0,
412  node0, boxset1->getRightNode(node1), false);
413  }
414  }
415  else
416  {
417  if (boxset1->isLeafNode(node1))
418  {
419  //collide left recursive
421  boxset0, boxset1,
422  collision_pairs, trans_cache_1to0,
423  boxset0->getLeftNode(node0), node1, false);
424 
425  //collide right recursive
426 
428  boxset0, boxset1,
429  collision_pairs, trans_cache_1to0,
430  boxset0->getRightNode(node0), node1, false);
431  }
432  else
433  {
434  //collide left0 left1
435 
437  boxset0, boxset1,
438  collision_pairs, trans_cache_1to0,
439  boxset0->getLeftNode(node0), boxset1->getLeftNode(node1), false);
440 
441  //collide left0 right1
442 
444  boxset0, boxset1,
445  collision_pairs, trans_cache_1to0,
446  boxset0->getLeftNode(node0), boxset1->getRightNode(node1), false);
447 
448  //collide right0 left1
449 
451  boxset0, boxset1,
452  collision_pairs, trans_cache_1to0,
453  boxset0->getRightNode(node0), boxset1->getLeftNode(node1), false);
454 
455  //collide right0 right1
456 
458  boxset0, boxset1,
459  collision_pairs, trans_cache_1to0,
460  boxset0->getRightNode(node0), boxset1->getRightNode(node1), false);
461 
462  } // else if node1 is not a leaf
463  } // else if node0 is not a leaf
464 }
465 
467  const btGImpactQuantizedBvh* boxset1, const btTransform& trans1,
468  btPairSet& collision_pairs)
469 {
470  if (boxset0->getNodeCount() == 0 || boxset1->getNodeCount() == 0) return;
471 
472  BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0;
473 
474  trans_cache_1to0.calc_from_homogenic(trans0, trans1);
475 
476 #ifdef TRI_COLLISION_PROFILING
477  bt_begin_gim02_q_tree_time();
478 #endif //TRI_COLLISION_PROFILING
479 
481  boxset0, boxset1,
482  &collision_pairs, trans_cache_1to0, 0, 0, true);
483 #ifdef TRI_COLLISION_PROFILING
484  bt_end_gim02_q_tree_time();
485 #endif //TRI_COLLISION_PROFILING
486 }
typedef float(TangentPoint)[2]
NSNotificationCenter * center
BT_BOX_BOX_TRANSFORM_CACHE
Class for transforming a model1 to the space of model0.
btAABB
Axis aligned box.
SIMD_FORCE_INLINE 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)
btAlignedObjectArray< btScalar > m_data
SIMD_FORCE_INLINE 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:314
#define SIMD_FORCE_INLINE
Definition: btScalar.h:280
#define btAssert(x)
Definition: btScalar.h:295
int numIndices() const
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
SIMD_FORCE_INLINE int size() const
return the number of elements in the array
void swap(int index0, int index1)
SIMD_FORCE_INLINE void resize(int newsize, const T &fillData=T())
SIMD_FORCE_INLINE 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:23
void reset()
Resets the initial reference time.
unsigned long long int getTimeMicroseconds()
Structure for containing Boxes.
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
SIMD_FORCE_INLINE int getNodeData(int nodeindex) const
btPrimitiveManagerBase * m_primitive_manager
void buildSet()
this rebuild the entire set
SIMD_FORCE_INLINE int getRightNode(int nodeindex) const
bool boxQuery(const btAABB &box, btAlignedObjectArray< int > &collided_results) const
returns the indices of the primitives in the m_primitive_manager
btQuantizedBvhTree m_box_tree
SIMD_FORCE_INLINE bool isLeafNode(int nodeindex) const
tells if the node is a leaf
SIMD_FORCE_INLINE int getNodeCount() const
node count
SIMD_FORCE_INLINE int getLeftNode(int nodeindex) const
SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB &bound)
SIMD_FORCE_INLINE int getEscapeNodeIndex(int nodeindex) const
SIMD_FORCE_INLINE void getNodeBound(int nodeindex, btAABB &bound) 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:35
void push_pair(int index1, int index2)
Definition: btGImpactBvh.h:41
virtual int get_primitive_count() const =0
virtual void get_primitive_box(int prim_index, btAABB &primbox) const =0
SIMD_FORCE_INLINE void quantizePoint(unsigned short *quantizedpoint, const btVector3 &point) const
void _build_sub_tree(GIM_BVH_DATA_ARRAY &primitive_boxes, int startIndex, int endIndex)
void calc_quantization(GIM_BVH_DATA_ARRAY &primitive_boxes, btScalar boundMargin=btScalar(1.0))
void build_tree(GIM_BVH_DATA_ARRAY &primitive_boxes)
stackless build tree
SIMD_FORCE_INLINE bool testQuantizedBoxOverlapp(int node_index, unsigned short *quantizedMin, unsigned short *quantizedMax) const
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)
SIMD_FORCE_INLINE void setNodeBound(int nodeindex, const btAABB &bound)