← Back to branch summary

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

« back to all changes in this revision

Viewing changes to tests/bullet/src/BulletCollision/BroadphaseCollision/btDbvt.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/BroadphaseCollision/btDbvt.cpp
 
1
/*
 
2
Bullet Continuous Collision Detection and Physics Library
 
3
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
 
4
 
 
5
This software is provided 'as-is', without any express or implied warranty.
 
6
In no event will the authors be held liable for any damages arising from the use of this software.
 
7
Permission is granted to anyone to use this software for any purpose,
 
8
including commercial applications, and to alter it and redistribute it freely,
 
9
subject to the following restrictions:
 
10
 
 
11
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.
 
12
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
 
13
3. This notice may not be removed or altered from any source distribution.
 
14
*/
 
15
///btDbvt implementation by Nathanael Presson
 
16
 
 
17
#include "btDbvt.h"
 
18
 
 
19
//
 
20
typedef btAlignedObjectArray<btDbvtNode*>                       tNodeArray;
 
21
typedef btAlignedObjectArray<const btDbvtNode*> tConstNodeArray;
 
22
 
 
23
//
 
24
struct btDbvtNodeEnumerator : btDbvt::ICollide
 
25
{
 
26
        tConstNodeArray nodes;
 
27
        void Process(const btDbvtNode* n) { nodes.push_back(n); }
 
28
};
 
29
 
 
30
//
 
31
static DBVT_INLINE int                  indexof(const btDbvtNode* node)
 
32
{
 
33
        return(node->parent->childs[1]==node);
 
34
}
 
35
 
 
36
//
 
37
static DBVT_INLINE btDbvtVolume merge(  const btDbvtVolume& a,
 
38
                                                                          const btDbvtVolume& b)
 
39
{
 
40
#if (DBVT_MERGE_IMPL==DBVT_IMPL_SSE)
 
41
        ATTRIBUTE_ALIGNED16(char locals[sizeof(btDbvtAabbMm)]);
 
42
        btDbvtVolume&   res=*(btDbvtVolume*)locals;
 
43
#else
 
44
                btDbvtVolume    res;
 
45
#endif
 
46
        Merge(a,b,res);
 
47
        return(res);
 
48
}
 
49
 
 
50
// volume+edge lengths
 
51
static DBVT_INLINE btScalar             size(const btDbvtVolume& a)
 
52
{
 
53
        const btVector3 edges=a.Lengths();
 
54
        return( edges.x()*edges.y()*edges.z()+
 
55
                edges.x()+edges.y()+edges.z());
 
56
}
 
57
 
 
58
//
 
59
static void                                             getmaxdepth(const btDbvtNode* node,int depth,int& maxdepth)
 
60
{
 
61
        if(node->isinternal())
 
62
        {
 
63
                getmaxdepth(node->childs[0],depth+1,maxdepth);
 
64
                getmaxdepth(node->childs[1],depth+1,maxdepth);
 
65
        } else maxdepth=btMax(maxdepth,depth);
 
66
}
 
67
 
 
68
//
 
69
static DBVT_INLINE void                 deletenode(     btDbvt* pdbvt,
 
70
                                                                                   btDbvtNode* node)
 
71
{
 
72
        btAlignedFree(pdbvt->m_free);
 
73
        pdbvt->m_free=node;
 
74
}
 
75
 
 
76
//
 
77
static void                                             recursedeletenode(      btDbvt* pdbvt,
 
78
                                                                                                  btDbvtNode* node)
 
79
{
 
80
        if(!node->isleaf())
 
81
        {
 
82
                recursedeletenode(pdbvt,node->childs[0]);
 
83
                recursedeletenode(pdbvt,node->childs[1]);
 
84
        }
 
85
        if(node==pdbvt->m_root) pdbvt->m_root=0;
 
86
        deletenode(pdbvt,node);
 
87
}
 
88
 
 
89
//
 
90
static DBVT_INLINE btDbvtNode*  createnode(     btDbvt* pdbvt,
 
91
                                                                                   btDbvtNode* parent,
 
92
                                                                                   void* data)
 
93
{
 
94
        btDbvtNode*     node;
 
95
        if(pdbvt->m_free)
 
96
        { node=pdbvt->m_free;pdbvt->m_free=0; }
 
97
        else
 
98
        { node=new(btAlignedAlloc(sizeof(btDbvtNode),16)) btDbvtNode(); }
 
99
        node->parent    =       parent;
 
100
        node->data              =       data;
 
101
        node->childs[1] =       0;
 
102
        return(node);
 
103
}
 
104
 
 
105
//
 
106
static DBVT_INLINE btDbvtNode*  createnode(     btDbvt* pdbvt,
 
107
                                                                                   btDbvtNode* parent,
 
108
                                                                                   const btDbvtVolume& volume,
 
109
                                                                                   void* data)
 
110
{
 
111
        btDbvtNode*     node=createnode(pdbvt,parent,data);
 
112
        node->volume=volume;
 
113
        return(node);
 
114
}
 
115
 
 
116
//
 
117
static DBVT_INLINE btDbvtNode*  createnode(     btDbvt* pdbvt,
 
118
                                                                                   btDbvtNode* parent,
 
119
                                                                                   const btDbvtVolume& volume0,
 
120
                                                                                   const btDbvtVolume& volume1,
 
121
                                                                                   void* data)
 
122
{
 
123
        btDbvtNode*     node=createnode(pdbvt,parent,data);
 
124
        Merge(volume0,volume1,node->volume);
 
125
        return(node);
 
126
}
 
127
 
 
128
//
 
129
static void                                             insertleaf(     btDbvt* pdbvt,
 
130
                                                                                   btDbvtNode* root,
 
131
                                                                                   btDbvtNode* leaf)
 
132
{
 
133
        if(!pdbvt->m_root)
 
134
        {
 
135
                pdbvt->m_root   =       leaf;
 
136
                leaf->parent    =       0;
 
137
        }
 
138
        else
 
139
        {
 
140
                if(!root->isleaf())
 
141
                {
 
142
                        do      {
 
143
                                root=root->childs[Select(       leaf->volume,
 
144
                                        root->childs[0]->volume,
 
145
                                        root->childs[1]->volume)];
 
146
                        } while(!root->isleaf());
 
147
                }
 
148
                btDbvtNode*     prev=root->parent;
 
149
                btDbvtNode*     node=createnode(pdbvt,prev,leaf->volume,root->volume,0);
 
150
                if(prev)
 
151
                {
 
152
                        prev->childs[indexof(root)]     =       node;
 
153
                        node->childs[0]                         =       root;root->parent=node;
 
154
                        node->childs[1]                         =       leaf;leaf->parent=node;
 
155
                        do      {
 
156
                                if(!prev->volume.Contain(node->volume))
 
157
                                        Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume);
 
158
                                else
 
159
                                        break;
 
160
                                node=prev;
 
161
                        } while(0!=(prev=node->parent));
 
162
                }
 
163
                else
 
164
                {
 
165
                        node->childs[0] =       root;root->parent=node;
 
166
                        node->childs[1] =       leaf;leaf->parent=node;
 
167
                        pdbvt->m_root   =       node;
 
168
                }
 
169
        }
 
170
}
 
171
 
 
172
//
 
173
static btDbvtNode*                              removeleaf(     btDbvt* pdbvt,
 
174
                                                                                   btDbvtNode* leaf)
 
