Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics_merge/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.h @ 2518

Last change on this file since 2518 was 2442, checked in by rgrieder, 16 years ago

Finally merged physics stuff. Target is physics_merge because I'll have to do some testing first.
  • Property svn:eol-style set to native
File size: 31.7 KB
Line 
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
15///btDbvt implementation by Nathanael Presson
16
17#ifndef BT_DYNAMIC_BOUNDING_VOLUME_TREE_H
18#define BT_DYNAMIC_BOUNDING_VOLUME_TREE_H
19
20#include "LinearMath/btAlignedObjectArray.h"
21#include "LinearMath/btVector3.h"
22#include "LinearMath/btTransform.h"
23#include "LinearMath/btAabbUtil2.h"
24
25//
26// Compile time configuration
27//
28
29
30// Implementation profiles
31#define DBVT_IMPL_GENERIC               0       // Generic implementation       
32#define DBVT_IMPL_SSE                   1       // SSE
33
34// Template implementation of ICollide
35#ifdef WIN32
36#if (defined (_MSC_VER) && _MSC_VER >= 1400)
37#define DBVT_USE_TEMPLATE               1
38#else
39#define DBVT_USE_TEMPLATE               0
40#endif
41#else
42#define DBVT_USE_TEMPLATE               0
43#endif
44
45// Use only intrinsics instead of inline asm
46#define DBVT_USE_INTRINSIC_SSE  1
47
48// Using memmov for collideOCL
49#define DBVT_USE_MEMMOVE                1
50
51// Enable benchmarking code
52#define DBVT_ENABLE_BENCHMARK   0
53
54// Inlining
55#define DBVT_INLINE                             SIMD_FORCE_INLINE
56
57// Specific methods implementation
58
59//SSE gives errors on a MSVC 7.1
60#if (defined (WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION))
61#define DBVT_SELECT_IMPL                DBVT_IMPL_SSE
62#define DBVT_MERGE_IMPL                 DBVT_IMPL_SSE
63#define DBVT_INT0_IMPL                  DBVT_IMPL_SSE
64#else
65#define DBVT_SELECT_IMPL                DBVT_IMPL_GENERIC
66#define DBVT_MERGE_IMPL                 DBVT_IMPL_GENERIC
67#define DBVT_INT0_IMPL                  DBVT_IMPL_GENERIC
68#endif
69
70#if     (DBVT_SELECT_IMPL==DBVT_IMPL_SSE)||     \
71        (DBVT_MERGE_IMPL==DBVT_IMPL_SSE)||      \
72        (DBVT_INT0_IMPL==DBVT_IMPL_SSE)
73#include <emmintrin.h>
74#endif
75
76//
77// Auto config and checks
78//
79
80#if DBVT_USE_TEMPLATE
81#define DBVT_VIRTUAL
82#define DBVT_VIRTUAL_DTOR(a)
83#define DBVT_PREFIX                                     template <typename T>
84#define DBVT_IPOLICY                            T& policy
85#define DBVT_CHECKTYPE                          static const ICollide&  typechecker=*(T*)0;
86#else
87#define DBVT_VIRTUAL_DTOR(a)            virtual ~a() {}
88#define DBVT_VIRTUAL                            virtual
89#define DBVT_PREFIX
90#define DBVT_IPOLICY                            ICollide& policy
91#define DBVT_CHECKTYPE
92#endif
93
94#if DBVT_USE_MEMMOVE
95#ifndef __CELLOS_LV2__
96#include <memory.h>
97#endif
98#include <string.h>
99#endif
100
101#ifndef DBVT_USE_TEMPLATE
102#error "DBVT_USE_TEMPLATE undefined"
103#endif
104
105#ifndef DBVT_USE_MEMMOVE
106#error "DBVT_USE_MEMMOVE undefined"
107#endif
108
109#ifndef DBVT_ENABLE_BENCHMARK
110#error "DBVT_ENABLE_BENCHMARK undefined"
111#endif
112
113#ifndef DBVT_SELECT_IMPL
114#error "DBVT_SELECT_IMPL undefined"
115#endif
116
117#ifndef DBVT_MERGE_IMPL
118#error "DBVT_MERGE_IMPL undefined"
119#endif
120
121#ifndef DBVT_INT0_IMPL
122#error "DBVT_INT0_IMPL undefined"
123#endif
124
125//
126// Defaults volumes
127//
128
129/* btDbvtAabbMm                 */ 
130struct  btDbvtAabbMm
131{
132        DBVT_INLINE btVector3                   Center() const  { return((mi+mx)/2); }
133        DBVT_INLINE btVector3                   Lengths() const { return(mx-mi); }
134        DBVT_INLINE btVector3                   Extents() const { return((mx-mi)/2); }
135        DBVT_INLINE const btVector3&    Mins() const    { return(mi); }
136        DBVT_INLINE const btVector3&    Maxs() const    { return(mx); }
137        static inline btDbvtAabbMm              FromCE(const btVector3& c,const btVector3& e);
138        static inline btDbvtAabbMm              FromCR(const btVector3& c,btScalar r);
139        static inline btDbvtAabbMm              FromMM(const btVector3& mi,const btVector3& mx);
140        static inline btDbvtAabbMm              FromPoints(const btVector3* pts,int n);
141        static inline btDbvtAabbMm              FromPoints(const btVector3** ppts,int n);
142        DBVT_INLINE void                                Expand(const btVector3& e);
143        DBVT_INLINE void                                SignedExpand(const btVector3& e);
144        DBVT_INLINE bool                                Contain(const btDbvtAabbMm& a) const;
145        DBVT_INLINE int                                 Classify(const btVector3& n,btScalar o,int s) const;
146        DBVT_INLINE btScalar                    ProjectMinimum(const btVector3& v,unsigned signs) const;
147        DBVT_INLINE friend bool                 Intersect(      const btDbvtAabbMm& a,
148                const btDbvtAabbMm& b);
149        DBVT_INLINE friend bool                 Intersect(      const btDbvtAabbMm& a,
150                const btDbvtAabbMm& b,
151                const btTransform& xform);
152        DBVT_INLINE friend bool                 Intersect(      const btDbvtAabbMm& a,
153                const btVector3& b);
154
155        DBVT_INLINE friend btScalar             Proximity(      const btDbvtAabbMm& a,
156                const btDbvtAabbMm& b);
157        DBVT_INLINE friend int                  Select(         const btDbvtAabbMm& o,
158                const btDbvtAabbMm& a,
159                const btDbvtAabbMm& b);
160        DBVT_INLINE friend void                 Merge(          const btDbvtAabbMm& a,
161                const btDbvtAabbMm& b,
162                btDbvtAabbMm& r);
163        DBVT_INLINE friend bool                 NotEqual(       const btDbvtAabbMm& a,
164                const btDbvtAabbMm& b);
165private:
166        DBVT_INLINE void                                AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const;
167private:
168        btVector3       mi,mx;
169};
170
171// Types       
172typedef btDbvtAabbMm    btDbvtVolume;
173
174/* btDbvtNode                           */ 
175struct  btDbvtNode
176{
177        btDbvtVolume    volume;
178        btDbvtNode*             parent;
179        DBVT_INLINE bool        isleaf() const          { return(childs[1]==0); }
180        DBVT_INLINE bool        isinternal() const      { return(!isleaf()); }
181        union
182        {
183                btDbvtNode*     childs[2];
184                void*   data;
185                int             dataAsInt;
186        };
187};
188
189///The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree).
190///This btDbvt is used for soft body collision detection and for the btDbvtBroadphase. It has a fast insert, remove and update of nodes.
191///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure.
192struct  btDbvt
193{
194        /* Stack element        */ 
195        struct  sStkNN
196        {
197                const btDbvtNode*       a;
198                const btDbvtNode*       b;
199                sStkNN() {}
200                sStkNN(const btDbvtNode* na,const btDbvtNode* nb) : a(na),b(nb) {}
201        };
202        struct  sStkNP
203        {
204                const btDbvtNode*       node;
205                int                     mask;
206                sStkNP(const btDbvtNode* n,unsigned m) : node(n),mask(m) {}
207        };
208        struct  sStkNPS
209        {
210                const btDbvtNode*       node;
211                int                     mask;
212                btScalar        value;
213                sStkNPS() {}
214                sStkNPS(const btDbvtNode* n,unsigned m,btScalar v) : node(n),mask(m),value(v) {}
215        };
216        struct  sStkCLN
217        {
218                const btDbvtNode*       node;
219                btDbvtNode*             parent;
220                sStkCLN(const btDbvtNode* n,btDbvtNode* p) : node(n),parent(p) {}
221        };
222        // Policies/Interfaces
223
224        /* ICollide     */ 
225        struct  ICollide
226        {               
227                DBVT_VIRTUAL_DTOR(ICollide)
228                        DBVT_VIRTUAL void       Process(const btDbvtNode*,const btDbvtNode*)            {}
229                DBVT_VIRTUAL void       Process(const btDbvtNode*)                                      {}
230                DBVT_VIRTUAL void       Process(const btDbvtNode* n,btScalar)                   { Process(n); }
231                DBVT_VIRTUAL bool       Descent(const btDbvtNode*)                                      { return(true); }
232                DBVT_VIRTUAL bool       AllLeaves(const btDbvtNode*)                                    { return(true); }
233        };
234        /* IWriter      */ 
235        struct  IWriter
236        {
237                virtual ~IWriter() {}
238                virtual void            Prepare(const btDbvtNode* root,int numnodes)=0;
239                virtual void            WriteNode(const btDbvtNode*,int index,int parent,int child0,int child1)=0;
240                virtual void            WriteLeaf(const btDbvtNode*,int index,int parent)=0;
241        };
242        /* IClone       */ 
243        struct  IClone
244        {
245                virtual ~IClone()       {}
246                virtual void            CloneLeaf(btDbvtNode*) {}
247        };
248
249        // Constants
250        enum    {
251                SIMPLE_STACKSIZE        =       64,
252                DOUBLE_STACKSIZE        =       SIMPLE_STACKSIZE*2
253        };
254
255        // Fields
256        btDbvtNode*             m_root;
257        btDbvtNode*             m_free;
258        int                             m_lkhd;
259        int                             m_leaves;
260        unsigned                m_opath;
261
262       
263        btAlignedObjectArray<sStkNN>    m_stkStack;
264
265
266        // Methods
267        btDbvt();
268        ~btDbvt();
269        void                    clear();
270        bool                    empty() const { return(0==m_root); }
271        void                    optimizeBottomUp();
272        void                    optimizeTopDown(int bu_treshold=128);
273        void                    optimizeIncremental(int passes);
274        btDbvtNode*             insert(const btDbvtVolume& box,void* data);
275        void                    update(btDbvtNode* leaf,int lookahead=-1);
276        void                    update(btDbvtNode* leaf,btDbvtVolume& volume);
277        bool                    update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity,btScalar margin);
278        bool                    update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity);
279        bool                    update(btDbvtNode* leaf,btDbvtVolume& volume,btScalar margin); 
280        void                    remove(btDbvtNode* leaf);
281        void                    write(IWriter* iwriter) const;
282        void                    clone(btDbvt& dest,IClone* iclone=0) const;
283        static int              maxdepth(const btDbvtNode* node);
284        static int              countLeaves(const btDbvtNode* node);
285        static void             extractLeaves(const btDbvtNode* node,btAlignedObjectArray<const btDbvtNode*>& leaves);
286#if DBVT_ENABLE_BENCHMARK
287        static void             benchmark();
288#else
289        static void             benchmark(){}
290#endif
291        // DBVT_IPOLICY must support ICollide policy/interface
292        DBVT_PREFIX
293                static void             enumNodes(      const btDbvtNode* root,
294                DBVT_IPOLICY);
295        DBVT_PREFIX
296                static void             enumLeaves(     const btDbvtNode* root,
297                DBVT_IPOLICY);
298        DBVT_PREFIX
299                void            collideTT(      const btDbvtNode* root0,
300                const btDbvtNode* root1,
301                DBVT_IPOLICY);
302
303        DBVT_PREFIX
304                void            collideTTpersistentStack(       const btDbvtNode* root0,
305                  const btDbvtNode* root1,
306                  DBVT_IPOLICY);
307
308        DBVT_PREFIX
309                void            collideTT(      const btDbvtNode* root0,
310                const btDbvtNode* root1,
311                const btTransform& xform,
312                DBVT_IPOLICY);
313        DBVT_PREFIX
314                void            collideTT(      const btDbvtNode* root0,
315                const btTransform& xform0,
316                const btDbvtNode* root1,
317                const btTransform& xform1,
318                DBVT_IPOLICY);
319        DBVT_PREFIX
320                void            collideTV(      const btDbvtNode* root,
321                const btDbvtVolume& volume,
322                DBVT_IPOLICY);
323        ///rayTest is a re-entrant ray test, and can be called in parallel as long as the btAlignedAlloc is thread-safe (uses locking etc)
324        ///rayTest is slower than rayTestInternal, because it builds a local stack, using memory allocations, and it recomputes signs/rayDirectionInverses each time
325        DBVT_PREFIX
326                static void             rayTest(        const btDbvtNode* root,
327                const btVector3& rayFrom,
328                const btVector3& rayTo,
329                DBVT_IPOLICY);
330        ///rayTestInternal is faster than rayTest, because it uses a persistent stack (to reduce dynamic memory allocations to a minimum) and it uses precomputed signs/rayInverseDirections
331        ///rayTestInternal is used by btDbvtBroadphase to accelerate world ray casts
332        DBVT_PREFIX
333                void            rayTestInternal(        const btDbvtNode* root,
334                                                                const btVector3& rayFrom,
335                                                                const btVector3& rayTo,
336                                                                const btVector3& rayDirectionInverse,
337                                                                unsigned int signs[3],
338                                                                btScalar lambda_max,
339                                                                const btVector3& aabbMin,
340                                                                const btVector3& aabbMax,
341                                                                DBVT_IPOLICY) const;
342
343        DBVT_PREFIX
344                static void             collideKDOP(const btDbvtNode* root,
345                const btVector3* normals,
346                const btScalar* offsets,
347                int count,
348                DBVT_IPOLICY);
349        DBVT_PREFIX
350                static void             collideOCL(     const btDbvtNode* root,
351                const btVector3* normals,
352                const btScalar* offsets,
353                const btVector3& sortaxis,
354                int count,                                                             
355                DBVT_IPOLICY,
356                bool fullsort=true);
357        DBVT_PREFIX
358                static void             collideTU(      const btDbvtNode* root,
359                DBVT_IPOLICY);
360        // Helpers     
361        static DBVT_INLINE int  nearest(const int* i,const btDbvt::sStkNPS* a,btScalar v,int l,int h)
362        {
363                int     m=0;
364                while(l<h)
365                {
366                        m=(l+h)>>1;
367                        if(a[i[m]].value>=v) l=m+1; else h=m;
368                }
369                return(h);
370        }
371        static DBVT_INLINE int  allocate(       btAlignedObjectArray<int>& ifree,
372                btAlignedObjectArray<sStkNPS>& stock,
373                const sStkNPS& value)
374        {
375                int     i;
376                if(ifree.size()>0)
377                { i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; }
378                else
379                { i=stock.size();stock.push_back(value); }
380                return(i); 
381        }
382        //
383private:
384        btDbvt(const btDbvt&)   {}     
385};
386
387//
388// Inline's
389//
390
391//
392inline btDbvtAabbMm                     btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e)
393{
394        btDbvtAabbMm box;
395        box.mi=c-e;box.mx=c+e;
396        return(box);
397}
398
399//
400inline btDbvtAabbMm                     btDbvtAabbMm::FromCR(const btVector3& c,btScalar r)
401{
402        return(FromCE(c,btVector3(r,r,r)));
403}
404
405//
406inline btDbvtAabbMm                     btDbvtAabbMm::FromMM(const btVector3& mi,const btVector3& mx)
407{
408        btDbvtAabbMm box;
409        box.mi=mi;box.mx=mx;
410        return(box);
411}
412
413//
414inline btDbvtAabbMm                     btDbvtAabbMm::FromPoints(const btVector3* pts,int n)
415{
416        btDbvtAabbMm box;
417        box.mi=box.mx=pts[0];
418        for(int i=1;i<n;++i)
419        {
420                box.mi.setMin(pts[i]);
421                box.mx.setMax(pts[i]);
422        }
423        return(box);
424}
425
426//
427inline btDbvtAabbMm                     btDbvtAabbMm::FromPoints(const btVector3** ppts,int n)
428{
429        btDbvtAabbMm box;
430        box.mi=box.mx=*ppts[0];
431        for(int i=1;i<n;++i)
432        {
433                box.mi.setMin(*ppts[i]);
434                box.mx.setMax(*ppts[i]);
435        }
436        return(box);
437}
438
439//
440DBVT_INLINE void                btDbvtAabbMm::Expand(const btVector3& e)
441{
442        mi-=e;mx+=e;
443}
444
445//
446DBVT_INLINE void                btDbvtAabbMm::SignedExpand(const btVector3& e)
447{
448        if(e.x()>0) mx.setX(mx.x()+e[0]); else mi.setX(mi.x()+e[0]);
449        if(e.y()>0) mx.setY(mx.y()+e[1]); else mi.setY(mi.y()+e[1]);
450        if(e.z()>0) mx.setZ(mx.z()+e[2]); else mi.setZ(mi.z()+e[2]);
451}
452
453//
454DBVT_INLINE bool                btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const
455{
456        return( (mi.x()<=a.mi.x())&&
457                (mi.y()<=a.mi.y())&&
458                (mi.z()<=a.mi.z())&&
459                (mx.x()>=a.mx.x())&&
460                (mx.y()>=a.mx.y())&&
461                (mx.z()>=a.mx.z()));
462}
463
464//
465DBVT_INLINE int         btDbvtAabbMm::Classify(const btVector3& n,btScalar o,int s) const
466{
467        btVector3                       pi,px;
468        switch(s)
469        {
470        case    (0+0+0):        px=btVector3(mi.x(),mi.y(),mi.z());
471                pi=btVector3(mx.x(),mx.y(),mx.z());break;
472        case    (1+0+0):        px=btVector3(mx.x(),mi.y(),mi.z());
473                pi=btVector3(mi.x(),mx.y(),mx.z());break;
474        case    (0+2+0):        px=btVector3(mi.x(),mx.y(),mi.z());
475                pi=btVector3(mx.x(),mi.y(),mx.z());break;
476        case    (1+2+0):        px=btVector3(mx.x(),mx.y(),mi.z());
477                pi=btVector3(mi.x(),mi.y(),mx.z());break;
478        case    (0+0+4):        px=btVector3(mi.x(),mi.y(),mx.z());
479                pi=btVector3(mx.x(),mx.y(),mi.z());break;
480        case    (1+0+4):        px=btVector3(mx.x(),mi.y(),mx.z());
481                pi=btVector3(mi.x(),mx.y(),mi.z());break;
482        case    (0+2+4):        px=btVector3(mi.x(),mx.y(),mx.z());
483                pi=btVector3(mx.x(),mi.y(),mi.z());break;
484        case    (1+2+4):        px=btVector3(mx.x(),mx.y(),mx.z());
485                pi=btVector3(mi.x(),mi.y(),mi.z());break;
486        }
487        if((dot(n,px)+o)<0)             return(-1);
488        if((dot(n,pi)+o)>=0)    return(+1);
489        return(0);
490}
491
492//
493DBVT_INLINE btScalar    btDbvtAabbMm::ProjectMinimum(const btVector3& v,unsigned signs) const
494{
495        const btVector3*        b[]={&mx,&mi};
496        const btVector3         p(      b[(signs>>0)&1]->x(),
497                b[(signs>>1)&1]->y(),
498                b[(signs>>2)&1]->z());
499        return(dot(p,v));
500}
501
502//
503DBVT_INLINE void                btDbvtAabbMm::AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const
504{
505        for(int i=0;i<3;++i)
506        {
507                if(d[i]<0)
508                { smi+=mx[i]*d[i];smx+=mi[i]*d[i]; }
509                else
510                { smi+=mi[i]*d[i];smx+=mx[i]*d[i]; }
511        }
512}
513
514//
515DBVT_INLINE bool                Intersect(      const btDbvtAabbMm& a,
516                                                                  const btDbvtAabbMm& b)
517{
518#if     DBVT_INT0_IMPL == DBVT_IMPL_SSE
519        const __m128    rt(_mm_or_ps(   _mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)),
520                _mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi))));
521        const __int32*  pu((const __int32*)&rt);
522        return((pu[0]|pu[1]|pu[2])==0);
523#else
524        return( (a.mi.x()<=b.mx.x())&&
525                (a.mx.x()>=b.mi.x())&&
526                (a.mi.y()<=b.mx.y())&&
527                (a.mx.y()>=b.mi.y())&&
528                (a.mi.z()<=b.mx.z())&&         
529                (a.mx.z()>=b.mi.z()));
530#endif
531}
532
533//
534DBVT_INLINE bool                Intersect(      const btDbvtAabbMm& a,
535                                                                  const btDbvtAabbMm& b,
536                                                                  const btTransform& xform)
537{
538        const btVector3         d0=xform*b.Center()-a.Center();
539        const btVector3         d1=d0*xform.getBasis();
540        btScalar                        s0[2]={0,0};
541        btScalar                        s1[2]={dot(xform.getOrigin(),d0),s1[0]};
542        a.AddSpan(d0,s0[0],s0[1]);
543        b.AddSpan(d1,s1[0],s1[1]);
544        if(s0[0]>(s1[1])) return(false);
545        if(s0[1]<(s1[0])) return(false);
546        return(true);
547}
548
549//
550DBVT_INLINE bool                Intersect(      const btDbvtAabbMm& a,
551                                                                  const btVector3& b)
552{
553        return( (b.x()>=a.mi.x())&&
554                (b.y()>=a.mi.y())&&
555                (b.z()>=a.mi.z())&&
556                (b.x()<=a.mx.x())&&
557                (b.y()<=a.mx.y())&&
558                (b.z()<=a.mx.z()));
559}
560
561
562
563
564
565//////////////////////////////////////
566
567
568//
569DBVT_INLINE btScalar    Proximity(      const btDbvtAabbMm& a,
570                                                                  const btDbvtAabbMm& b)
571{
572        const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx);
573        return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z()));
574}
575
576
577
578//
579DBVT_INLINE int                 Select( const btDbvtAabbMm& o,
580                                                           const btDbvtAabbMm& a,
581                                                           const btDbvtAabbMm& b)
582{
583#if     DBVT_SELECT_IMPL == DBVT_IMPL_SSE
584        static ATTRIBUTE_ALIGNED16(const unsigned __int32)      mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff};
585        ///@todo: the intrinsic version is 11% slower
586#if DBVT_USE_INTRINSIC_SSE
587
588        union btSSEUnion ///NOTE: if we use more intrinsics, move btSSEUnion into the LinearMath directory
589        {
590           __m128               ssereg;
591           float                floats[4];
592           int                  ints[4];
593        };
594
595        __m128  omi(_mm_load_ps(o.mi));
596        omi=_mm_add_ps(omi,_mm_load_ps(o.mx));
597        __m128  ami(_mm_load_ps(a.mi));
598        ami=_mm_add_ps(ami,_mm_load_ps(a.mx));
599        ami=_mm_sub_ps(ami,omi);
600        ami=_mm_and_ps(ami,_mm_load_ps((const float*)mask));
601        __m128  bmi(_mm_load_ps(b.mi));
602        bmi=_mm_add_ps(bmi,_mm_load_ps(b.mx));
603        bmi=_mm_sub_ps(bmi,omi);
604        bmi=_mm_and_ps(bmi,_mm_load_ps((const float*)mask));
605        __m128  t0(_mm_movehl_ps(ami,ami));
606        ami=_mm_add_ps(ami,t0);
607        ami=_mm_add_ss(ami,_mm_shuffle_ps(ami,ami,1));
608        __m128 t1(_mm_movehl_ps(bmi,bmi));
609        bmi=_mm_add_ps(bmi,t1);
610        bmi=_mm_add_ss(bmi,_mm_shuffle_ps(bmi,bmi,1));
611       
612        btSSEUnion tmp;
613        tmp.ssereg = _mm_cmple_ss(bmi,ami);
614        return tmp.ints[0]&1;
615
616#else
617        ATTRIBUTE_ALIGNED16(__int32     r[1]);
618        __asm
619        {
620                mov             eax,o
621                        mov             ecx,a
622                        mov             edx,b
623                        movaps  xmm0,[eax]
624                movaps  xmm5,mask
625                        addps   xmm0,[eax+16]   
626                movaps  xmm1,[ecx]
627                movaps  xmm2,[edx]
628                addps   xmm1,[ecx+16]
629                addps   xmm2,[edx+16]
630                subps   xmm1,xmm0
631                        subps   xmm2,xmm0
632                        andps   xmm1,xmm5
633                        andps   xmm2,xmm5
634                        movhlps xmm3,xmm1
635                        movhlps xmm4,xmm2
636                        addps   xmm1,xmm3
637                        addps   xmm2,xmm4
638                        pshufd  xmm3,xmm1,1
639                        pshufd  xmm4,xmm2,1
640                        addss   xmm1,xmm3
641                        addss   xmm2,xmm4
642                        cmpless xmm2,xmm1
643                        movss   r,xmm2
644        }
645        return(r[0]&1);
646#endif
647#else
648        return(Proximity(o,a)<Proximity(o,b)?0:1);
649#endif
650}
651
652//
653DBVT_INLINE void                Merge(  const btDbvtAabbMm& a,
654                                                          const btDbvtAabbMm& b,
655                                                          btDbvtAabbMm& r)
656{
657#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
658        __m128  ami(_mm_load_ps(a.mi));
659        __m128  amx(_mm_load_ps(a.mx));
660        __m128  bmi(_mm_load_ps(b.mi));
661        __m128  bmx(_mm_load_ps(b.mx));
662        ami=_mm_min_ps(ami,bmi);
663        amx=_mm_max_ps(amx,bmx);
664        _mm_store_ps(r.mi,ami);
665        _mm_store_ps(r.mx,amx);
666#else
667        for(int i=0;i<3;++i)
668        {
669                if(a.mi[i]<b.mi[i]) r.mi[i]=a.mi[i]; else r.mi[i]=b.mi[i];
670                if(a.mx[i]>b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i];
671        }
672#endif
673}
674
675//
676DBVT_INLINE bool                NotEqual(       const btDbvtAabbMm& a,
677                                                                 const btDbvtAabbMm& b)
678{
679        return( (a.mi.x()!=b.mi.x())||
680                (a.mi.y()!=b.mi.y())||
681                (a.mi.z()!=b.mi.z())||
682                (a.mx.x()!=b.mx.x())||
683                (a.mx.y()!=b.mx.y())||
684                (a.mx.z()!=b.mx.z()));
685}
686
687//
688// Inline's
689//
690
691//
692DBVT_PREFIX
693inline void             btDbvt::enumNodes(      const btDbvtNode* root,
694                                                                  DBVT_IPOLICY)
695{
696        DBVT_CHECKTYPE
697                policy.Process(root);
698        if(root->isinternal())
699        {
700                enumNodes(root->childs[0],policy);
701                enumNodes(root->childs[1],policy);
702        }
703}
704
705//
706DBVT_PREFIX
707inline void             btDbvt::enumLeaves(     const btDbvtNode* root,
708                                                                   DBVT_IPOLICY)
709{
710        DBVT_CHECKTYPE
711                if(root->isinternal())
712                {
713                        enumLeaves(root->childs[0],policy);
714                        enumLeaves(root->childs[1],policy);
715                }
716                else
717                {
718                        policy.Process(root);
719                }
720}
721
722//
723DBVT_PREFIX
724inline void             btDbvt::collideTT(      const btDbvtNode* root0,
725                                                                  const btDbvtNode* root1,
726                                                                  DBVT_IPOLICY)
727{
728        DBVT_CHECKTYPE
729                if(root0&&root1)
730                {
731                        int                                                             depth=1;
732                        int                                                             treshold=DOUBLE_STACKSIZE-4;
733                        btAlignedObjectArray<sStkNN>    stkStack;
734                        stkStack.resize(DOUBLE_STACKSIZE);
735                        stkStack[0]=sStkNN(root0,root1);
736                        do      {               
737                                sStkNN  p=stkStack[--depth];
738                                if(depth>treshold)
739                                {
740                                        stkStack.resize(stkStack.size()*2);
741                                        treshold=stkStack.size()-4;
742                                }
743                                if(p.a==p.b)
744                                {
745                                        if(p.a->isinternal())
746                                        {
747                                                stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]);
748                                                stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]);
749                                                stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]);
750                                        }
751                                }
752                                else if(Intersect(p.a->volume,p.b->volume))
753                                {
754                                        if(p.a->isinternal())
755                                        {
756                                                if(p.b->isinternal())
757                                                {
758                                                        stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
759                                                        stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
760                                                        stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
761                                                        stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
762                                                }
763                                                else
764                                                {
765                                                        stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
766                                                        stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
767                                                }
768                                        }
769                                        else
770                                        {
771                                                if(p.b->isinternal())
772                                                {
773                                                        stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
774                                                        stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
775                                                }
776                                                else
777                                                {
778                                                        policy.Process(p.a,p.b);
779                                                }
780                                        }
781                                }
782                        } while(depth);
783                }
784}
785
786
787
788DBVT_PREFIX
789inline void             btDbvt::collideTTpersistentStack(       const btDbvtNode* root0,
790                                                                  const btDbvtNode* root1,
791                                                                  DBVT_IPOLICY)
792{
793        DBVT_CHECKTYPE
794                if(root0&&root1)
795                {
796                        int                                                             depth=1;
797                        int                                                             treshold=DOUBLE_STACKSIZE-4;
798                       
799                        m_stkStack.resize(DOUBLE_STACKSIZE);
800                        m_stkStack[0]=sStkNN(root0,root1);
801                        do      {               
802                                sStkNN  p=m_stkStack[--depth];
803                                if(depth>treshold)
804                                {
805                                        m_stkStack.resize(m_stkStack.size()*2);
806                                        treshold=m_stkStack.size()-4;
807                                }
808                                if(p.a==p.b)
809                                {
810                                        if(p.a->isinternal())
811                                        {
812                                                m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]);
813                                                m_stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]);
814                                                m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]);
815                                        }
816                                }
817                                else if(Intersect(p.a->volume,p.b->volume))
818                                {
819                                        if(p.a->isinternal())
820                                        {
821                                                if(p.b->isinternal())
822                                                {
823                                                        m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
824                                                        m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
825                                                        m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
826                                                        m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
827                                                }
828                                                else
829                                                {
830                                                        m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
831                                                        m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
832                                                }
833                                        }
834                                        else
835                                        {
836                                                if(p.b->isinternal())
837                                                {
838                                                        m_stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
839                                                        m_stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
840                                                }
841                                                else
842                                                {
843                                                        policy.Process(p.a,p.b);
844                                                }
845                                        }
846                                }
847                        } while(depth);
848                }
849}
850
851
852//
853DBVT_PREFIX
854inline void             btDbvt::collideTT(      const btDbvtNode* root0,
855                                                                  const btDbvtNode* root1,
856                                                                  const btTransform& xform,
857                                                                  DBVT_IPOLICY)
858{
859        DBVT_CHECKTYPE
860                if(root0&&root1)
861                {
862                        int                                                             depth=1;
863                        int                                                             treshold=DOUBLE_STACKSIZE-4;
864                        btAlignedObjectArray<sStkNN>    stkStack;
865                        stkStack.resize(DOUBLE_STACKSIZE);
866                        stkStack[0]=sStkNN(root0,root1);
867                        do      {
868                                sStkNN  p=stkStack[--depth];
869                                if(Intersect(p.a->volume,p.b->volume,xform))
870                                {
871                                        if(depth>treshold)
872                                        {
873                                                stkStack.resize(stkStack.size()*2);
874                                                treshold=stkStack.size()-4;
875                                        }
876                                        if(p.a->isinternal())
877                                        {
878                                                if(p.b->isinternal())
879                                                {                                       
880                                                        stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
881                                                        stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
882                                                        stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
883                                                        stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
884                                                }
885                                                else
886                                                {
887                                                        stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
888                                                        stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
889                                                }
890                                        }
891                                        else
892                                        {
893                                                if(p.b->isinternal())
894                                                {
895                                                        stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
896                                                        stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
897                                                }
898                                                else
899                                                {
900                                                        policy.Process(p.a,p.b);
901                                                }
902                                        }
903                                }
904                        } while(depth);
905                }
906}
907
908//
909DBVT_PREFIX
910inline void             btDbvt::collideTT(      const btDbvtNode* root0,
911                                                                  const btTransform& xform0,
912                                                                  const btDbvtNode* root1,
913                                                                  const btTransform& xform1,
914                                                                  DBVT_IPOLICY)
915{
916        const btTransform       xform=xform0.inverse()*xform1;
917        collideTT(root0,root1,xform,policy);
918}
919
920//
921DBVT_PREFIX
922inline void             btDbvt::collideTV(      const btDbvtNode* root,
923                                                                  const btDbvtVolume& vol,
924                                                                  DBVT_IPOLICY)
925{
926        DBVT_CHECKTYPE
927                if(root)
928                {
929                        ATTRIBUTE_ALIGNED16(btDbvtVolume)               volume(vol);
930                        btAlignedObjectArray<const btDbvtNode*> stack;
931                        stack.resize(0);
932                        stack.reserve(SIMPLE_STACKSIZE);
933                        stack.push_back(root);
934                        do      {
935                                const btDbvtNode*       n=stack[stack.size()-1];
936                                stack.pop_back();
937                                if(Intersect(n->volume,volume))
938                                {
939                                        if(n->isinternal())
940                                        {
941                                                stack.push_back(n->childs[0]);
942                                                stack.push_back(n->childs[1]);
943                                        }
944                                        else
945                                        {
946                                                policy.Process(n);
947                                        }
948                                }
949                        } while(stack.size()>0);
950                }
951}
952
953DBVT_PREFIX
954inline void             btDbvt::rayTestInternal(        const btDbvtNode* root,
955                                                                const btVector3& rayFrom,
956                                                                const btVector3& rayTo,
957                                                                const btVector3& rayDirectionInverse,
958                                                                unsigned int signs[3],
959                                                                btScalar lambda_max,
960                                                                const btVector3& aabbMin,
961                                                                const btVector3& aabbMax,
962                                                                DBVT_IPOLICY) const
963{
964        DBVT_CHECKTYPE
965        if(root)
966        {
967                btVector3 resultNormal;
968
969                int                                                             depth=1;
970                int                                                             treshold=DOUBLE_STACKSIZE-2;
971                btAlignedObjectArray<const btDbvtNode*> stack;
972                stack.resize(DOUBLE_STACKSIZE);
973                stack[0]=root;
974                btVector3 bounds[2];
975                do     
976                {
977                        const btDbvtNode*       node=stack[--depth];
978                        bounds[0] = node->volume.Mins()+aabbMin;
979                        bounds[1] = node->volume.Maxs()+aabbMax;
980                        btScalar tmin=1.f,lambda_min=0.f;
981                        unsigned int result1=false;
982                        result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
983                        if(result1)
984                        {
985                                if(node->isinternal())
986                                {
987                                        if(depth>treshold)
988                                        {
989                                                stack.resize(stack.size()*2);
990                                                treshold=stack.size()-2;
991                                        }
992                                        stack[depth++]=node->childs[0];
993                                        stack[depth++]=node->childs[1];
994                                }
995                                else
996                                {
997                                        policy.Process(node);
998                                }
999                        }
1000                } while(depth);
1001        }
1002}
1003
1004//
1005DBVT_PREFIX
1006inline void             btDbvt::rayTest(        const btDbvtNode* root,
1007                                                                const btVector3& rayFrom,
1008                                                                const btVector3& rayTo,
1009                                                                DBVT_IPOLICY)
1010{
1011        DBVT_CHECKTYPE
1012                if(root)
1013                {
1014                        btVector3 rayDir = (rayTo-rayFrom);
1015                        rayDir.normalize ();
1016
1017                        ///what about division by zero? --> just set rayDirection[i] to INF/1e30
1018                        btVector3 rayDirectionInverse;
1019                        rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
1020                        rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
1021                        rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
1022                        unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0};
1023
1024                        btScalar lambda_max = rayDir.dot(rayTo-rayFrom);
1025
1026                        btVector3 resultNormal;
1027
1028                        btAlignedObjectArray<const btDbvtNode*> stack;
1029
1030                        int                                                             depth=1;
1031                        int                                                             treshold=DOUBLE_STACKSIZE-2;
1032
1033                        stack.resize(DOUBLE_STACKSIZE);
1034                        stack[0]=root;
1035                        btVector3 bounds[2];
1036                        do      {
1037                                const btDbvtNode*       node=stack[--depth];
1038
1039                                bounds[0] = node->volume.Mins();
1040                                bounds[1] = node->volume.Maxs();
1041                               
1042                                btScalar tmin=1.f,lambda_min=0.f;
1043                                unsigned int result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
1044
1045#ifdef COMPARE_BTRAY_AABB2
1046                                btScalar param=1.f;
1047                                bool result2 = btRayAabb(rayFrom,rayTo,node->volume.Mins(),node->volume.Maxs(),param,resultNormal);
1048                                btAssert(result1 == result2);
1049#endif //TEST_BTRAY_AABB2
1050
1051                                if(result1)
1052                                {
1053                                        if(node->isinternal())
1054                                        {
1055                                                if(depth>treshold)
1056                                                {
1057                                                        stack.resize(stack.size()*2);
1058                                                        treshold=stack.size()-2;
1059                                                }
1060                                                stack[depth++]=node->childs[0];
1061                                                stack[depth++]=node->childs[1];
1062                                        }
1063                                        else
1064                                        {
1065                                                policy.Process(node);
1066                                        }
1067                                }
1068                        } while(depth);
1069
1070                }
1071}
1072
1073//
1074DBVT_PREFIX
1075inline void             btDbvt::collideKDOP(const btDbvtNode* root,
1076                                                                        const btVector3* normals,
1077                                                                        const btScalar* offsets,
1078                                                                        int count,
1079                                                                        DBVT_IPOLICY)
1080{
1081        DBVT_CHECKTYPE
1082                if(root)
1083                {
1084                        const int                                               inside=(1<<count)-1;
1085                        btAlignedObjectArray<sStkNP>    stack;
1086                        int                                                             signs[sizeof(unsigned)*8];
1087                        btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
1088                        for(int i=0;i<count;++i)
1089                        {
1090                                signs[i]=       ((normals[i].x()>=0)?1:0)+
1091                                        ((normals[i].y()>=0)?2:0)+
1092                                        ((normals[i].z()>=0)?4:0);
1093                        }
1094                        stack.reserve(SIMPLE_STACKSIZE);
1095                        stack.push_back(sStkNP(root,0));
1096                        do      {
1097                                sStkNP  se=stack[stack.size()-1];
1098                                bool    out=false;
1099                                stack.pop_back();
1100                                for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
1101                                {
1102                                        if(0==(se.mask&j))
1103                                        {
1104                                                const int       side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
1105                                                switch(side)
1106                                                {
1107                                                case    -1:     out=true;break;
1108                                                case    +1:     se.mask|=j;break;
1109                                                }
1110                                        }
1111                                }
1112                                if(!out)
1113                                {
1114                                        if((se.mask!=inside)&&(se.node->isinternal()))
1115                                        {
1116                                                stack.push_back(sStkNP(se.node->childs[0],se.mask));
1117                                                stack.push_back(sStkNP(se.node->childs[1],se.mask));
1118                                        }
1119                                        else
1120                                        {
1121                                                if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy);
1122                                        }
1123                                }
1124                        } while(stack.size());
1125                }
1126}
1127
1128//
1129DBVT_PREFIX
1130inline void             btDbvt::collideOCL(     const btDbvtNode* root,
1131                                                                   const btVector3* normals,
1132                                                                   const btScalar* offsets,
1133                                                                   const btVector3& sortaxis,
1134                                                                   int count,
1135                                                                   DBVT_IPOLICY,
1136                                                                   bool fsort)
1137{
1138        DBVT_CHECKTYPE
1139                if(root)
1140                {
1141                        const unsigned                                  srtsgns=(sortaxis[0]>=0?1:0)+
1142                                (sortaxis[1]>=0?2:0)+
1143                                (sortaxis[2]>=0?4:0);
1144                        const int                                               inside=(1<<count)-1;
1145                        btAlignedObjectArray<sStkNPS>   stock;
1146                        btAlignedObjectArray<int>               ifree;
1147                        btAlignedObjectArray<int>               stack;
1148                        int                                                             signs[sizeof(unsigned)*8];
1149                        btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
1150                        for(int i=0;i<count;++i)
1151                        {
1152                                signs[i]=       ((normals[i].x()>=0)?1:0)+
1153                                        ((normals[i].y()>=0)?2:0)+
1154                                        ((normals[i].z()>=0)?4:0);
1155                        }
1156                        stock.reserve(SIMPLE_STACKSIZE);
1157                        stack.reserve(SIMPLE_STACKSIZE);
1158                        ifree.reserve(SIMPLE_STACKSIZE);
1159                        stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns))));
1160                        do      {
1161                                const int       id=stack[stack.size()-1];
1162                                sStkNPS         se=stock[id];
1163                                stack.pop_back();ifree.push_back(id);
1164                                if(se.mask!=inside)
1165                                {
1166                                        bool    out=false;
1167                                        for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
1168                                        {
1169                                                if(0==(se.mask&j))
1170                                                {
1171                                                        const int       side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
1172                                                        switch(side)
1173                                                        {
1174                                                        case    -1:     out=true;break;
1175                                                        case    +1:     se.mask|=j;break;
1176                                                        }
1177                                                }
1178                                        }
1179                                        if(out) continue;
1180                                }
1181                                if(policy.Descent(se.node))
1182                                {
1183                                        if(se.node->isinternal())
1184                                        {
1185                                                const btDbvtNode* pns[]={       se.node->childs[0],se.node->childs[1]};
1186                                                sStkNPS         nes[]={ sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)),
1187                                                        sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))};
1188                                                const int       q=nes[0].value<nes[1].value?1:0;                               
1189                                                int                     j=stack.size();
1190                                                if(fsort&&(j>0))
1191                                                {
1192                                                        /* Insert 0     */ 
1193                                                        j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size());
1194                                                        stack.push_back(0);
1195#if DBVT_USE_MEMMOVE
1196                                                        memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
1197#else
1198                                                        for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
1199#endif
1200                                                        stack[j]=allocate(ifree,stock,nes[q]);
1201                                                        /* Insert 1     */ 
1202                                                        j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size());
1203                                                        stack.push_back(0);
1204#if DBVT_USE_MEMMOVE
1205                                                        memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
1206#else
1207                                                        for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
1208#endif
1209                                                        stack[j]=allocate(ifree,stock,nes[1-q]);
1210                                                }
1211                                                else
1212                                                {
1213                                                        stack.push_back(allocate(ifree,stock,nes[q]));
1214                                                        stack.push_back(allocate(ifree,stock,nes[1-q]));
1215                                                }
1216                                        }
1217                                        else
1218                                        {
1219                                                policy.Process(se.node,se.value);
1220                                        }
1221                                }
1222                        } while(stack.size());
1223                }
1224}
1225
1226//
1227DBVT_PREFIX
1228inline void             btDbvt::collideTU(      const btDbvtNode* root,
1229                                                                  DBVT_IPOLICY)
1230{
1231        DBVT_CHECKTYPE
1232                if(root)
1233                {
1234                        btAlignedObjectArray<const btDbvtNode*> stack;
1235                        stack.reserve(SIMPLE_STACKSIZE);
1236                        stack.push_back(root);
1237                        do      {
1238                                const btDbvtNode*       n=stack[stack.size()-1];
1239                                stack.pop_back();
1240                                if(policy.Descent(n))
1241                                {
1242                                        if(n->isinternal())
1243                                        { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); }
1244                                        else
1245                                        { policy.Process(n); }
1246                                }
1247                        } while(stack.size()>0);
1248                }
1249}
1250
1251//
1252// PP Cleanup
1253//
1254
1255#undef DBVT_USE_MEMMOVE
1256#undef DBVT_USE_TEMPLATE
1257#undef DBVT_VIRTUAL_DTOR
1258#undef DBVT_VIRTUAL
1259#undef DBVT_PREFIX
1260#undef DBVT_IPOLICY
1261#undef DBVT_CHECKTYPE
1262#undef DBVT_IMPL_GENERIC
1263#undef DBVT_IMPL_SSE
1264#undef DBVT_USE_INTRINSIC_SSE
1265#undef DBVT_SELECT_IMPL
1266#undef DBVT_MERGE_IMPL
1267#undef DBVT_INT0_IMPL
1268
1269#endif
Note: See TracBrowser for help on using the repository browser.