← 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/btGImpactBvh.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/btGImpactBvh.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
#include "btGImpactBvh.h"
 
24
#include "LinearMath/btQuickprof.h"
 
25
 
 
26
#ifdef TRI_COLLISION_PROFILING
 
27
 
 
28
btClock g_tree_clock;
 
29
 
 
30
float g_accum_tree_collision_time = 0;
 
31
int g_count_traversing = 0;
 
32
 
 
33
 
 
34
void bt_begin_gim02_tree_time()
 
35
{
 
36
        g_tree_clock.reset();
 
37
}
 
38
 
 
39
void 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
 
46
float 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
 
 
67
int 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
 
 
99
int 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
 
 
160
void 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
 
212
void 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
 
 
225
void 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
 
265
void 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
 
281
bool 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
 
319
bool 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
 
 
357
SIMD_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
 
375
static 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
 
 
475
void 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