175
{
 
176
        if(leaf==pdbvt->m_root)
 
177
        {
 
178
                pdbvt->m_root=0;
 
179
                return(0);
 
180
        }
 
181
        else
 
182
        {
 
183
                btDbvtNode*     parent=leaf->parent;
 
184
                btDbvtNode*     prev=parent->parent;
 
185
                btDbvtNode*     sibling=parent->childs[1-indexof(leaf)];                        
 
186
                if(prev)
 
187
                {
 
188
                        prev->childs[indexof(parent)]=sibling;
 
189
                        sibling->parent=prev;
 
190
                        deletenode(pdbvt,parent);
 
191
                        while(prev)
 
192
                        {
 
193
                                const btDbvtVolume      pb=prev->volume;
 
194
                                Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume);
 
195
                                if(NotEqual(pb,prev->volume))
 
196
                                {
 
197
                                        prev=prev->parent;
 
198
                                } else break;
 
199
                        }
 
200
                        return(prev?prev:pdbvt->m_root);
 
201
                }
 
202
                else
 
203
                {                                                               
 
204
                        pdbvt->m_root=sibling;
 
205
                        sibling->parent=0;
 
206
                        deletenode(pdbvt,parent);
 
207
                        return(pdbvt->m_root);
 
208
                }                       
 
209
        }
 
210
}
 
211
 
 
212
//
 
213
static void                                             fetchleaves(btDbvt* pdbvt,
 
214
                                                                                        btDbvtNode* root,
 
215
                                                                                        tNodeArray& leaves,
 
216
                                                                                        int depth=-1)
 
217
{
 
218
        if(root->isinternal()&&depth)
 
219
        {
 
220
                fetchleaves(pdbvt,root->childs[0],leaves,depth-1);
 
221
                fetchleaves(pdbvt,root->childs[1],leaves,depth-1);
 
222
                deletenode(pdbvt,root);
 
223
        }
 
224
        else
 
225
        {
 
226
                leaves.push_back(root);
 
227
        }
 
228
}
 
229
 
 
230
//
 
231
static void                                             split(  const tNodeArray& leaves,
 
232
                                                                          tNodeArray& left,
 
233
                                                                          tNodeArray& right,
 
234
                                                                          const btVector3& org,
 
235
                                                                          const btVector3& axis)
 
236
{
 
237
        left.resize(0);
 
238
        right.resize(0);
 
239
        for(int i=0,ni=leaves.size();i<ni;++i)
 
240
        {
 
241
                if(btDot(axis,leaves[i]->volume.Center()-org)<0)
 
242
                        left.push_back(leaves[i]);
 
243
                else
 
244
                        right.push_back(leaves[i]);
 
245
        }
 
246
}
 
247
 
 
248
//
 
249
static btDbvtVolume                             bounds( const tNodeArray& leaves)
 
250
{
 
251
#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
 
252
        ATTRIBUTE_ALIGNED16(char        locals[sizeof(btDbvtVolume)]);
 
253
        btDbvtVolume&   volume=*(btDbvtVolume*)locals;
 
254
        volume=leaves[0]->volume;
 
255
#else
 
256
        btDbvtVolume volume=leaves[0]->volume;
 
257
#endif
 
258
        for(int i=1,ni=leaves.size();i<ni;++i)
 
259
        {
 
260
                Merge(volume,leaves[i]->volume,volume);
 
261
        }
 
262
        return(volume);
 
263
}
 
264
 
 
265
//
 
266
static void                                             bottomup(       btDbvt* pdbvt,
 
267
                                                                                 tNodeArray& leaves)
 
268
{
 
269
        while(leaves.size()>1)
 
270
        {
 
271
                btScalar        minsize=SIMD_INFINITY;
 
272
                int                     minidx[2]={-1,-1};
 
273
                for(int i=0;i<leaves.size();++i)
 
274
                {
 
275
                        for(int j=i+1;j<leaves.size();++j)
 
276
                        {
 
277
                                const btScalar  sz=size(merge(leaves[i]->volume,leaves[j]->volume));
 
278
                                if(sz<minsize)
 
279
                                {
 
280
                                        minsize         =       sz;
 
281
                                        minidx[0]       =       i;
 
282
                                        minidx[1]       =       j;
 
283
                                }
 
284
                        }
 
285
                }
 
286
                btDbvtNode*     n[]     =       {leaves[minidx[0]],leaves[minidx[1]]};
 
287
                btDbvtNode*     p       =       createnode(pdbvt,0,n[0]->volume,n[1]->volume,0);
 
288
                p->childs[0]            =       n[0];
 
289
                p->childs[1]            =       n[1];
 
290
                n[0]->parent            =       p;
 
291
                n[1]->parent            =       p;
 
292
                leaves[minidx[0]]       =       p;
 
293
                leaves.swap(minidx[1],leaves.size()-1);
 
294
                leaves.pop_back();
 
295
        }
 
296
}
 
297
 
 
298
//
 
299
static btDbvtNode*                      topdown(btDbvt* pdbvt,
 
300
                                                                        tNodeArray& leaves,
 
301
                                                                        int bu_treshold)
 
302
{
 
303
        static const btVector3  axis[]={btVector3(1,0,0),
 
304
                btVector3(0,1,0),
 
305
                btVector3(0,0,1)};
 
306
        if(leaves.size()>1)
 
307
        {
 
308
                if(leaves.size()>bu_treshold)
 
309
                {
 
310
                        const btDbvtVolume      vol=bounds(leaves);
 
311
                        const btVector3                 org=vol.Center();
 
312
                        tNodeArray                              sets[2];
 
313
                        int                                             bestaxis=-1;
 
314
                        int                                             bestmidp=leaves.size();
 
315
                        int                                             splitcount[3][2]={{0,0},{0,0},{0,0}};
 
316
                        int i;
 
317
                        for( i=0;i<leaves.size();++i)
 
318
                        {
 
319
                                const btVector3 x=leaves[i]->volume.Center()-org;
 
320
                                for(int j=0;j<3;++j)
 
321
                                {
 
322
                                        ++splitcount[j][btDot(x,axis[j])>0?1:0];
 
323
                                }
 
324
                        }
 
325
                        for( i=0;i<3;++i)
 
326
                        {
 
327
                                if((splitcount[i][0]>0)&&(splitcount[i][1]>0))
 
328
                                {
 
329
                                        const int       midp=(int)btFabs(btScalar(splitcount[i][0]-splitcount[i][1]));
 
330
                                        if(midp<bestmidp)
 
331
                                        {
 
332
                                                bestaxis=i;
 
333
                                                bestmidp=midp;
 
334
                                        }
 
335
                                }
 
336
                        }
 
337
                        if(bestaxis>=0)
 
338
                        {
 
339
                                sets[0].reserve(splitcount[bestaxis][0]);
 
340
                                sets[1].reserve(splitcount[bestaxis][1]);
 
341
                                split(leaves,sets[0],sets[1],org,axis[bestaxis]);
 
342
                        }
 
343
                        else
 
344
                        {
 
345
                                sets[0].reserve(leaves.size()/2+1);
 
346
                                sets[1].reserve(leaves.size()/2);
 
347
                                for(int i=0,ni=leaves.size();i<ni;++i)
 
348
                                {
 
349
                                        sets[i&1].push_back(leaves[i]);
 
350
                                }
 
351
                        }
 
352
                        btDbvtNode*     node=createnode(pdbvt,0,vol,0);
 
353
                        node->childs[0]=topdown(pdbvt,sets[0],bu_treshold);
 
354
                        node->childs[1]=topdown(pdbvt,sets[1],bu_treshold);
 
355
                        node->childs[0]->parent=node;
 
356
                        node->childs[1]->parent=node;
 
357
                        return(node);
 
358
                }
 
359
                else
 
360
                {
 
361
                        bottomup(pdbvt,leaves);
 
362
                        return(leaves[0]);
 
363
                }
 
364
        }
 
365
        return(leaves[0]);
 
366
}
 
