Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/external/bullet/BulletCollision/Gimpact/btGImpactBvh.cpp @ 8066

Last change on this file since 8066 was 5781, checked in by rgrieder, 15 years ago

Reverted trunk again. We might want to find a way to delete these revisions again (x3n's changes are still available as diff in the commit mails).

  • Property svn:eol-style set to native
File size: 11.8 KB
Line 
1/*! \file gim_box_set.h
2\author Francisco Len Nßjera
3*/
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"
24#include "LinearMath/btQuickprof.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
45//! Gets the average time in miliseconds of tree collisions
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
65/////////////////////// btBvhTree /////////////////////////////////
66
67int 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
99int 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
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
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
212void 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
225void 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
265void 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
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
318//! returns the indices of the primitives in the m_primitive_manager
319bool 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
357SIMD_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
375static 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
475void 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
Note: See TracBrowser for help on using the repository browser.