Merging trunk up to revision 41245.
[blender.git] / extern / bullet2 / 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