367
 
 
368
//
 
369
static DBVT_INLINE btDbvtNode*  sort(btDbvtNode* n,btDbvtNode*& r)
 
370
{
 
371
        btDbvtNode*     p=n->parent;
 
372
        btAssert(n->isinternal());
 
373
        if(p>n)
 
374
        {
 
375
                const int               i=indexof(n);
 
376
                const int               j=1-i;
 
377
                btDbvtNode*     s=p->childs[j];
 
378
                btDbvtNode*     q=p->parent;
 
379
                btAssert(n==p->childs[i]);
 
380
                if(q) q->childs[indexof(p)]=n; else r=n;
 
381
                s->parent=n;
 
382
                p->parent=n;
 
383
                n->parent=q;
 
384
                p->childs[0]=n->childs[0];
 
385
                p->childs[1]=n->childs[1];
 
386
                n->childs[0]->parent=p;
 
387
                n->childs[1]->parent=p;
 
388
                n->childs[i]=p;
 
389
                n->childs[j]=s;
 
390
                btSwap(p->volume,n->volume);
 
391
                return(p);
 
392
        }
 
393
        return(n);
 
394
}
 
395
 
 
396
#if 0
 
397
static DBVT_INLINE btDbvtNode*  walkup(btDbvtNode* n,int count)
 
398
{
 
399
        while(n&&(count--)) n=n->parent;
 
400
        return(n);
 
401
}
 
402
#endif
 
403
 
 
404
//
 
405
// Api
 
406
//
 
407
 
 
408
//
 
409
btDbvt::btDbvt()
 
410
{
 
411
        m_root          =       0;
 
412
        m_free          =       0;
 
413
        m_lkhd          =       -1;
 
414
        m_leaves        =       0;
 
415
        m_opath         =       0;
 
416
}
 
417
 
 
418
//
 
419
btDbvt::~btDbvt()
 
420
{
 
421
        clear();
 
422
}
 
423
 
 
424
//
 
425
void                    btDbvt::clear()
 
426
{
 
427
        if(m_root)      
 
428
                recursedeletenode(this,m_root);
 
429
        btAlignedFree(m_free);
 
430
        m_free=0;
 
431
        m_lkhd          =       -1;
 
432
        m_stkStack.clear();
 
433
        m_opath         =       0;
 
434
        
 
435
}
 
436
 
 
437
//
 
438
void                    btDbvt::optimizeBottomUp()
 
439
{
 
440
        if(m_root)
 
441
        {
 
442
                tNodeArray leaves;
 
443
                leaves.reserve(m_leaves);
 
444
                fetchleaves(this,m_root,leaves);
 
445
                bottomup(this,leaves);
 
446
                m_root=leaves[0];
 
447
        }
 
448
}
 
449
 
 
450
//
 
451
void                    btDbvt::optimizeTopDown(int bu_treshold)
 
452
{
 
453
        if(m_root)
 
454
        {
 
455
                tNodeArray      leaves;
 
456
                leaves.reserve(m_leaves);
 
457
                fetchleaves(this,m_root,leaves);
 
458
                m_root=topdown(this,leaves,bu_treshold);
 
459
        }
 
460
}
 
461
 
 
462
//
 
463
void                    btDbvt::optimizeIncremental(int passes)
 
464
{
 
465
        if(passes<0) passes=m_leaves;
 
466
        if(m_root&&(passes>0))
 
467
        {
 
468
                do      {
 
469
                        btDbvtNode*             node=m_root;
 
470
                        unsigned        bit=0;
 
471
                        while(node->isinternal())
 
472
                        {
 
473
                                node=sort(node,m_root)->childs[(m_opath>>bit)&1];
 
474
                                bit=(bit+1)&(sizeof(unsigned)*8-1);
 
475
                        }
 
476
                        update(node);
 
477
                        ++m_opath;
 
478
                } while(--passes);
 
479
        }
 
480
}
 
481
 
 
482
//
 
483
btDbvtNode*     btDbvt::insert(const btDbvtVolume& volume,void* data)
 
484
{
 
485
        btDbvtNode*     leaf=createnode(this,0,volume,data);
 
486
        insertleaf(this,m_root,leaf);
 
487
        ++m_leaves;
 
488
        return(leaf);
 
489
}
 
490
 
 
491
//
 
492
void                    btDbvt::update(btDbvtNode* leaf,int lookahead)
 
493
{
 
494
        btDbvtNode*     root=removeleaf(this,leaf);
 
495
        if(root)
 
496
        {
 
497
                if(lookahead>=0)
 
498
                {
 
499
                        for(int i=0;(i<lookahead)&&root->parent;++i)
 
500
                        {
 
501
                                root=root->parent;
 
502
                        }
 
503
                } else root=m_root;
 
504
        }
 
505
        insertleaf(this,root,leaf);
 
506
}
 
507
 
 
508
//
 
509
void                    btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume)
 
510
{
 
511
        btDbvtNode*     root=removeleaf(this,leaf);
 
512
        if(root)
 
513
        {
 
514
                if(m_lkhd>=0)
 
515
                {
 
516
                        for(int i=0;(i<m_lkhd)&&root->parent;++i)
 
517
                        {
 
518
                                root=root->parent;
 
519
                        }
 
520
                } else root=m_root;
 
521
        }
 
522
        leaf->volume=volume;
 
523
        insertleaf(this,root,leaf);
 
524
}
 
525
 
 
526
//
 
527
bool                    btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity,btScalar margin)
 
528
{
 
529
        if(leaf->volume.Contain(volume)) return(false);
 
530
        volume.Expand(btVector3(margin,margin,margin));
 
531
        volume.SignedExpand(velocity);
 
532
        update(leaf,volume);
 
533
        return(true);
 
534
}
 
535
 
 
536
//
 
537
bool                    btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity)
 
538
{
 
539
        if(leaf->volume.Contain(volume)) return(false);
 
540
        volume.SignedExpand(velocity);
 
541
        update(leaf,volume);
 
542
        return(true);
 
543
}
 
544
 
 
545
//
 
546
bool                    btDbvt::update(btDbvtNode* leaf,btDbvtVolume& volume,btScalar margin)
 
547
{
 
548
        if(leaf->volume.Contain(volume)) return(false);
 
549
        volume.Expand(btVector3(margin,margin,margin));
 
550
        update(leaf,volume);
 
551
        return(true);
 
552
}
 
553
 
 
554
//
 
555
void                    btDbvt::remove(btDbvtNode* leaf)
 
556
{
 
557
        removeleaf(this,leaf);
 
558
        deletenode(this,leaf);
 
559
        --m_leaves;
 
560
}
 
561
 
 
562
//
 
563
void                    btDbvt::write(IWriter* iwriter) const
 
564
{
 
565
        btDbvtNodeEnumerator    nodes;
 
566
        nodes.nodes.reserve(m_leaves*2);
 
567
        enumNodes(m_root,nodes);
 
568
        iwriter->Prepare(m_root,nodes.nodes.size());
 
569
        for(int i=0;i<nodes.nodes.size();++i)
 
570
        {
 
571
                const btDbvtNode* n=nodes.nodes[i];
 
572
                int                     p=-1;
 
573
                if(n->parent) p=nodes.nodes.findLinearSearch(n->parent);
 
574
                if(n->isinternal())
 
575
                {
 
576
                        const int       c0=nodes.nodes.findLinearSearch(n->childs[0]);
 
577
                        const int       c1=nodes.nodes.findLinearSearch(n->childs[1]);
 
578
                        iwriter->WriteNode(n,i,p,c0,c1);
 
579
                }
 
580
                else
 
581
                {
 
582
                        iwriter->WriteLeaf(n,i,p);
 
583
                }       
 
584
        }
 
585
}
 
