Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/ggz/src/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp @ 3358

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

Merged presentation branch back to trunk.

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