== SoC Bullet - Bullet Upgrade to 2.76 ==
[blender.git] / extern / bullet2 / BulletCollision / Gimpact / btGImpactBvh.cpp
1 /*! \file gim_box_set.h
2 \author Francisco Le\7fn N├čjera
3 */
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
45 //! Gets the average time in miliseconds of tree collisions
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
65 /////////////////////// btBvhTree /////////////////////////////////
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
211 //! stackless build tree
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
223 ////////////////////////////////////class btGImpactBvh
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
264 //! this rebuild the entire set
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
280 //! returns the indices of the primitives in the m_primitive_manager
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
318 //! returns the indices of the primitives in the m_primitive_manager
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