586
 
 
587
//
 
588
void                    btDbvt::clone(btDbvt& dest,IClone* iclone) const
 
589
{
 
590
        dest.clear();
 
591
        if(m_root!=0)
 
592
        {       
 
593
                btAlignedObjectArray<sStkCLN>   stack;
 
594
                stack.reserve(m_leaves);
 
595
                stack.push_back(sStkCLN(m_root,0));
 
596
                do      {
 
597
                        const int               i=stack.size()-1;
 
598
                        const sStkCLN   e=stack[i];
 
599
                        btDbvtNode*                     n=createnode(&dest,e.parent,e.node->volume,e.node->data);
 
600
                        stack.pop_back();
 
601
                        if(e.parent!=0)
 
602
                                e.parent->childs[i&1]=n;
 
603
                        else
 
604
                                dest.m_root=n;
 
605
                        if(e.node->isinternal())
 
606
                        {
 
607
                                stack.push_back(sStkCLN(e.node->childs[0],n));
 
608
                                stack.push_back(sStkCLN(e.node->childs[1],n));
 
609
                        }
 
610
                        else
 
611
                        {
 
612
                                iclone->CloneLeaf(n);
 
613
                        }
 
614
                } while(stack.size()>0);
 
615
        }
 
616
}
 
617
 
 
618
//
 
619
int                             btDbvt::maxdepth(const btDbvtNode* node)
 
620
{
 
621
        int     depth=0;
 
622
        if(node) getmaxdepth(node,1,depth);
 
623
        return(depth);
 
624
}
 
625
 
 
626
//
 
627
int                             btDbvt::countLeaves(const btDbvtNode* node)
 
628
{
 
629
        if(node->isinternal())
 
630
                return(countLeaves(node->childs[0])+countLeaves(node->childs[1]));
 
631
        else
 
632
                return(1);
 
633
}
 
634
 
 
635
//
 
636
void                    btDbvt::extractLeaves(const btDbvtNode* node,btAlignedObjectArray<const btDbvtNode*>& leaves)
 
637
{
 
638
        if(node->isinternal())
 
639
        {
 
640
                extractLeaves(node->childs[0],leaves);
 
641
                extractLeaves(node->childs[1],leaves);
 
642
        }
 
643
        else
 
644
        {
 
645
                leaves.push_back(node);
 
646
        }       
 
647
}
 
648
 
 
649
//
 
650
#if DBVT_ENABLE_BENCHMARK
 
651
 
 
652
#include <stdio.h>
 
653
#include <stdlib.h>
 
654
#include "LinearMath/btQuickProf.h"
 
655
 
 
656
/*
 
657
q6600,2.4ghz
 
658
 
 
659
/Ox /Ob2 /Oi /Ot /I "." /I "..\.." /I "..\..\src" /D "NDEBUG" /D "_LIB" /D "_WINDOWS" /D "_CRT_SECURE_NO_DEPRECATE" /D "_CRT_NONSTDC_NO_DEPRECATE" /D "WIN32"
 
660
/GF /FD /MT /GS- /Gy /arch:SSE2 /Zc:wchar_t- /Fp"..\..\out\release8\build\libbulletcollision\libbulletcollision.pch"
 
661
/Fo"..\..\out\release8\build\libbulletcollision\\"
 
662
/Fd"..\..\out\release8\build\libbulletcollision\bulletcollision.pdb"
 
663
/W3 /nologo /c /Wp64 /Zi /errorReport:prompt
 
664
 
 
665
Benchmarking dbvt...
 
666
World scale: 100.000000
 
667
Extents base: 1.000000
 
668
Extents range: 4.000000
 
669
Leaves: 8192
 
670
sizeof(btDbvtVolume): 32 bytes
 
671
sizeof(btDbvtNode):   44 bytes
 
672
[1] btDbvtVolume intersections: 3499 ms (-1%)
 
673
[2] btDbvtVolume merges: 1934 ms (0%)
 
674
[3] btDbvt::collideTT: 5485 ms (-21%)
 
675
[4] btDbvt::collideTT self: 2814 ms (-20%)
 
676
[5] btDbvt::collideTT xform: 7379 ms (-1%)
 
677
[6] btDbvt::collideTT xform,self: 7270 ms (-2%)
 
678
[7] btDbvt::rayTest: 6314 ms (0%),(332143 r/s)
 
679
[8] insert/remove: 2093 ms (0%),(1001983 ir/s)
 
680
[9] updates (teleport): 1879 ms (-3%),(1116100 u/s)
 
681
[10] updates (jitter): 1244 ms (-4%),(1685813 u/s)
 
682
[11] optimize (incremental): 2514 ms (0%),(1668000 o/s)
 
683
[12] btDbvtVolume notequal: 3659 ms (0%)
 
684
[13] culling(OCL+fullsort): 2218 ms (0%),(461 t/s)
 
685
[14] culling(OCL+qsort): 3688 ms (5%),(2221 t/s)
 
686
[15] culling(KDOP+qsort): 1139 ms (-1%),(7192 t/s)
 
687
[16] insert/remove batch(256): 5092 ms (0%),(823704 bir/s)
 
688
[17] btDbvtVolume select: 3419 ms (0%)
 
689
*/
 
690
 
 
691
struct btDbvtBenchmark
 
692
{
 
693
        struct NilPolicy : btDbvt::ICollide
 
694
        {
 
695
                NilPolicy() : m_pcount(0),m_depth(-SIMD_INFINITY),m_checksort(true)             {}
 
696
                void    Process(const btDbvtNode*,const btDbvtNode*)                            { ++m_pcount; }
 
697
                void    Process(const btDbvtNode*)                                                                      { ++m_pcount; }
 
698
                void    Process(const btDbvtNode*,btScalar depth)
 
699
                {
 
700
                        ++m_pcount;
 
701
                        if(m_checksort)
 
702
                        { if(depth>=m_depth) m_depth=depth; else printf("wrong depth: %f (should be >= %f)\r\n",depth,m_depth); }
 
703
                }
 
704
                int                     m_pcount;
 
705
                btScalar        m_depth;
 
706
                bool            m_checksort;
 
707
        };
 
708
        struct P14 : btDbvt::ICollide
 
709
        {
 
710
                struct Node
 
711
                {
 
712
                        const btDbvtNode*       leaf;
 
713
                        btScalar                        depth;
 
714
                };
 
715
                void Process(const btDbvtNode* leaf,btScalar depth)
 
716
                {
 
717
                        Node    n;
 
718
                        n.leaf  =       leaf;
 
719
                        n.depth =       depth;
 
720
                }
 
721
                static int sortfnc(const Node& a,const Node& b)
 
722
                {
 
723
                        if(a.depth<b.depth) return(+1);
 
724
                        if(a.depth>b.depth) return(-1);
 
725
                        return(0);
 
726
                }
 
727
                btAlignedObjectArray<Node>              m_nodes;
 
728
        };
 
729
        struct P15 : btDbvt::ICollide
 
730
        {
 
731
                struct Node
 
732
                {
 
733
                        const btDbvtNode*       leaf;
 
734
                        btScalar                        depth;
 
735
                };
 
736
                void Process(const btDbvtNode* leaf)
 
737
                {
 
738
                        Node    n;
 
739
                        n.leaf  =       leaf;
 
740
                        n.depth =       dot(leaf->volume.Center(),m_axis);
 
741
                }
 
742
                static int sortfnc(const Node& a,const Node& b)
 
743
                {
 
744
                        if(a.depth<b.depth) return(+1);
 
745
                        if(a.depth>b.depth) return(-1);
 
746
                        return(0);
 
747
                }
 
748
                btAlignedObjectArray<Node>              m_nodes;
 
749
                btVector3                                               m_axis;
 
750
        };
 
751
        static btScalar                 RandUnit()
 
752
        {
 
753
                return(rand()/(btScalar)RAND_MAX);
 
754
        }
 
755
        static btVector3                RandVector3()
 
756
        {
 
757
                return(btVector3(RandUnit(),RandUnit(),RandUnit()));
 
758
        }
 
759
        static btVector3                RandVector3(btScalar cs)
 
760
        {
 
761
                return(RandVector3()*cs-btVector3(cs,cs,cs)/2);
 
762
        }
 
763
        static btDbvtVolume     RandVolume(btScalar cs,btScalar eb,btScalar es)
 
764
        {
 
765
                return(btDbvtVolume::FromCE(RandVector3(cs),btVector3(eb,eb,eb)+RandVector3()*es));
 
766
        }
 
767
        static btTransform              RandTransform(btScalar cs)
 
768
        {
 
769
                btTransform     t;
 
770
                t.setOrigin(RandVector3(cs));
 
771
                t.setRotation(btQuaternion(RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2).normalized());
 
772
                return(t);
 
773
        }
 
774
        static void                             RandTree(btScalar cs,btScalar eb,btScalar es,int leaves,btDbvt& dbvt)
 
775
        {
 
776
                dbvt.clear();
 
777
                for(int i=0;i<leaves;++i)
 
778
                {
 
779
                        dbvt.insert(RandVolume(cs,eb,es),0);
 
780
                }
 
781
        }
 
782
};
 
