Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/external/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.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: 12.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
24#include "btGImpactQuantizedBvh.h"
25#include "LinearMath/btQuickprof.h"
26
27#ifdef TRI_COLLISION_PROFILING
28btClock g_q_tree_clock;
29
30
31float g_q_accum_tree_collision_time = 0;
32int g_q_count_traversing = 0;
33
34
35void bt_begin_gim02_q_tree_time()
36{
37        g_q_tree_clock.reset();
38}
39
40void 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
47//! Gets the average time in miliseconds of tree collisions
48float 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
67/////////////////////// btQuantizedBvhTree /////////////////////////////////
68
69void btQuantizedBvhTree::calc_quantization(
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
81        bt_calc_quantization_parameters(
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
88int btQuantizedBvhTree::_calc_splitting_axis(
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
120int btQuantizedBvhTree::_sort_and_calc_splitting_index(
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
181void 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
232//! stackless build tree
233void btQuantizedBvhTree::build_tree(
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
245////////////////////////////////////class btGImpactQuantizedBvh
246
247void btGImpactQuantizedBvh::refit()
248{
249        int nodecount = getNodeCount();
250        while(nodecount--)
251        {
252                if(isLeafNode(nodecount))
253                {
254                        btAABB leafbox;
255                        m_primitive_manager->get_primitive_box(getNodeData(nodecount),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
286//! this rebuild the entire set
287void btGImpactQuantizedBvh::buildSet()
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
302//! returns the indices of the primitives in the m_primitive_manager
303bool 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
347//! returns the indices of the primitives in the m_primitive_manager
348bool btGImpactQuantizedBvh::rayQuery(
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
386SIMD_FORCE_INLINE bool _quantized_node_collision(
387        btGImpactQuantizedBvh * boxset0, 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
404static void _find_quantized_collision_pairs_recursive(
405        btGImpactQuantizedBvh * boxset0, 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
413        if( _quantized_node_collision(
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
431                        _find_quantized_collision_pairs_recursive(
432                                                                boxset0,boxset1,
433                                                                collision_pairs,trans_cache_1to0,
434                                                                node0,boxset1->getLeftNode(node1),false);
435
436                        //collide right recursive
437                        _find_quantized_collision_pairs_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
451                        _find_quantized_collision_pairs_recursive(
452                                                                boxset0,boxset1,
453                                                                collision_pairs,trans_cache_1to0,
454                                                                boxset0->getLeftNode(node0),node1,false);
455
456
457                        //collide right recursive
458
459                        _find_quantized_collision_pairs_recursive(
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
472                        _find_quantized_collision_pairs_recursive(
473                                boxset0,boxset1,
474                                collision_pairs,trans_cache_1to0,
475                                boxset0->getLeftNode(node0),boxset1->getLeftNode(node1),false);
476
477                        //collide left0 right1
478
479                        _find_quantized_collision_pairs_recursive(
480                                boxset0,boxset1,
481                                collision_pairs,trans_cache_1to0,
482                                boxset0->getLeftNode(node0),boxset1->getRightNode(node1),false);
483
484
485                        //collide right0 left1
486
487                        _find_quantized_collision_pairs_recursive(
488                                boxset0,boxset1,
489                                collision_pairs,trans_cache_1to0,
490                                boxset0->getRightNode(node0),boxset1->getLeftNode(node1),false);
491
492                        //collide right0 right1
493
494                        _find_quantized_collision_pairs_recursive(
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
504void btGImpactQuantizedBvh::find_collision(btGImpactQuantizedBvh * boxset0, const btTransform & trans0,
505                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
519        _find_quantized_collision_pairs_recursive(
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
Note: See TracBrowser for help on using the repository browser.