Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics/src/bullet/BulletCollision/GIMPACT/btGImpactBvh.cpp @ 2192

Last change on this file since 2192 was 2192, checked in by rgrieder, 16 years ago

Reverted all changes of attempt to update physics branch.

  • Property svn:eol-style set to native
File size: 11.9 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        BT_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        BT_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        bool unbal = (splitIndex==startIndex) || (splitIndex == (endIndex));
154        btAssert(!unbal);
155
156        return splitIndex;
157
158}
159
160
161void btBvhTree::_build_sub_tree(BT_BVH_DATA_ARRAY & primitive_boxes, int startIndex,  int endIndex)
162{
163        int curIndex = m_num_nodes;
164        m_num_nodes++;
165
166        btAssert((endIndex-startIndex)>0);
167
168        if ((endIndex-startIndex)==1)
169        {
170            //We have a leaf node
171            setNodeBound(curIndex,primitive_boxes[startIndex].m_bound);
172                m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data);
173
174                return;
175        }
176        //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
177
178        //split axis
179        int splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex);
180
181        splitIndex = _sort_and_calc_splitting_index(
182                        primitive_boxes,startIndex,endIndex,
183                        splitIndex//split axis
184                        );
185
186
187        //calc this node bounding box
188
189        btAABB node_bound;
190        node_bound.invalidate();
191
192        for (int i=startIndex;i<endIndex;i++)
193        {
194                node_bound.merge(primitive_boxes[i].m_bound);
195        }
196
197        setNodeBound(curIndex,node_bound);
198
199
200        //build left branch
201        _build_sub_tree(primitive_boxes, startIndex, splitIndex );
202
203
204        //build right branch
205         _build_sub_tree(primitive_boxes, splitIndex ,endIndex);
206
207        m_node_array[curIndex].setEscapeIndex(m_num_nodes - curIndex);
208
209
210}
211
212//! stackless build tree
213void btBvhTree::build_tree(
214        BT_BVH_DATA_ARRAY & primitive_boxes)
215{
216        // initialize node count to 0
217        m_num_nodes = 0;
218        // allocate nodes
219        m_node_array.resize(primitive_boxes.size()*2);
220
221        _build_sub_tree(primitive_boxes, 0, primitive_boxes.size());
222}
223
224////////////////////////////////////class btGImpactBvh
225
226void btGImpactBvh::refit()
227{
228        int nodecount = getNodeCount();
229        while(nodecount--)
230        {
231                if(isLeafNode(nodecount))
232                {
233                        btAABB leafbox;
234                        m_primitive_manager->get_primitive_box(getNodeData(nodecount),leafbox);
235                        setNodeBound(nodecount,leafbox);
236                }
237                else
238                {
239                        //const BT_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount);
240                        //get left bound
241                        btAABB bound;
242                        bound.invalidate();
243
244                        btAABB temp_box;
245
246                        int child_node = getLeftNode(nodecount);
247                        if(child_node)
248                        {
249                                getNodeBound(child_node,temp_box);
250                                bound.merge(temp_box);
251                        }
252
253                        child_node = getRightNode(nodecount);
254                        if(child_node)
255                        {
256                                getNodeBound(child_node,temp_box);
257                                bound.merge(temp_box);
258                        }
259
260                        setNodeBound(nodecount,bound);
261                }
262        }
263}
264
265//! this rebuild the entire set
266void btGImpactBvh::buildSet()
267{
268        //obtain primitive boxes
269        BT_BVH_DATA_ARRAY primitive_boxes;
270        primitive_boxes.resize(m_primitive_manager->get_primitive_count());
271
272        for (int i = 0;i<primitive_boxes.size() ;i++ )
273        {
274                 m_primitive_manager->get_primitive_box(i,primitive_boxes[i].m_bound);
275                 primitive_boxes[i].m_data = i;
276        }
277
278        m_box_tree.build_tree(primitive_boxes);
279}
280
281//! returns the indices of the primitives in the m_primitive_manager
282bool btGImpactBvh::boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const
283{
284        int curIndex = 0;
285        int numNodes = getNodeCount();
286
287        while (curIndex < numNodes)
288        {
289                btAABB bound;
290                getNodeBound(curIndex,bound);
291
292                //catch bugs in tree data
293
294                bool aabbOverlap = bound.has_collision(box);
295                bool isleafnode = isLeafNode(curIndex);
296
297                if (isleafnode && aabbOverlap)
298                {
299                        collided_results.push_back(getNodeData(curIndex));
300                }
301
302                if (aabbOverlap || isleafnode)
303                {
304                        //next subnode
305                        curIndex++;
306                }
307                else
308                {
309                        //skip node
310                        curIndex+= getEscapeNodeIndex(curIndex);
311                }
312        }
313        if(collided_results.size()>0) return true;
314        return false;
315}
316
317
318
319//! returns the indices of the primitives in the m_primitive_manager
320bool btGImpactBvh::rayQuery(
321        const btVector3 & ray_dir,const btVector3 & ray_origin ,
322        btAlignedObjectArray<int> & collided_results) const
323{
324        int curIndex = 0;
325        int numNodes = getNodeCount();
326
327        while (curIndex < numNodes)
328        {
329                btAABB bound;
330                getNodeBound(curIndex,bound);
331
332                //catch bugs in tree data
333
334                bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir);
335                bool isleafnode = isLeafNode(curIndex);
336
337                if (isleafnode && aabbOverlap)
338                {
339                        collided_results.push_back(getNodeData( curIndex));
340                }
341
342                if (aabbOverlap || isleafnode)
343                {
344                        //next subnode
345                        curIndex++;
346                }
347                else
348                {
349                        //skip node
350                        curIndex+= getEscapeNodeIndex(curIndex);
351                }
352        }
353        if(collided_results.size()>0) return true;
354        return false;
355}
356
357
358SIMD_FORCE_INLINE bool _node_collision(
359        btGImpactBvh * boxset0, btGImpactBvh * boxset1,
360        const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
361        int node0 ,int node1, bool complete_primitive_tests)
362{
363        btAABB box0;
364        boxset0->getNodeBound(node0,box0);
365        btAABB box1;
366        boxset1->getNodeBound(node1,box1);
367
368        return box0.overlapping_trans_cache(box1,trans_cache_1to0,complete_primitive_tests );
369//      box1.appy_transform_trans_cache(trans_cache_1to0);
370//      return box0.has_collision(box1);
371
372}
373
374
375//stackless recursive collision routine
376static void _find_collision_pairs_recursive(
377        btGImpactBvh * boxset0, btGImpactBvh * boxset1,
378        btPairSet * collision_pairs,
379        const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
380        int node0, int node1, bool complete_primitive_tests)
381{
382
383
384
385        if( _node_collision(
386                boxset0,boxset1,trans_cache_1to0,
387                node0,node1,complete_primitive_tests) ==false) return;//avoid colliding internal nodes
388
389        if(boxset0->isLeafNode(node0))
390        {
391                if(boxset1->isLeafNode(node1))
392                {
393                        // collision result
394                        collision_pairs->push_pair(
395                                boxset0->getNodeData(node0),boxset1->getNodeData(node1));
396                        return;
397                }
398                else
399                {
400
401                        //collide left recursive
402
403                        _find_collision_pairs_recursive(
404                                                                boxset0,boxset1,
405                                                                collision_pairs,trans_cache_1to0,
406                                                                node0,boxset1->getLeftNode(node1),false);
407
408                        //collide right recursive
409                        _find_collision_pairs_recursive(
410                                                                boxset0,boxset1,
411                                                                collision_pairs,trans_cache_1to0,
412                                                                node0,boxset1->getRightNode(node1),false);
413
414
415                }
416        }
417        else
418        {
419                if(boxset1->isLeafNode(node1))
420                {
421
422                        //collide left recursive
423                        _find_collision_pairs_recursive(
424                                                                boxset0,boxset1,
425                                                                collision_pairs,trans_cache_1to0,
426                                                                boxset0->getLeftNode(node0),node1,false);
427
428
429                        //collide right recursive
430
431                        _find_collision_pairs_recursive(
432                                                                boxset0,boxset1,
433                                                                collision_pairs,trans_cache_1to0,
434                                                                boxset0->getRightNode(node0),node1,false);
435
436
437                }
438                else
439                {
440                        //collide left0 left1
441
442
443
444                        _find_collision_pairs_recursive(
445                                boxset0,boxset1,
446                                collision_pairs,trans_cache_1to0,
447                                boxset0->getLeftNode(node0),boxset1->getLeftNode(node1),false);
448
449                        //collide left0 right1
450
451                        _find_collision_pairs_recursive(
452                                boxset0,boxset1,
453                                collision_pairs,trans_cache_1to0,
454                                boxset0->getLeftNode(node0),boxset1->getRightNode(node1),false);
455
456
457                        //collide right0 left1
458
459                        _find_collision_pairs_recursive(
460                                boxset0,boxset1,
461                                collision_pairs,trans_cache_1to0,
462                                boxset0->getRightNode(node0),boxset1->getLeftNode(node1),false);
463
464                        //collide right0 right1
465
466                        _find_collision_pairs_recursive(
467                                boxset0,boxset1,
468                                collision_pairs,trans_cache_1to0,
469                                boxset0->getRightNode(node0),boxset1->getRightNode(node1),false);
470
471                }// else if node1 is not a leaf
472        }// else if node0 is not a leaf
473}
474
475
476void btGImpactBvh::find_collision(btGImpactBvh * boxset0, const btTransform & trans0,
477                btGImpactBvh * boxset1, const btTransform & trans1,
478                btPairSet & collision_pairs)
479{
480
481        if(boxset0->getNodeCount()==0 || boxset1->getNodeCount()==0 ) return;
482
483        BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0;
484
485        trans_cache_1to0.calc_from_homogenic(trans0,trans1);
486
487#ifdef TRI_COLLISION_PROFILING
488        bt_begin_gim02_tree_time();
489#endif //TRI_COLLISION_PROFILING
490
491        _find_collision_pairs_recursive(
492                boxset0,boxset1,
493                &collision_pairs,trans_cache_1to0,0,0,true);
494#ifdef TRI_COLLISION_PROFILING
495        bt_end_gim02_tree_time();
496#endif //TRI_COLLISION_PROFILING
497
498}
499
Note: See TracBrowser for help on using the repository browser.