783
 
 
784
void                    btDbvt::benchmark()
 
785
{
 
786
        static const btScalar   cfgVolumeCenterScale            =       100;
 
787
        static const btScalar   cfgVolumeExentsBase                     =       1;
 
788
        static const btScalar   cfgVolumeExentsScale            =       4;
 
789
        static const int                cfgLeaves                                       =       8192;
 
790
        static const bool               cfgEnable                                       =       true;
 
791
 
 
792
        //[1] btDbvtVolume intersections
 
793
        bool                                    cfgBenchmark1_Enable            =       cfgEnable;
 
794
        static const int                cfgBenchmark1_Iterations        =       8;
 
795
        static const int                cfgBenchmark1_Reference         =       3499;
 
796
        //[2] btDbvtVolume merges
 
797
        bool                                    cfgBenchmark2_Enable            =       cfgEnable;
 
798
        static const int                cfgBenchmark2_Iterations        =       4;
 
799
        static const int                cfgBenchmark2_Reference         =       1945;
 
800
        //[3] btDbvt::collideTT
 
801
        bool                                    cfgBenchmark3_Enable            =       cfgEnable;
 
802
        static const int                cfgBenchmark3_Iterations        =       512;
 
803
        static const int                cfgBenchmark3_Reference         =       5485;
 
804
        //[4] btDbvt::collideTT self
 
805
        bool                                    cfgBenchmark4_Enable            =       cfgEnable;
 
806
        static const int                cfgBenchmark4_Iterations        =       512;
 
807
        static const int                cfgBenchmark4_Reference         =       2814;
 
808
        //[5] btDbvt::collideTT xform
 
809
        bool                                    cfgBenchmark5_Enable            =       cfgEnable;
 
810
        static const int                cfgBenchmark5_Iterations        =       512;
 
811
        static const btScalar   cfgBenchmark5_OffsetScale       =       2;
 
812
        static const int                cfgBenchmark5_Reference         =       7379;
 
813
        //[6] btDbvt::collideTT xform,self
 
814
        bool                                    cfgBenchmark6_Enable            =       cfgEnable;
 
815
        static const int                cfgBenchmark6_Iterations        =       512;
 
816
        static const btScalar   cfgBenchmark6_OffsetScale       =       2;
 
817
        static const int                cfgBenchmark6_Reference         =       7270;
 
818
        //[7] btDbvt::rayTest
 
819
        bool                                    cfgBenchmark7_Enable            =       cfgEnable;
 
820
        static const int                cfgBenchmark7_Passes            =       32;
 
821
        static const int                cfgBenchmark7_Iterations        =       65536;
 
822
        static const int                cfgBenchmark7_Reference         =       6307;
 
823
        //[8] insert/remove
 
824
        bool                                    cfgBenchmark8_Enable            =       cfgEnable;
 
825
        static const int                cfgBenchmark8_Passes            =       32;
 
826
        static const int                cfgBenchmark8_Iterations        =       65536;
 
827
        static const int                cfgBenchmark8_Reference         =       2105;
 
828
        //[9] updates (teleport)
 
829
        bool                                    cfgBenchmark9_Enable            =       cfgEnable;
 
830
        static const int                cfgBenchmark9_Passes            =       32;
 
831
        static const int                cfgBenchmark9_Iterations        =       65536;
 
832
        static const int                cfgBenchmark9_Reference         =       1879;
 
833
        //[10] updates (jitter)
 
834
        bool                                    cfgBenchmark10_Enable           =       cfgEnable;
 
835
        static const btScalar   cfgBenchmark10_Scale            =       cfgVolumeCenterScale/10000;
 
836
        static const int                cfgBenchmark10_Passes           =       32;
 
837
        static const int                cfgBenchmark10_Iterations       =       65536;
 
838
        static const int                cfgBenchmark10_Reference        =       1244;
 
839
        //[11] optimize (incremental)
 
840
        bool                                    cfgBenchmark11_Enable           =       cfgEnable;
 
841
        static const int                cfgBenchmark11_Passes           =       64;
 
842
        static const int                cfgBenchmark11_Iterations       =       65536;
 
843
        static const int                cfgBenchmark11_Reference        =       2510;
 
844
        //[12] btDbvtVolume notequal
 
845
        bool                                    cfgBenchmark12_Enable           =       cfgEnable;
 
846
        static const int                cfgBenchmark12_Iterations       =       32;
 
847
        static const int                cfgBenchmark12_Reference        =       3677;
 
848
        //[13] culling(OCL+fullsort)
 
849
        bool                                    cfgBenchmark13_Enable           =       cfgEnable;
 
850
        static const int                cfgBenchmark13_Iterations       =       1024;
 
851
        static const int                cfgBenchmark13_Reference        =       2231;
 
852
        //[14] culling(OCL+qsort)
 
853
        bool                                    cfgBenchmark14_Enable           =       cfgEnable;
 
854
        static const int                cfgBenchmark14_Iterations       =       8192;
 
855
        static const int                cfgBenchmark14_Reference        =       3500;
 
856
        //[15] culling(KDOP+qsort)
 
857
        bool                                    cfgBenchmark15_Enable           =       cfgEnable;
 
858
        static const int                cfgBenchmark15_Iterations       =       8192;
 
859
        static const int                cfgBenchmark15_Reference        =       1151;
 
860
        //[16] insert/remove batch
 
861
        bool                                    cfgBenchmark16_Enable           =       cfgEnable;
 
862
        static const int                cfgBenchmark16_BatchCount       =       256;
 
863
        static const int                cfgBenchmark16_Passes           =       16384;
 
864
        static const int                cfgBenchmark16_Reference        =       5138;
 
865
        //[17] select
 
866
        bool                                    cfgBenchmark17_Enable           =       cfgEnable;
 
867
        static const int                cfgBenchmark17_Iterations       =       4;
 
868
        static const int                cfgBenchmark17_Reference        =       3390;
 
869
 
 
870
        btClock                                 wallclock;
 
871
        printf("Benchmarking dbvt...\r\n");
 
872
        printf("\tWorld scale: %f\r\n",cfgVolumeCenterScale);
 
873
        printf("\tExtents base: %f\r\n",cfgVolumeExentsBase);
 
874
        printf("\tExtents range: %f\r\n",cfgVolumeExentsScale);
 
875
        printf("\tLeaves: %u\r\n",cfgLeaves);
 
876
        printf("\tsizeof(btDbvtVolume): %u bytes\r\n",sizeof(btDbvtVolume));
 
877
        printf("\tsizeof(btDbvtNode):   %u bytes\r\n",sizeof(btDbvtNode));
 
878
        if(cfgBenchmark1_Enable)
 
879
        {// Benchmark 1 
 
880
                srand(380843);
 
881
                btAlignedObjectArray<btDbvtVolume>      volumes;
 
882
                btAlignedObjectArray<bool>                      results;
 
883
                volumes.resize(cfgLeaves);
 
884
                results.resize(cfgLeaves);
 
885
                for(int i=0;i<cfgLeaves;++i)
 
886
                {
 
887
                        volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
 
888
                }
 
889
                printf("[1] btDbvtVolume intersections: ");
 
890
                wallclock.reset();
 
891
                for(int i=0;i<cfgBenchmark1_Iterations;++i)
 
892
                {
 
893
                        for(int j=0;j<cfgLeaves;++j)
 
894
                        {
 
895
                                for(int k=0;k<cfgLeaves;++k)
 
896
                                {
 
897
                                        results[k]=Intersect(volumes[j],volumes[k]);
 
898
                                }
 
899
                        }
 
900
                }
 
901
                const int time=(int)wallclock.getTimeMilliseconds();
 
902
                printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark1_Reference)*100/time);
 
