Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/external/bullet/BulletCollision/BroadphaseCollision/btDbvt.h @ 8324

Last change on this file since 8324 was 5781, checked in by rgrieder, 15 years ago

Reverted trunk again. We might want to find a way to delete these revisions again (x3n's changes are still available as diff in the commit mails).

  • Property svn:eol-style set to native
File size: 31.1 KB
RevLine 
[1963]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"
[2430]23#include "LinearMath/btAabbUtil2.h"
[1963]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
[2430]36#if (defined (_MSC_VER) && _MSC_VER >= 1400)
37#define DBVT_USE_TEMPLATE               1
38#else
39#define DBVT_USE_TEMPLATE               0
[1963]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
[2882]60#ifdef BT_USE_SSE
[1963]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
[2882]85#define DBVT_CHECKTYPE                          static const ICollide&  typechecker=*(T*)1;(void)typechecker;
[1963]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{
[2430]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);
[2882]149       
[2430]150        DBVT_INLINE friend bool                 Intersect(      const btDbvtAabbMm& a,
151                const btVector3& b);
152
153        DBVT_INLINE friend btScalar             Proximity(      const btDbvtAabbMm& a,
154                const btDbvtAabbMm& b);
155        DBVT_INLINE friend int                  Select(         const btDbvtAabbMm& o,
156                const btDbvtAabbMm& a,
157                const btDbvtAabbMm& b);
158        DBVT_INLINE friend void                 Merge(          const btDbvtAabbMm& a,
159                const btDbvtAabbMm& b,
160                btDbvtAabbMm& r);
161        DBVT_INLINE friend bool                 NotEqual(       const btDbvtAabbMm& a,
162                const btDbvtAabbMm& b);
[1963]163private:
[2430]164        DBVT_INLINE void                                AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const;
[1963]165private:
[2430]166        btVector3       mi,mx;
[1963]167};
168
169// Types       
170typedef btDbvtAabbMm    btDbvtVolume;
171
172/* btDbvtNode                           */ 
173struct  btDbvtNode
174{
175        btDbvtVolume    volume;
176        btDbvtNode*             parent;
177        DBVT_INLINE bool        isleaf() const          { return(childs[1]==0); }
178        DBVT_INLINE bool        isinternal() const      { return(!isleaf()); }
[2430]179        union
180        {
181                btDbvtNode*     childs[2];
182                void*   data;
183                int             dataAsInt;
184        };
[1963]185};
186
187///The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree).
188///This btDbvt is used for soft body collision detection and for the btDbvtBroadphase. It has a fast insert, remove and update of nodes.
189///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure.
190struct  btDbvt
[2430]191{
[1963]192        /* Stack element        */ 
193        struct  sStkNN
[2430]194        {
[1963]195                const btDbvtNode*       a;
196                const btDbvtNode*       b;
197                sStkNN() {}
198                sStkNN(const btDbvtNode* na,const btDbvtNode* nb) : a(na),b(nb) {}
[2430]199        };
[1963]200        struct  sStkNP
[2430]201        {
[1963]202                const btDbvtNode*       node;
203                int                     mask;
204                sStkNP(const btDbvtNode* n,unsigned m) : node(n),mask(m) {}
[2430]205        };
[1963]206        struct  sStkNPS
[2430]207        {
[1963]208                const btDbvtNode*       node;
209                int                     mask;
210                btScalar        value;
211                sStkNPS() {}
212                sStkNPS(const btDbvtNode* n,unsigned m,btScalar v) : node(n),mask(m),value(v) {}
[2430]213        };
[1963]214        struct  sStkCLN
[2430]215        {
[1963]216                const btDbvtNode*       node;
217                btDbvtNode*             parent;
218                sStkCLN(const btDbvtNode* n,btDbvtNode* p) : node(n),parent(p) {}
[2430]219        };
[1963]220        // Policies/Interfaces
[2430]221
[1963]222        /* ICollide     */ 
223        struct  ICollide
[2430]224        {               
[1963]225                DBVT_VIRTUAL_DTOR(ICollide)
[2430]226                        DBVT_VIRTUAL void       Process(const btDbvtNode*,const btDbvtNode*)            {}
[1963]227                DBVT_VIRTUAL void       Process(const btDbvtNode*)                                      {}
228                DBVT_VIRTUAL void       Process(const btDbvtNode* n,btScalar)                   { Process(n); }
229                DBVT_VIRTUAL bool       Descent(const btDbvtNode*)                                      { return(true); }
230                DBVT_VIRTUAL bool       AllLeaves(const btDbvtNode*)                                    { return(true); }
[2430]231        };
[1963]232        /* IWriter      */ 
233        struct  IWriter
[2430]234        {
[1963]235                virtual ~IWriter() {}
236                virtual void            Prepare(const btDbvtNode* root,int numnodes)=0;
237                virtual void            WriteNode(const btDbvtNode*,int index,int parent,int child0,int child1)=0;
238                virtual void            WriteLeaf(const btDbvtNode*,int index,int parent)=0;
[2430]239        };
[1963]240        /* IClone       */ 
241        struct  IClone
[2430]242        {
[1963]243                virtual ~IClone()       {}
244                virtual void            CloneLeaf(btDbvtNode*) {}
[2430]245        };
246
[1963]247        // Constants
248        enum    {
[2430]249                SIMPLE_STACKSIZE        =       64,
250                DOUBLE_STACKSIZE        =       SIMPLE_STACKSIZE*2
251        };
252
[1963]253        // Fields
254        btDbvtNode*             m_root;
255        btDbvtNode*             m_free;
256        int                             m_lkhd;
257        int                             m_leaves;
258        unsigned                m_opath;
[2430]259
260       
261        btAlignedObjectArray<sStkNN>    m_stkStack;
262
263
[1963]264        // Methods
[2430]265        btDbvt();
266        ~btDbvt();
[1963]267        void                    clear();
268        bool                    empty() const { return(0==m_root); }
269        void                    optimizeBottomUp();
270        void                    optimizeTopDown(int bu_treshold=128);
271        void                    optimizeIncremental(int passes);
272        btDbvtNode*             insert(const btDbvtVolume& box,void* data);
273        void                    update(btDbvtNode* leaf,int lookahead=-1);
[2430]274        void                    update(btDbvtNode* leaf,btDbvtVolume& volume);
275        bool                    update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity,btScalar margin);
276        bool                    update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity);
277        bool                    update(btDbvtNode* leaf,btDbvtVolume& volume,btScalar margin); 
[1963]278        void                    remove(btDbvtNode* leaf);
279        void                    write(IWriter* iwriter) const;
280        void                    clone(btDbvt& dest,IClone* iclone=0) const;
281        static int              maxdepth(const btDbvtNode* node);
282        static int              countLeaves(const btDbvtNode* node);
283        static void             extractLeaves(const btDbvtNode* node,btAlignedObjectArray<const btDbvtNode*>& leaves);
[2430]284#if DBVT_ENABLE_BENCHMARK
[1963]285        static void             benchmark();
[2430]286#else
[1963]287        static void             benchmark(){}
[2430]288#endif
[1963]289        // DBVT_IPOLICY must support ICollide policy/interface
290        DBVT_PREFIX
[2430]291                static void             enumNodes(      const btDbvtNode* root,
292                DBVT_IPOLICY);
[1963]293        DBVT_PREFIX
[2430]294                static void             enumLeaves(     const btDbvtNode* root,
295                DBVT_IPOLICY);
[1963]296        DBVT_PREFIX
[2430]297                void            collideTT(      const btDbvtNode* root0,
298                const btDbvtNode* root1,
299                DBVT_IPOLICY);
300
[1963]301        DBVT_PREFIX
[2430]302                void            collideTTpersistentStack(       const btDbvtNode* root0,
303                  const btDbvtNode* root1,
304                  DBVT_IPOLICY);
[2882]305#if 0
[1963]306        DBVT_PREFIX
[2430]307                void            collideTT(      const btDbvtNode* root0,
308                const btDbvtNode* root1,
309                const btTransform& xform,
310                DBVT_IPOLICY);
[1963]311        DBVT_PREFIX
[2430]312                void            collideTT(      const btDbvtNode* root0,
313                const btTransform& xform0,
314                const btDbvtNode* root1,
315                const btTransform& xform1,
316                DBVT_IPOLICY);
[2882]317#endif
318
[1963]319        DBVT_PREFIX
[2430]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
[1963]325        DBVT_PREFIX
[2430]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
[1963]332        DBVT_PREFIX
[2430]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
[1963]343        DBVT_PREFIX
[2430]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);
[1963]360        // Helpers     
361        static DBVT_INLINE int  nearest(const int* i,const btDbvt::sStkNPS* a,btScalar v,int l,int h)
[2430]362        {
[1963]363                int     m=0;
364                while(l<h)
[2430]365                {
[1963]366                        m=(l+h)>>1;
367                        if(a[i[m]].value>=v) l=m+1; else h=m;
[2430]368                }
[1972]369                return(h);
[2430]370        }
[1963]371        static DBVT_INLINE int  allocate(       btAlignedObjectArray<int>& ifree,
[2430]372                btAlignedObjectArray<sStkNPS>& stock,
373                const sStkNPS& value)
374        {
[1963]375                int     i;
376                if(ifree.size()>0)
[2430]377                { i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; }
378                else
379                { i=stock.size();stock.push_back(value); }
[1963]380                return(i); 
[2430]381        }
[1963]382        //
[2430]383private:
384        btDbvt(const btDbvt&)   {}     
385};
[1963]386
387//
388// Inline's
389//
390
391//
392inline btDbvtAabbMm                     btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e)
393{
[2430]394        btDbvtAabbMm box;
395        box.mi=c-e;box.mx=c+e;
396        return(box);
[1963]397}
[2430]398
[1963]399//
400inline btDbvtAabbMm                     btDbvtAabbMm::FromCR(const btVector3& c,btScalar r)
401{
[2430]402        return(FromCE(c,btVector3(r,r,r)));
[1963]403}
[2430]404
[1963]405//
406inline btDbvtAabbMm                     btDbvtAabbMm::FromMM(const btVector3& mi,const btVector3& mx)
407{
[2430]408        btDbvtAabbMm box;
409        box.mi=mi;box.mx=mx;
410        return(box);
[1963]411}
[2430]412
[1963]413//
414inline btDbvtAabbMm                     btDbvtAabbMm::FromPoints(const btVector3* pts,int n)
415{
[2430]416        btDbvtAabbMm box;
417        box.mi=box.mx=pts[0];
418        for(int i=1;i<n;++i)
[1963]419        {
[2430]420                box.mi.setMin(pts[i]);
421                box.mx.setMax(pts[i]);
[1963]422        }
[2430]423        return(box);
[1963]424}
425
426//
427inline btDbvtAabbMm                     btDbvtAabbMm::FromPoints(const btVector3** ppts,int n)
428{
[2430]429        btDbvtAabbMm box;
430        box.mi=box.mx=*ppts[0];
431        for(int i=1;i<n;++i)
[1963]432        {
[2430]433                box.mi.setMin(*ppts[i]);
434                box.mx.setMax(*ppts[i]);
[1963]435        }
[2430]436        return(box);
[1963]437}
438
439//
440DBVT_INLINE void                btDbvtAabbMm::Expand(const btVector3& e)
441{
[2430]442        mi-=e;mx+=e;
[1963]443}
[2430]444
[1963]445//
446DBVT_INLINE void                btDbvtAabbMm::SignedExpand(const btVector3& e)
447{
[2430]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]);
[1963]451}
[2430]452
[1963]453//
454DBVT_INLINE bool                btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const
455{
[2430]456        return( (mi.x()<=a.mi.x())&&
[1963]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{
[2430]467        btVector3                       pi,px;
468        switch(s)
[1963]469        {
470        case    (0+0+0):        px=btVector3(mi.x(),mi.y(),mi.z());
[2430]471                pi=btVector3(mx.x(),mx.y(),mx.z());break;
[1963]472        case    (1+0+0):        px=btVector3(mx.x(),mi.y(),mi.z());
[2430]473                pi=btVector3(mi.x(),mx.y(),mx.z());break;
[1963]474        case    (0+2+0):        px=btVector3(mi.x(),mx.y(),mi.z());
[2430]475                pi=btVector3(mx.x(),mi.y(),mx.z());break;
[1963]476        case    (1+2+0):        px=btVector3(mx.x(),mx.y(),mi.z());
[2430]477                pi=btVector3(mi.x(),mi.y(),mx.z());break;
[1963]478        case    (0+0+4):        px=btVector3(mi.x(),mi.y(),mx.z());
[2430]479                pi=btVector3(mx.x(),mx.y(),mi.z());break;
[1963]480        case    (1+0+4):        px=btVector3(mx.x(),mi.y(),mx.z());
[2430]481                pi=btVector3(mi.x(),mx.y(),mi.z());break;
[1963]482        case    (0+2+4):        px=btVector3(mi.x(),mx.y(),mx.z());
[2430]483                pi=btVector3(mx.x(),mi.y(),mi.z());break;
[1963]484        case    (1+2+4):        px=btVector3(mx.x(),mx.y(),mx.z());
[2430]485                pi=btVector3(mi.x(),mi.y(),mi.z());break;
[1963]486        }
[2430]487        if((dot(n,px)+o)<0)             return(-1);
488        if((dot(n,pi)+o)>=0)    return(+1);
489        return(0);
[1963]490}
491
492//
493DBVT_INLINE btScalar    btDbvtAabbMm::ProjectMinimum(const btVector3& v,unsigned signs) const
494{
[2430]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));
[1963]500}
501
502//
503DBVT_INLINE void                btDbvtAabbMm::AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const
504{
[2430]505        for(int i=0;i<3;++i)
[1963]506        {
[2430]507                if(d[i]<0)
[1963]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}
[2430]513
[1963]514//
515DBVT_INLINE bool                Intersect(      const btDbvtAabbMm& a,
[2430]516                                                                  const btDbvtAabbMm& b)
[1963]517{
518#if     DBVT_INT0_IMPL == DBVT_IMPL_SSE
[2430]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);
[1963]523#else
[2430]524        return( (a.mi.x()<=b.mx.x())&&
[1963]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
[2882]534
[1963]535//
536DBVT_INLINE bool                Intersect(      const btDbvtAabbMm& a,
[2430]537                                                                  const btVector3& b)
[1963]538{
[2430]539        return( (b.x()>=a.mi.x())&&
[1963]540                (b.y()>=a.mi.y())&&
541                (b.z()>=a.mi.z())&&
542                (b.x()<=a.mx.x())&&
543                (b.y()<=a.mx.y())&&
544                (b.z()<=a.mx.z()));
545}
546
[2430]547
548
549
550
551//////////////////////////////////////
552
553
[1963]554//
555DBVT_INLINE btScalar    Proximity(      const btDbvtAabbMm& a,
[2430]556                                                                  const btDbvtAabbMm& b)
[1963]557{
[2430]558        const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx);
559        return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z()));
[1963]560}
561
[2430]562
563
[1963]564//
565DBVT_INLINE int                 Select( const btDbvtAabbMm& o,
[2430]566                                                           const btDbvtAabbMm& a,
567                                                           const btDbvtAabbMm& b)
[1963]568{
569#if     DBVT_SELECT_IMPL == DBVT_IMPL_SSE
[2430]570        static ATTRIBUTE_ALIGNED16(const unsigned __int32)      mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff};
571        ///@todo: the intrinsic version is 11% slower
572#if DBVT_USE_INTRINSIC_SSE
573
574        union btSSEUnion ///NOTE: if we use more intrinsics, move btSSEUnion into the LinearMath directory
575        {
576           __m128               ssereg;
577           float                floats[4];
578           int                  ints[4];
579        };
580
[1963]581        __m128  omi(_mm_load_ps(o.mi));
582        omi=_mm_add_ps(omi,_mm_load_ps(o.mx));
583        __m128  ami(_mm_load_ps(a.mi));
584        ami=_mm_add_ps(ami,_mm_load_ps(a.mx));
585        ami=_mm_sub_ps(ami,omi);
586        ami=_mm_and_ps(ami,_mm_load_ps((const float*)mask));
587        __m128  bmi(_mm_load_ps(b.mi));
588        bmi=_mm_add_ps(bmi,_mm_load_ps(b.mx));
589        bmi=_mm_sub_ps(bmi,omi);
590        bmi=_mm_and_ps(bmi,_mm_load_ps((const float*)mask));
591        __m128  t0(_mm_movehl_ps(ami,ami));
592        ami=_mm_add_ps(ami,t0);
593        ami=_mm_add_ss(ami,_mm_shuffle_ps(ami,ami,1));
[2430]594        __m128 t1(_mm_movehl_ps(bmi,bmi));
[1963]595        bmi=_mm_add_ps(bmi,t1);
596        bmi=_mm_add_ss(bmi,_mm_shuffle_ps(bmi,bmi,1));
[2430]597       
598        btSSEUnion tmp;
599        tmp.ssereg = _mm_cmple_ss(bmi,ami);
600        return tmp.ints[0]&1;
601
602#else
603        ATTRIBUTE_ALIGNED16(__int32     r[1]);
[1963]604        __asm
[2430]605        {
[1963]606                mov             eax,o
[2430]607                        mov             ecx,a
608                        mov             edx,b
609                        movaps  xmm0,[eax]
[1963]610                movaps  xmm5,mask
[2430]611                        addps   xmm0,[eax+16]   
[1963]612                movaps  xmm1,[ecx]
613                movaps  xmm2,[edx]
614                addps   xmm1,[ecx+16]
615                addps   xmm2,[edx+16]
616                subps   xmm1,xmm0
[2430]617                        subps   xmm2,xmm0
618                        andps   xmm1,xmm5
619                        andps   xmm2,xmm5
620                        movhlps xmm3,xmm1
621                        movhlps xmm4,xmm2
622                        addps   xmm1,xmm3
623                        addps   xmm2,xmm4
624                        pshufd  xmm3,xmm1,1
625                        pshufd  xmm4,xmm2,1
626                        addss   xmm1,xmm3
627                        addss   xmm2,xmm4
628                        cmpless xmm2,xmm1
629                        movss   r,xmm2
630        }
[1963]631        return(r[0]&1);
[2430]632#endif
[1963]633#else
[2430]634        return(Proximity(o,a)<Proximity(o,b)?0:1);
[1963]635#endif
636}
637
638//
639DBVT_INLINE void                Merge(  const btDbvtAabbMm& a,
[2430]640                                                          const btDbvtAabbMm& b,
641                                                          btDbvtAabbMm& r)
[1963]642{
643#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
[2430]644        __m128  ami(_mm_load_ps(a.mi));
645        __m128  amx(_mm_load_ps(a.mx));
646        __m128  bmi(_mm_load_ps(b.mi));
647        __m128  bmx(_mm_load_ps(b.mx));
648        ami=_mm_min_ps(ami,bmi);
649        amx=_mm_max_ps(amx,bmx);
650        _mm_store_ps(r.mi,ami);
651        _mm_store_ps(r.mx,amx);
[1963]652#else
[2430]653        for(int i=0;i<3;++i)
[1963]654        {
[2430]655                if(a.mi[i]<b.mi[i]) r.mi[i]=a.mi[i]; else r.mi[i]=b.mi[i];
656                if(a.mx[i]>b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i];
[1963]657        }
658#endif
659}
660
661//
662DBVT_INLINE bool                NotEqual(       const btDbvtAabbMm& a,
[2430]663                                                                 const btDbvtAabbMm& b)
[1963]664{
[2430]665        return( (a.mi.x()!=b.mi.x())||
[1963]666                (a.mi.y()!=b.mi.y())||
667                (a.mi.z()!=b.mi.z())||
668                (a.mx.x()!=b.mx.x())||
669                (a.mx.y()!=b.mx.y())||
670                (a.mx.z()!=b.mx.z()));
671}
672
673//
674// Inline's
675//
676
677//
678DBVT_PREFIX
679inline void             btDbvt::enumNodes(      const btDbvtNode* root,
[2430]680                                                                  DBVT_IPOLICY)
[1963]681{
[2430]682        DBVT_CHECKTYPE
683                policy.Process(root);
684        if(root->isinternal())
[1963]685        {
[2430]686                enumNodes(root->childs[0],policy);
687                enumNodes(root->childs[1],policy);
[1963]688        }
689}
690
691//
692DBVT_PREFIX
693inline void             btDbvt::enumLeaves(     const btDbvtNode* root,
[2430]694                                                                   DBVT_IPOLICY)
[1963]695{
[2430]696        DBVT_CHECKTYPE
697                if(root->isinternal())
698                {
699                        enumLeaves(root->childs[0],policy);
700                        enumLeaves(root->childs[1],policy);
701                }
702                else
703                {
704                        policy.Process(root);
705                }
[1963]706}
707
708//
709DBVT_PREFIX
710inline void             btDbvt::collideTT(      const btDbvtNode* root0,
[2430]711                                                                  const btDbvtNode* root1,
712                                                                  DBVT_IPOLICY)
[1963]713{
[2430]714        DBVT_CHECKTYPE
715                if(root0&&root1)
716                {
717                        int                                                             depth=1;
718                        int                                                             treshold=DOUBLE_STACKSIZE-4;
719                        btAlignedObjectArray<sStkNN>    stkStack;
720                        stkStack.resize(DOUBLE_STACKSIZE);
721                        stkStack[0]=sStkNN(root0,root1);
722                        do      {               
723                                sStkNN  p=stkStack[--depth];
724                                if(depth>treshold)
[1963]725                                {
[2430]726                                        stkStack.resize(stkStack.size()*2);
727                                        treshold=stkStack.size()-4;
[1963]728                                }
[2430]729                                if(p.a==p.b)
[1963]730                                {
[2430]731                                        if(p.a->isinternal())
[1963]732                                        {
[2430]733                                                stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]);
734                                                stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]);
735                                                stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]);
[1963]736                                        }
737                                }
[2430]738                                else if(Intersect(p.a->volume,p.b->volume))
[1963]739                                {
[2430]740                                        if(p.a->isinternal())
[1963]741                                        {
[2430]742                                                if(p.b->isinternal())
743                                                {
744                                                        stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
745                                                        stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
746                                                        stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
747                                                        stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
748                                                }
749                                                else
750                                                {
751                                                        stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
752                                                        stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
753                                                }
[1963]754                                        }
755                                        else
756                                        {
[2430]757                                                if(p.b->isinternal())
758                                                {
759                                                        stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
760                                                        stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
761                                                }
762                                                else
763                                                {
764                                                        policy.Process(p.a,p.b);
765                                                }
[1963]766                                        }
767                                }
[2430]768                        } while(depth);
769                }
[1963]770}
771
[2430]772
773
[1963]774DBVT_PREFIX
[2430]775inline void             btDbvt::collideTTpersistentStack(       const btDbvtNode* root0,
776                                                                  const btDbvtNode* root1,
777                                                                  DBVT_IPOLICY)
[1963]778{
[2430]779        DBVT_CHECKTYPE
780                if(root0&&root1)
781                {
782                        int                                                             depth=1;
783                        int                                                             treshold=DOUBLE_STACKSIZE-4;
784                       
785                        m_stkStack.resize(DOUBLE_STACKSIZE);
786                        m_stkStack[0]=sStkNN(root0,root1);
787                        do      {               
788                                sStkNN  p=m_stkStack[--depth];
789                                if(depth>treshold)
[1963]790                                {
[2430]791                                        m_stkStack.resize(m_stkStack.size()*2);
792                                        treshold=m_stkStack.size()-4;
[1972]793                                }
[2430]794                                if(p.a==p.b)
[1972]795                                {
[2430]796                                        if(p.a->isinternal())
797                                        {
798                                                m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]);
799                                                m_stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]);
800                                                m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]);
[1972]801                                        }
[2430]802                                }
803                                else if(Intersect(p.a->volume,p.b->volume))
804                                {
805                                        if(p.a->isinternal())
806                                        {
807                                                if(p.b->isinternal())
808                                                {
809                                                        m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
810                                                        m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
811                                                        m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
812                                                        m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
813                                                }
814                                                else
815                                                {
816                                                        m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
817                                                        m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
818                                                }
819                                        }
[1972]820                                        else
[1963]821                                        {
[2430]822                                                if(p.b->isinternal())
823                                                {
824                                                        m_stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
825                                                        m_stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
826                                                }
827                                                else
828                                                {
829                                                        policy.Process(p.a,p.b);
830                                                }
[1963]831                                        }
[1972]832                                }
[2430]833                        } while(depth);
834                }
835}
836
[2882]837#if 0
[2430]838//
839DBVT_PREFIX
840inline void             btDbvt::collideTT(      const btDbvtNode* root0,
841                                                                  const btDbvtNode* root1,
842                                                                  const btTransform& xform,
843                                                                  DBVT_IPOLICY)
844{
845        DBVT_CHECKTYPE
846                if(root0&&root1)
847                {
848                        int                                                             depth=1;
849                        int                                                             treshold=DOUBLE_STACKSIZE-4;
850                        btAlignedObjectArray<sStkNN>    stkStack;
851                        stkStack.resize(DOUBLE_STACKSIZE);
852                        stkStack[0]=sStkNN(root0,root1);
853                        do      {
854                                sStkNN  p=stkStack[--depth];
855                                if(Intersect(p.a->volume,p.b->volume,xform))
[1972]856                                {
[2430]857                                        if(depth>treshold)
[1963]858                                        {
[2430]859                                                stkStack.resize(stkStack.size()*2);
860                                                treshold=stkStack.size()-4;
[1963]861                                        }
[2430]862                                        if(p.a->isinternal())
863                                        {
864                                                if(p.b->isinternal())
865                                                {                                       
866                                                        stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
867                                                        stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
868                                                        stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
869                                                        stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
870                                                }
871                                                else
872                                                {
873                                                        stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
874                                                        stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
875                                                }
876                                        }
[1963]877                                        else
878                                        {
[2430]879                                                if(p.b->isinternal())
880                                                {
881                                                        stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
882                                                        stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
883                                                }
884                                                else
885                                                {
886                                                        policy.Process(p.a,p.b);
887                                                }
[1963]888                                        }
889                                }
[2430]890                        } while(depth);
891                }
[1963]892}
893//
894DBVT_PREFIX
895inline void             btDbvt::collideTT(      const btDbvtNode* root0,
[2430]896                                                                  const btTransform& xform0,
897                                                                  const btDbvtNode* root1,
898                                                                  const btTransform& xform1,
899                                                                  DBVT_IPOLICY)
[1963]900{
[2430]901        const btTransform       xform=xform0.inverse()*xform1;
902        collideTT(root0,root1,xform,policy);
[1963]903}
[2882]904#endif
[1963]905
906//
907DBVT_PREFIX
908inline void             btDbvt::collideTV(      const btDbvtNode* root,
[2430]909                                                                  const btDbvtVolume& vol,
910                                                                  DBVT_IPOLICY)
[1963]911{
[2430]912        DBVT_CHECKTYPE
913                if(root)
914                {
915                        ATTRIBUTE_ALIGNED16(btDbvtVolume)               volume(vol);
916                        btAlignedObjectArray<const btDbvtNode*> stack;
917                        stack.resize(0);
918                        stack.reserve(SIMPLE_STACKSIZE);
919                        stack.push_back(root);
920                        do      {
921                                const btDbvtNode*       n=stack[stack.size()-1];
922                                stack.pop_back();
923                                if(Intersect(n->volume,volume))
924                                {
925                                        if(n->isinternal())
926                                        {
927                                                stack.push_back(n->childs[0]);
928                                                stack.push_back(n->childs[1]);
929                                        }
930                                        else
931                                        {
932                                                policy.Process(n);
933                                        }
934                                }
935                        } while(stack.size()>0);
936                }
937}
938
939DBVT_PREFIX
940inline void             btDbvt::rayTestInternal(        const btDbvtNode* root,
941                                                                const btVector3& rayFrom,
942                                                                const btVector3& rayTo,
943                                                                const btVector3& rayDirectionInverse,
944                                                                unsigned int signs[3],
945                                                                btScalar lambda_max,
946                                                                const btVector3& aabbMin,
947                                                                const btVector3& aabbMax,
948                                                                DBVT_IPOLICY) const
949{
950        DBVT_CHECKTYPE
951        if(root)
[1972]952        {
[2430]953                btVector3 resultNormal;
954
955                int                                                             depth=1;
956                int                                                             treshold=DOUBLE_STACKSIZE-2;
957                btAlignedObjectArray<const btDbvtNode*> stack;
958                stack.resize(DOUBLE_STACKSIZE);
959                stack[0]=root;
960                btVector3 bounds[2];
961                do     
962                {
963                        const btDbvtNode*       node=stack[--depth];
964                        bounds[0] = node->volume.Mins()+aabbMin;
965                        bounds[1] = node->volume.Maxs()+aabbMax;
966                        btScalar tmin=1.f,lambda_min=0.f;
967                        unsigned int result1=false;
968                        result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
969                        if(result1)
[1972]970                        {
[2430]971                                if(node->isinternal())
[1963]972                                {
[2430]973                                        if(depth>treshold)
974                                        {
975                                                stack.resize(stack.size()*2);
976                                                treshold=stack.size()-2;
977                                        }
978                                        stack[depth++]=node->childs[0];
979                                        stack[depth++]=node->childs[1];
[1963]980                                }
[1972]981                                else
982                                {
[2430]983                                        policy.Process(node);
[1972]984                                }
985                        }
[2430]986                } while(depth);
[1972]987        }
[1963]988}
989
990//
991DBVT_PREFIX
[2430]992inline void             btDbvt::rayTest(        const btDbvtNode* root,
993                                                                const btVector3& rayFrom,
994                                                                const btVector3& rayTo,
995                                                                DBVT_IPOLICY)
[1963]996{
[2430]997        DBVT_CHECKTYPE
998                if(root)
999                {
1000                        btVector3 rayDir = (rayTo-rayFrom);
1001                        rayDir.normalize ();
1002
1003                        ///what about division by zero? --> just set rayDirection[i] to INF/1e30
1004                        btVector3 rayDirectionInverse;
1005                        rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
1006                        rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
1007                        rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
1008                        unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0};
1009
1010                        btScalar lambda_max = rayDir.dot(rayTo-rayFrom);
1011
1012                        btVector3 resultNormal;
1013
1014                        btAlignedObjectArray<const btDbvtNode*> stack;
1015
1016                        int                                                             depth=1;
1017                        int                                                             treshold=DOUBLE_STACKSIZE-2;
1018
1019                        stack.resize(DOUBLE_STACKSIZE);
1020                        stack[0]=root;
1021                        btVector3 bounds[2];
1022                        do      {
1023                                const btDbvtNode*       node=stack[--depth];
1024
1025                                bounds[0] = node->volume.Mins();
1026                                bounds[1] = node->volume.Maxs();
1027                               
1028                                btScalar tmin=1.f,lambda_min=0.f;
1029                                unsigned int result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
1030
1031#ifdef COMPARE_BTRAY_AABB2
1032                                btScalar param=1.f;
1033                                bool result2 = btRayAabb(rayFrom,rayTo,node->volume.Mins(),node->volume.Maxs(),param,resultNormal);
1034                                btAssert(result1 == result2);
1035#endif //TEST_BTRAY_AABB2
1036
1037                                if(result1)
[1963]1038                                {
[2430]1039                                        if(node->isinternal())
1040                                        {
1041                                                if(depth>treshold)
1042                                                {
1043                                                        stack.resize(stack.size()*2);
1044                                                        treshold=stack.size()-2;
1045                                                }
1046                                                stack[depth++]=node->childs[0];
1047                                                stack[depth++]=node->childs[1];
1048                                        }
1049                                        else
1050                                        {
1051                                                policy.Process(node);
1052                                        }
[1963]1053                                }
[2430]1054                        } while(depth);
1055
1056                }
[1963]1057}
1058
1059//
1060DBVT_PREFIX
1061inline void             btDbvt::collideKDOP(const btDbvtNode* root,
1062                                                                        const btVector3* normals,
1063                                                                        const btScalar* offsets,
1064                                                                        int count,
1065                                                                        DBVT_IPOLICY)
1066{
[2430]1067        DBVT_CHECKTYPE
1068                if(root)
[1963]1069                {
[2430]1070                        const int                                               inside=(1<<count)-1;
1071                        btAlignedObjectArray<sStkNP>    stack;
1072                        int                                                             signs[sizeof(unsigned)*8];
1073                        btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
1074                        for(int i=0;i<count;++i)
1075                        {
1076                                signs[i]=       ((normals[i].x()>=0)?1:0)+
[1963]1077                                        ((normals[i].y()>=0)?2:0)+
1078                                        ((normals[i].z()>=0)?4:0);
[2430]1079                        }
1080                        stack.reserve(SIMPLE_STACKSIZE);
1081                        stack.push_back(sStkNP(root,0));
1082                        do      {
1083                                sStkNP  se=stack[stack.size()-1];
1084                                bool    out=false;
1085                                stack.pop_back();
1086                                for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
[1963]1087                                {
[2430]1088                                        if(0==(se.mask&j))
[1963]1089                                        {
[2430]1090                                                const int       side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
1091                                                switch(side)
1092                                                {
1093                                                case    -1:     out=true;break;
1094                                                case    +1:     se.mask|=j;break;
1095                                                }
[1963]1096                                        }
1097                                }
[2430]1098                                if(!out)
[1963]1099                                {
[2430]1100                                        if((se.mask!=inside)&&(se.node->isinternal()))
1101                                        {
1102                                                stack.push_back(sStkNP(se.node->childs[0],se.mask));
1103                                                stack.push_back(sStkNP(se.node->childs[1],se.mask));
1104                                        }
1105                                        else
1106                                        {
1107                                                if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy);
1108                                        }
[1963]1109                                }
[2430]1110                        } while(stack.size());
1111                }
[1963]1112}
1113
1114//
1115DBVT_PREFIX
1116inline void             btDbvt::collideOCL(     const btDbvtNode* root,
[2430]1117                                                                   const btVector3* normals,
1118                                                                   const btScalar* offsets,
1119                                                                   const btVector3& sortaxis,
1120                                                                   int count,
1121                                                                   DBVT_IPOLICY,
1122                                                                   bool fsort)
[1963]1123{
[2430]1124        DBVT_CHECKTYPE
1125                if(root)
[1963]1126                {
[2430]1127                        const unsigned                                  srtsgns=(sortaxis[0]>=0?1:0)+
1128                                (sortaxis[1]>=0?2:0)+
1129                                (sortaxis[2]>=0?4:0);
1130                        const int                                               inside=(1<<count)-1;
1131                        btAlignedObjectArray<sStkNPS>   stock;
1132                        btAlignedObjectArray<int>               ifree;
1133                        btAlignedObjectArray<int>               stack;
1134                        int                                                             signs[sizeof(unsigned)*8];
1135                        btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
1136                        for(int i=0;i<count;++i)
1137                        {
1138                                signs[i]=       ((normals[i].x()>=0)?1:0)+
[1963]1139                                        ((normals[i].y()>=0)?2:0)+
1140                                        ((normals[i].z()>=0)?4:0);
[2430]1141                        }
1142                        stock.reserve(SIMPLE_STACKSIZE);
1143                        stack.reserve(SIMPLE_STACKSIZE);
1144                        ifree.reserve(SIMPLE_STACKSIZE);
1145                        stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns))));
1146                        do      {
1147                                const int       id=stack[stack.size()-1];
1148                                sStkNPS         se=stock[id];
1149                                stack.pop_back();ifree.push_back(id);
1150                                if(se.mask!=inside)
[1963]1151                                {
[2430]1152                                        bool    out=false;
1153                                        for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
[1963]1154                                        {
[2430]1155                                                if(0==(se.mask&j))
[1963]1156                                                {
[2430]1157                                                        const int       side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
1158                                                        switch(side)
1159                                                        {
1160                                                        case    -1:     out=true;break;
1161                                                        case    +1:     se.mask|=j;break;
1162                                                        }
[1963]1163                                                }
1164                                        }
[2430]1165                                        if(out) continue;
[1963]1166                                }
[2430]1167                                if(policy.Descent(se.node))
[1963]1168                                {
[2430]1169                                        if(se.node->isinternal())
[1963]1170                                        {
[2430]1171                                                const btDbvtNode* pns[]={       se.node->childs[0],se.node->childs[1]};
1172                                                sStkNPS         nes[]={ sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)),
1173                                                        sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))};
1174                                                const int       q=nes[0].value<nes[1].value?1:0;                               
1175                                                int                     j=stack.size();
1176                                                if(fsort&&(j>0))
1177                                                {
1178                                                        /* Insert 0     */ 
1179                                                        j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size());
1180                                                        stack.push_back(0);
1181#if DBVT_USE_MEMMOVE
1182                                                        memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
1183#else
1184                                                        for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
1185#endif
1186                                                        stack[j]=allocate(ifree,stock,nes[q]);
1187                                                        /* Insert 1     */ 
1188                                                        j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size());
1189                                                        stack.push_back(0);
1190#if DBVT_USE_MEMMOVE
1191                                                        memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
1192#else
1193                                                        for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
1194#endif
1195                                                        stack[j]=allocate(ifree,stock,nes[1-q]);
1196                                                }
1197                                                else
1198                                                {
1199                                                        stack.push_back(allocate(ifree,stock,nes[q]));
1200                                                        stack.push_back(allocate(ifree,stock,nes[1-q]));
1201                                                }
[1963]1202                                        }
1203                                        else
1204                                        {
[2430]1205                                                policy.Process(se.node,se.value);
[1963]1206                                        }
1207                                }
[2430]1208                        } while(stack.size());
1209                }
[1963]1210}
1211
1212//
1213DBVT_PREFIX
1214inline void             btDbvt::collideTU(      const btDbvtNode* root,
[2430]1215                                                                  DBVT_IPOLICY)
[1963]1216{
[2430]1217        DBVT_CHECKTYPE
1218                if(root)
1219                {
1220                        btAlignedObjectArray<const btDbvtNode*> stack;
1221                        stack.reserve(SIMPLE_STACKSIZE);
1222                        stack.push_back(root);
1223                        do      {
1224                                const btDbvtNode*       n=stack[stack.size()-1];
1225                                stack.pop_back();
1226                                if(policy.Descent(n))
1227                                {
1228                                        if(n->isinternal())
1229                                        { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); }
1230                                        else
1231                                        { policy.Process(n); }
1232                                }
1233                        } while(stack.size()>0);
1234                }
[1963]1235}
1236
1237//
1238// PP Cleanup
1239//
1240
1241#undef DBVT_USE_MEMMOVE
1242#undef DBVT_USE_TEMPLATE
1243#undef DBVT_VIRTUAL_DTOR
1244#undef DBVT_VIRTUAL
1245#undef DBVT_PREFIX
1246#undef DBVT_IPOLICY
1247#undef DBVT_CHECKTYPE
1248#undef DBVT_IMPL_GENERIC
1249#undef DBVT_IMPL_SSE
1250#undef DBVT_USE_INTRINSIC_SSE
1251#undef DBVT_SELECT_IMPL
1252#undef DBVT_MERGE_IMPL
1253#undef DBVT_INT0_IMPL
1254
1255#endif
Note: See TracBrowser for help on using the repository browser.