← Back to branch summary

~ubuntu-branches/ubuntu/vivid/emscripten/vivid

« back to all changes in this revision

Viewing changes to tests/bullet/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp

  • Committer: Package Import Robot
  • Author(s): Sylvestre Ledru
  • Date: 2013-05-02 13:11:51 UTC
  • Revision ID: package-import@ubuntu.com-20130502131151-q8dvteqr1ef2x7xz
Tags: upstream-1.4.1~20130504~adb56cb
Import upstream version 1.4.1~20130504~adb56cb

Show diffs side-by-side

added added

removed removed

Lines of Context:
tests/bullet/src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp
 
1
/*! \file gim_box_set.h
 
2
\author Francisco Leon Najera
 
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
 
 
24
#include "btGImpactQuantizedBvh.h"
 
25
#include "LinearMath/btQuickprof.h"
 
26
 
 
27
#ifdef TRI_COLLISION_PROFILING
 
28
btClock g_q_tree_clock;
 
29
 
 
30
 
 
31
float g_q_accum_tree_collision_time = 0;
 
32
int g_q_count_traversing = 0;
 
33
 
 
34
 
 
35
void bt_begin_gim02_q_tree_time()
 
36
{
 
37
        g_q_tree_clock.reset();
 
38
}
 
39
 
 
40
void 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
 
48
float 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
 
 
69
void 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
 
 
88
int 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
 
 
120
int 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
 
 
181
void 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
 
233
void 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
 
 
247
void 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
 
287
void 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
 
303
bool 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
 
348
bool 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
 
 
386
SIMD_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
 
404
static 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
 
 
504
void 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