903
        }
 
904
        if(cfgBenchmark2_Enable)
 
905
        {// Benchmark 2 
 
906
                srand(380843);
 
907
                btAlignedObjectArray<btDbvtVolume>      volumes;
 
908
                btAlignedObjectArray<btDbvtVolume>      results;
 
909
                volumes.resize(cfgLeaves);
 
910
                results.resize(cfgLeaves);
 
911
                for(int i=0;i<cfgLeaves;++i)
 
912
                {
 
913
                        volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
 
914
                }
 
915
                printf("[2] btDbvtVolume merges: ");
 
916
                wallclock.reset();
 
917
                for(int i=0;i<cfgBenchmark2_Iterations;++i)
 
918
                {
 
919
                        for(int j=0;j<cfgLeaves;++j)
 
920
                        {
 
921
                                for(int k=0;k<cfgLeaves;++k)
 
922
                                {
 
923
                                        Merge(volumes[j],volumes[k],results[k]);
 
924
                                }
 
925
                        }
 
926
                }
 
927
                const int time=(int)wallclock.getTimeMilliseconds();
 
928
                printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark2_Reference)*100/time);
 
929
        }
 
930
        if(cfgBenchmark3_Enable)
 
931
        {// Benchmark 3 
 
932
                srand(380843);
 
933
                btDbvt                                          dbvt[2];
 
934
                btDbvtBenchmark::NilPolicy      policy;
 
935
                btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[0]);
 
936
                btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[1]);
 
937
                dbvt[0].optimizeTopDown();
 
938
                dbvt[1].optimizeTopDown();
 
939
                printf("[3] btDbvt::collideTT: ");
 
940
                wallclock.reset();
 
941
                for(int i=0;i<cfgBenchmark3_Iterations;++i)
 
942
                {
 
943
                        btDbvt::collideTT(dbvt[0].m_root,dbvt[1].m_root,policy);
 
944
                }
 
945
                const int time=(int)wallclock.getTimeMilliseconds();
 
946
                printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark3_Reference)*100/time);
 
947
        }
 
948
        if(cfgBenchmark4_Enable)
 
949
        {// Benchmark 4
 
950
                srand(380843);
 
951
                btDbvt                                          dbvt;
 
952
                btDbvtBenchmark::NilPolicy      policy;
 
953
                btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
 
954
                dbvt.optimizeTopDown();
 
955
                printf("[4] btDbvt::collideTT self: ");
 
956
                wallclock.reset();
 
957
                for(int i=0;i<cfgBenchmark4_Iterations;++i)
 
958
                {
 
959
                        btDbvt::collideTT(dbvt.m_root,dbvt.m_root,policy);
 
960
                }
 
961
                const int time=(int)wallclock.getTimeMilliseconds();
 
962
                printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark4_Reference)*100/time);
 
963
        }
 
964
        if(cfgBenchmark5_Enable)
 
965
        {// Benchmark 5 
 
966
                srand(380843);
 
967
                btDbvt                                                          dbvt[2];
 
968
                btAlignedObjectArray<btTransform>       transforms;
 
969
                btDbvtBenchmark::NilPolicy                      policy;
 
970
                transforms.resize(cfgBenchmark5_Iterations);
 
971
                for(int i=0;i<transforms.size();++i)
 
972
                {
 
973
                        transforms[i]=btDbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark5_OffsetScale);
 
974
                }
 
975
                btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[0]);
 
976
                btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt[1]);
 
977
                dbvt[0].optimizeTopDown();
 
978
                dbvt[1].optimizeTopDown();
 
979
                printf("[5] btDbvt::collideTT xform: ");
 
980
                wallclock.reset();
 
981
                for(int i=0;i<cfgBenchmark5_Iterations;++i)
 
982
                {
 
983
                        btDbvt::collideTT(dbvt[0].m_root,dbvt[1].m_root,transforms[i],policy);
 
984
                }
 
985
                const int time=(int)wallclock.getTimeMilliseconds();
 
986
                printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark5_Reference)*100/time);
 
987
        }
 
988
        if(cfgBenchmark6_Enable)
 
989
        {// Benchmark 6 
 
990
                srand(380843);
 
991
                btDbvt                                                          dbvt;
 
992
                btAlignedObjectArray<btTransform>       transforms;
 
993
                btDbvtBenchmark::NilPolicy                      policy;
 
994
                transforms.resize(cfgBenchmark6_Iterations);
 
995
                for(int i=0;i<transforms.size();++i)
 
996
                {
 
997
                        transforms[i]=btDbvtBenchmark::RandTransform(cfgVolumeCenterScale*cfgBenchmark6_OffsetScale);
 
998
                }
 
999
                btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
 
1000
                dbvt.optimizeTopDown();
 
1001
                printf("[6] btDbvt::collideTT xform,self: ");
 
1002
                wallclock.reset();
 
1003
                for(int i=0;i<cfgBenchmark6_Iterations;++i)
 
1004
                {
 
1005
                        btDbvt::collideTT(dbvt.m_root,dbvt.m_root,transforms[i],policy);                
 
1006
                }
 
1007
                const int time=(int)wallclock.getTimeMilliseconds();
 
1008
                printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark6_Reference)*100/time);
 
1009
        }
 
1010
        if(cfgBenchmark7_Enable)
 
1011
        {// Benchmark 7 
 
1012
                srand(380843);
 
1013
                btDbvt                                                          dbvt;
 
1014
                btAlignedObjectArray<btVector3>         rayorg;
 
1015
                btAlignedObjectArray<btVector3>         raydir;
 
1016
                btDbvtBenchmark::NilPolicy                      policy;
 
1017
                rayorg.resize(cfgBenchmark7_Iterations);
 
1018
                raydir.resize(cfgBenchmark7_Iterations);
 
1019
                for(int i=0;i<rayorg.size();++i)
 
1020
                {
 
1021
                        rayorg[i]=btDbvtBenchmark::RandVector3(cfgVolumeCenterScale*2);
 
1022
                        raydir[i]=btDbvtBenchmark::RandVector3(cfgVolumeCenterScale*2);
 
1023
                }
 
1024
                btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
 
1025
                dbvt.optimizeTopDown();
 
1026
                printf("[7] btDbvt::rayTest: ");
 
1027
                wallclock.reset();
 
1028
                for(int i=0;i<cfgBenchmark7_Passes;++i)
 
1029
                {
 
1030
                        for(int j=0;j<cfgBenchmark7_Iterations;++j)
 
1031
                        {
 
1032
                                btDbvt::rayTest(dbvt.m_root,rayorg[j],rayorg[j]+raydir[j],policy);
 
1033
                        }
 
1034
                }
 
1035
                const int       time=(int)wallclock.getTimeMilliseconds();
 
1036
                unsigned        rays=cfgBenchmark7_Passes*cfgBenchmark7_Iterations;
 
1037
                printf("%u ms (%i%%),(%u r/s)\r\n",time,(time-cfgBenchmark7_Reference)*100/time,(rays*1000)/time);
 
1038
        }
 
1039
        if(cfgBenchmark8_Enable)
 
1040
        {// Benchmark 8 
 
1041
                srand(380843);
 
1042
                btDbvt                                                          dbvt;
 
1043
                btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
 
1044
                dbvt.optimizeTopDown();
 
1045
                printf("[8] insert/remove: ");
 
1046
                wallclock.reset();
 
1047
                for(int i=0;i<cfgBenchmark8_Passes;++i)
 
1048
                {
 
1049
                        for(int j=0;j<cfgBenchmark8_Iterations;++j)
 
1050
                        {
 
1051
                                dbvt.remove(dbvt.insert(btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale),0));
 
1052
                        }
 
1053
                }
 
1054
                const int       time=(int)wallclock.getTimeMilliseconds();
 
1055
                const int       ir=cfgBenchmark8_Passes*cfgBenchmark8_Iterations;
 
1056
                printf("%u ms (%i%%),(%u ir/s)\r\n",time,(time-cfgBenchmark8_Reference)*100/time,ir*1000/time);
 
1057
        }
 
1058
        if(cfgBenchmark9_Enable)
 
1059
        {// Benchmark 9 
 
1060
                srand(380843);
 
1061
                btDbvt                                                                          dbvt;
 
1062
                btAlignedObjectArray<const btDbvtNode*> leaves;
 
1063
                btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
 
1064
                dbvt.optimizeTopDown();
 
1065
                dbvt.extractLeaves(dbvt.m_root,leaves);
 
1066
                printf("[9] updates (teleport): ");
 
1067
                wallclock.reset();
 
1068
                for(int i=0;i<cfgBenchmark9_Passes;++i)
 
1069
                {
 
1070
                        for(int j=0;j<cfgBenchmark9_Iterations;++j)
 
1071
                        {
 
1072
                                dbvt.update(const_cast<btDbvtNode*>(leaves[rand()%cfgLeaves]),
 
1073
                                        btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale));
 
1074
                        }
 
1075
                }
 
1076
                const int       time=(int)wallclock.getTimeMilliseconds();
 
1077
                const int       up=cfgBenchmark9_Passes*cfgBenchmark9_Iterations;
 
1078
                printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark9_Reference)*100/time,up*1000/time);
 
1079
        }
 
1080
        if(cfgBenchmark10_Enable)
 
1081
        {// Benchmark 10        
 
1082
                srand(380843);
 
1083
                btDbvt                                                                          dbvt;
 
1084
                btAlignedObjectArray<const btDbvtNode*> leaves;
 
1085
                btAlignedObjectArray<btVector3>                         vectors;
 
1086
                vectors.resize(cfgBenchmark10_Iterations);
 
1087
                for(int i=0;i<vectors.size();++i)
 
1088
                {
 
1089
                        vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1))*cfgBenchmark10_Scale;
 
1090
                }
 
1091
                btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
 
1092
                dbvt.optimizeTopDown();
 
1093
                dbvt.extractLeaves(dbvt.m_root,leaves);
 
1094
                printf("[10] updates (jitter): ");
 
1095
                wallclock.reset();
 
1096
 
 
1097
                for(int i=0;i<cfgBenchmark10_Passes;++i)
 
1098
                {
 
1099
                        for(int j=0;j<cfgBenchmark10_Iterations;++j)
 
1100
                        {                       
 
1101
                                const btVector3&        d=vectors[j];
 
1102
                                btDbvtNode*             l=const_cast<btDbvtNode*>(leaves[rand()%cfgLeaves]);
 
1103
                                btDbvtVolume            v=btDbvtVolume::FromMM(l->volume.Mins()+d,l->volume.Maxs()+d);
 
1104
                                dbvt.update(l,v);
 
1105
                        }
 
1106
                }
 
1107
                const int       time=(int)wallclock.getTimeMilliseconds();
 
1108
                const int       up=cfgBenchmark10_Passes*cfgBenchmark10_Iterations;
 
1109
                printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark10_Reference)*100/time,up*1000/time);
 
1110
        }
 
1111
        if(cfgBenchmark11_Enable)
 
1112
        {// Benchmark 11        
 
1113
                srand(380843);
 
1114
                btDbvt                                                                          dbvt;
 
1115
                btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
 
1116
                dbvt.optimizeTopDown();
 
1117
                printf("[11] optimize (incremental): ");
 
1118
                wallclock.reset();      
 
1119
                for(int i=0;i<cfgBenchmark11_Passes;++i)
 
1120
                {
 
1121
                        dbvt.optimizeIncremental(cfgBenchmark11_Iterations);
 
1122
                }
 
1123
                const int       time=(int)wallclock.getTimeMilliseconds();
 
1124
                const int       op=cfgBenchmark11_Passes*cfgBenchmark11_Iterations;
 
1125
                printf("%u ms (%i%%),(%u o/s)\r\n",time,(time-cfgBenchmark11_Reference)*100/time,op/time*1000);
 
1126
        }
 
1127
        if(cfgBenchmark12_Enable)
 
1128
        {// Benchmark 12        
 
1129
                srand(380843);
 
1130
                btAlignedObjectArray<btDbvtVolume>      volumes;
 
1131
                btAlignedObjectArray<bool>                              results;
 
1132
                volumes.resize(cfgLeaves);
 
1133
                results.resize(cfgLeaves);
 
1134
                for(int i=0;i<cfgLeaves;++i)
 
1135
                {
 
1136
                        volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
 
1137
                }
 
1138
                printf("[12] btDbvtVolume notequal: ");
 
1139
                wallclock.reset();
 
1140
                for(int i=0;i<cfgBenchmark12_Iterations;++i)
 
1141
                {
 
1142
                        for(int j=0;j<cfgLeaves;++j)
 
1143
                        {
 
1144
                                for(int k=0;k<cfgLeaves;++k)
 
1145
                                {
 
1146
                                        results[k]=NotEqual(volumes[j],volumes[k]);
 
1147
                                }
 
1148
                        }
 
1149
                }
 
1150
                const int time=(int)wallclock.getTimeMilliseconds();
 
1151
                printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark12_Reference)*100/time);
 
1152
        }
 
1153
        if(cfgBenchmark13_Enable)
 
1154
        {// Benchmark 13        
 
1155
                srand(380843);
 
1156
                btDbvt                                                          dbvt;
 
1157
                btAlignedObjectArray<btVector3>         vectors;
 
1158
                btDbvtBenchmark::NilPolicy                      policy;
 
1159
                vectors.resize(cfgBenchmark13_Iterations);
 
1160
                for(int i=0;i<vectors.size();++i)
 
1161
                {
 
1162
                        vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized();
 
1163
                }
 
1164
                btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
 
1165
                dbvt.optimizeTopDown();
 
1166
                printf("[13] culling(OCL+fullsort): ");
 
1167
                wallclock.reset();      
 
1168
                for(int i=0;i<cfgBenchmark13_Iterations;++i)
 
1169
                {
 
1170
                        static const btScalar   offset=0;
 
1171
                        policy.m_depth=-SIMD_INFINITY;
 
1172
                        dbvt.collideOCL(dbvt.m_root,&vectors[i],&offset,vectors[i],1,policy);
 
1173
                }
 
1174
                const int       time=(int)wallclock.getTimeMilliseconds();
 
1175
                const int       t=cfgBenchmark13_Iterations;
 
1176
                printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark13_Reference)*100/time,(t*1000)/time);
 
1177
        }
 
1178
        if(cfgBenchmark14_Enable)
 
1179
        {// Benchmark 14        
 
1180
                srand(380843);
 
1181
                btDbvt                                                          dbvt;
 
1182
                btAlignedObjectArray<btVector3>         vectors;
 
1183
                btDbvtBenchmark::P14                            policy;
 
1184
                vectors.resize(cfgBenchmark14_Iterations);
 
1185
                for(int i=0;i<vectors.size();++i)
 
1186
                {
 
1187
                        vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized();
 
1188
                }
 
1189
                btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
 
1190
                dbvt.optimizeTopDown();
 
1191
                policy.m_nodes.reserve(cfgLeaves);
 
1192
                printf("[14] culling(OCL+qsort): ");
 
1193
                wallclock.reset();      
 
1194
                for(int i=0;i<cfgBenchmark14_Iterations;++i)
 
1195
                {
 
1196
                        static const btScalar   offset=0;
 
1197
                        policy.m_nodes.resize(0);
 
1198
                        dbvt.collideOCL(dbvt.m_root,&vectors[i],&offset,vectors[i],1,policy,false);
 
1199
                        policy.m_nodes.quickSort(btDbvtBenchmark::P14::sortfnc);
 
1200
                }
 
1201
                const int       time=(int)wallclock.getTimeMilliseconds();
 
1202
                const int       t=cfgBenchmark14_Iterations;
 
1203
                printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark14_Reference)*100/time,(t*1000)/time);
 
1204
        }
 
1205
        if(cfgBenchmark15_Enable)
 
1206
        {// Benchmark 15        
 
1207
                srand(380843);
 
1208
                btDbvt                                                          dbvt;
 
1209
                btAlignedObjectArray<btVector3>         vectors;
 
1210
                btDbvtBenchmark::P15                            policy;
 
1211
                vectors.resize(cfgBenchmark15_Iterations);
 
1212
                for(int i=0;i<vectors.size();++i)
 
1213
                {
 
1214
                        vectors[i]=(btDbvtBenchmark::RandVector3()*2-btVector3(1,1,1)).normalized();
 
1215
                }
 
1216
                btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
 
1217
                dbvt.optimizeTopDown();
 
1218
                policy.m_nodes.reserve(cfgLeaves);
 
1219
                printf("[15] culling(KDOP+qsort): ");
 
1220
                wallclock.reset();      
 
1221
                for(int i=0;i<cfgBenchmark15_Iterations;++i)
 
1222
                {
 
1223
                        static const btScalar   offset=0;
 
1224
                        policy.m_nodes.resize(0);
 
1225
                        policy.m_axis=vectors[i];
 
1226
                        dbvt.collideKDOP(dbvt.m_root,&vectors[i],&offset,1,policy);
 
1227
                        policy.m_nodes.quickSort(btDbvtBenchmark::P15::sortfnc);
 
1228
                }
 
1229
                const int       time=(int)wallclock.getTimeMilliseconds();
 
1230
                const int       t=cfgBenchmark15_Iterations;
 
1231
                printf("%u ms (%i%%),(%u t/s)\r\n",time,(time-cfgBenchmark15_Reference)*100/time,(t*1000)/time);
 
1232
        }
 
1233
        if(cfgBenchmark16_Enable)
 
1234
        {// Benchmark 16        
 
1235
                srand(380843);
 
1236
                btDbvt                                                          dbvt;
 
1237
                btAlignedObjectArray<btDbvtNode*>       batch;
 
1238
                btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt);
 
1239
                dbvt.optimizeTopDown();
 
1240
                batch.reserve(cfgBenchmark16_BatchCount);
 
1241
                printf("[16] insert/remove batch(%u): ",cfgBenchmark16_BatchCount);
 
1242
                wallclock.reset();
 
1243
                for(int i=0;i<cfgBenchmark16_Passes;++i)
 
1244
                {
 
1245
                        for(int j=0;j<cfgBenchmark16_BatchCount;++j)
 
1246
                        {
 
1247
                                batch.push_back(dbvt.insert(btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale),0));
 
1248
                        }
 
1249
                        for(int j=0;j<cfgBenchmark16_BatchCount;++j)
 
1250
                        {
 
1251
                                dbvt.remove(batch[j]);
 
1252
                        }
 
1253
                        batch.resize(0);
 
1254
                }
 
1255
                const int       time=(int)wallclock.getTimeMilliseconds();
 
1256
                const int       ir=cfgBenchmark16_Passes*cfgBenchmark16_BatchCount;
 
1257
                printf("%u ms (%i%%),(%u bir/s)\r\n",time,(time-cfgBenchmark16_Reference)*100/time,int(ir*1000.0/time));
 
1258
        }
 
1259
        if(cfgBenchmark17_Enable)
 
1260
        {// Benchmark 17
 
1261
                srand(380843);
 
1262
                btAlignedObjectArray<btDbvtVolume>      volumes;
 
1263
                btAlignedObjectArray<int>                       results;
 
1264
                btAlignedObjectArray<int>                       indices;
 
1265
                volumes.resize(cfgLeaves);
 
1266
                results.resize(cfgLeaves);
 
1267
                indices.resize(cfgLeaves);
 
1268
                for(int i=0;i<cfgLeaves;++i)
 
1269
                {
 
1270
                        indices[i]=i;
 
1271
                        volumes[i]=btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale);
 
1272
                }
 
1273
                for(int i=0;i<cfgLeaves;++i)
 
1274
                {
 
1275
                        btSwap(indices[i],indices[rand()%cfgLeaves]);
 
1276
                }
 
1277
                printf("[17] btDbvtVolume select: ");
 
1278
                wallclock.reset();
 
1279
                for(int i=0;i<cfgBenchmark17_Iterations;++i)
 
1280
                {
 
1281
                        for(int j=0;j<cfgLeaves;++j)
 
1282
                        {
 
1283
                                for(int k=0;k<cfgLeaves;++k)
 
1284
                                {
 
1285
                                        const int idx=indices[k];
 
1286
                                        results[idx]=Select(volumes[idx],volumes[j],volumes[k]);
 
1287
                                }
 
1288
                        }
 
1289
                }
 
1290
                const int time=(int)wallclock.getTimeMilliseconds();
 
1291
                printf("%u ms (%i%%)\r\n",time,(time-cfgBenchmark17_Reference)*100/time);
 
1292
        }
 
1293
        printf("\r\n\r\n");
 
1294
}
 
1295
#endif