Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/wiimote/src/external/bullet/BulletCollision/BroadphaseCollision/btDbvt.h @ 11454

Last change on this file since 11454 was 8351, checked in by rgrieder, 14 years ago

Merged kicklib2 branch back to trunk (includes former branches ois_update, mac_osx and kicklib).

Notes for updating

Linux:
You don't need an extra package for CEGUILua and Tolua, it's already shipped with CEGUI.
However you do need to make sure that the OgreRenderer is installed too with CEGUI 0.7 (may be a separate package).
Also, Orxonox now recognises if you install the CgProgramManager (a separate package available on newer Ubuntu on Debian systems).

Windows:
Download the new dependency packages versioned 6.0 and use these. If you have problems with that or if you don't like the in game console problem mentioned below, you can download the new 4.3 version of the packages (only available for Visual Studio 2005/2008).

Key new features:

  • *Support for Mac OS X*
  • Visual Studio 2010 support
  • Bullet library update to 2.77
  • OIS library update to 1.3
  • Support for CEGUI 0.7 —> Support for Arch Linux and even SuSE
  • Improved install target
  • Compiles now with GCC 4.6
  • Ogre Cg Shader plugin activated for Linux if available
  • And of course lots of bug fixes

There are also some regressions:

  • No support for CEGUI 0.5, Ogre 1.4 and boost 1.35 - 1.39 any more
  • In game console is not working in main menu for CEGUI 0.7
  • Tolua (just the C lib, not the application) and CEGUILua libraries are no longer in our repository. —> You will need to get these as well when compiling Orxonox
  • And of course lots of new bugs we don't yet know about
  • Property svn:eol-style set to native
File size: 31.2 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 (BT_USE_SSE) && defined (_WIN32)
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*)1;(void)typechecker;
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#if !defined( __CELLOS_LV2__) && !defined(__MWERKS__)
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       
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);
163private:
164        DBVT_INLINE void                                AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const;
165private:
166        btVector3       mi,mx;
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()); }
179        union
180        {
181                btDbvtNode*     childs[2];
182                void*   data;
183                int             dataAsInt;
184        };
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
191{
192        /* Stack element        */ 
193        struct  sStkNN
194        {
195                const btDbvtNode*       a;
196                const btDbvtNode*       b;
197                sStkNN() {}
198                sStkNN(const btDbvtNode* na,const btDbvtNode* nb) : a(na),b(nb) {}
199        };
200        struct  sStkNP
201        {
202                const btDbvtNode*       node;
203                int                     mask;
204                sStkNP(const btDbvtNode* n,unsigned m) : node(n),mask(m) {}
205        };
206        struct  sStkNPS
207        {
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) {}
213        };
214        struct  sStkCLN
215        {
216                const btDbvtNode*       node;
217                btDbvtNode*             parent;
218                sStkCLN(const btDbvtNode* n,btDbvtNode* p) : node(n),parent(p) {}
219        };
220        // Policies/Interfaces
221
222        /* ICollide     */ 
223        struct  ICollide
224        {               
225                DBVT_VIRTUAL_DTOR(ICollide)
226                        DBVT_VIRTUAL void       Process(const btDbvtNode*,const btDbvtNode*)            {}
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); }
231        };
232        /* IWriter      */ 
233        struct  IWriter
234        {
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;
239        };
240        /* IClone       */ 
241        struct  IClone
242        {
243                virtual ~IClone()       {}
244                virtual void            CloneLeaf(btDbvtNode*) {}
245        };
246
247        // Constants
248        enum    {
249                SIMPLE_STACKSIZE        =       64,
250                DOUBLE_STACKSIZE        =       SIMPLE_STACKSIZE*2
251        };
252
253        // Fields
254        btDbvtNode*             m_root;
255        btDbvtNode*             m_free;
256        int                             m_lkhd;
257        int                             m_leaves;
258        unsigned                m_opath;
259
260       
261        btAlignedObjectArray<sStkNN>    m_stkStack;
262
263
264        // Methods
265        btDbvt();
266        ~btDbvt();
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);
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); 
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);
284#if DBVT_ENABLE_BENCHMARK
285        static void             benchmark();
286#else
287        static void             benchmark(){}
288#endif
289        // DBVT_IPOLICY must support ICollide policy/interface
290        DBVT_PREFIX
291                static void             enumNodes(      const btDbvtNode* root,
292                DBVT_IPOLICY);
293        DBVT_PREFIX
294                static void             enumLeaves(     const btDbvtNode* root,
295                DBVT_IPOLICY);
296        DBVT_PREFIX
297                void            collideTT(      const btDbvtNode* root0,
298                const btDbvtNode* root1,
299                DBVT_IPOLICY);
300
301        DBVT_PREFIX
302                void            collideTTpersistentStack(       const btDbvtNode* root0,
303                  const btDbvtNode* root1,
304                  DBVT_IPOLICY);
305#if 0
306        DBVT_PREFIX
307                void            collideTT(      const btDbvtNode* root0,
308                const btDbvtNode* root1,
309                const btTransform& xform,
310                DBVT_IPOLICY);
311        DBVT_PREFIX
312                void            collideTT(      const btDbvtNode* root0,
313                const btTransform& xform0,
314                const btDbvtNode* root1,
315                const btTransform& xform1,
316                DBVT_IPOLICY);
317#endif
318
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((btDot(n,px)+o)<0)           return(-1);
488        if((btDot(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(btDot(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
534
535//
536DBVT_INLINE bool                Intersect(      const btDbvtAabbMm& a,
537                                                                  const btVector3& b)
538{
539        return( (b.x()>=a.mi.x())&&
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
547
548
549
550
551//////////////////////////////////////
552
553
554//
555DBVT_INLINE btScalar    Proximity(      const btDbvtAabbMm& a,
556                                                                  const btDbvtAabbMm& b)
557{
558        const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx);
559        return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z()));
560}
561
562
563
564//
565DBVT_INLINE int                 Select( const btDbvtAabbMm& o,
566                                                           const btDbvtAabbMm& a,
567                                                           const btDbvtAabbMm& b)
568{
569#if     DBVT_SELECT_IMPL == DBVT_IMPL_SSE
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
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));
594        __m128 t1(_mm_movehl_ps(bmi,bmi));
595        bmi=_mm_add_ps(bmi,t1);
596        bmi=_mm_add_ss(bmi,_mm_shuffle_ps(bmi,bmi,1));
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]);
604        __asm
605        {
606                mov             eax,o
607                        mov             ecx,a
608                        mov             edx,b
609                        movaps  xmm0,[eax]
610                movaps  xmm5,mask
611                        addps   xmm0,[eax+16]   
612                movaps  xmm1,[ecx]
613                movaps  xmm2,[edx]
614                addps   xmm1,[ecx+16]
615                addps   xmm2,[edx+16]
616                subps   xmm1,xmm0
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        }
631        return(r[0]&1);
632#endif
633#else
634        return(Proximity(o,a)<Proximity(o,b)?0:1);
635#endif
636}
637
638//
639DBVT_INLINE void                Merge(  const btDbvtAabbMm& a,
640                                                          const btDbvtAabbMm& b,
641                                                          btDbvtAabbMm& r)
642{
643#if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
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);
652#else
653        for(int i=0;i<3;++i)
654        {
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];
657        }
658#endif
659}
660
661//
662DBVT_INLINE bool                NotEqual(       const btDbvtAabbMm& a,
663                                                                 const btDbvtAabbMm& b)
664{
665        return( (a.mi.x()!=b.mi.x())||
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,
680                                                                  DBVT_IPOLICY)
681{
682        DBVT_CHECKTYPE
683                policy.Process(root);
684        if(root->isinternal())
685        {
686                enumNodes(root->childs[0],policy);
687                enumNodes(root->childs[1],policy);
688        }
689}
690
691//
692DBVT_PREFIX
693inline void             btDbvt::enumLeaves(     const btDbvtNode* root,
694                                                                   DBVT_IPOLICY)
695{
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                }
706}
707
708//
709DBVT_PREFIX
710inline void             btDbvt::collideTT(      const btDbvtNode* root0,
711                                                                  const btDbvtNode* root1,
712                                                                  DBVT_IPOLICY)
713{
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)
725                                {
726                                        stkStack.resize(stkStack.size()*2);
727                                        treshold=stkStack.size()-4;
728                                }
729                                if(p.a==p.b)
730                                {
731                                        if(p.a->isinternal())
732                                        {
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]);
736                                        }
737                                }
738                                else if(Intersect(p.a->volume,p.b->volume))
739                                {
740                                        if(p.a->isinternal())
741                                        {
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                                                }
754                                        }
755                                        else
756                                        {
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                                                }
766                                        }
767                                }
768                        } while(depth);
769                }
770}
771
772
773
774DBVT_PREFIX
775inline void             btDbvt::collideTTpersistentStack(       const btDbvtNode* root0,
776                                                                  const btDbvtNode* root1,
777                                                                  DBVT_IPOLICY)
778{
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)
790                                {
791                                        m_stkStack.resize(m_stkStack.size()*2);
792                                        treshold=m_stkStack.size()-4;
793                                }
794                                if(p.a==p.b)
795                                {
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]);
801                                        }
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                                        }
820                                        else
821                                        {
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                                                }
831                                        }
832                                }
833                        } while(depth);
834                }
835}
836
837#if 0
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))
856                                {
857                                        if(depth>treshold)
858                                        {
859                                                stkStack.resize(stkStack.size()*2);
860                                                treshold=stkStack.size()-4;
861                                        }
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                                        }
877                                        else
878                                        {
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                                                }
888                                        }
889                                }
890                        } while(depth);
891                }
892}
893//
894DBVT_PREFIX
895inline void             btDbvt::collideTT(      const btDbvtNode* root0,
896                                                                  const btTransform& xform0,
897                                                                  const btDbvtNode* root1,
898                                                                  const btTransform& xform1,
899                                                                  DBVT_IPOLICY)
900{
901        const btTransform       xform=xform0.inverse()*xform1;
902        collideTT(root0,root1,xform,policy);
903}
904#endif
905
906//
907DBVT_PREFIX
908inline void             btDbvt::collideTV(      const btDbvtNode* root,
909                                                                  const btDbvtVolume& vol,
910                                                                  DBVT_IPOLICY)
911{
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        (void) rayTo;
951        DBVT_CHECKTYPE
952        if(root)
953        {
954                btVector3 resultNormal;
955
956                int                                                             depth=1;
957                int                                                             treshold=DOUBLE_STACKSIZE-2;
958                btAlignedObjectArray<const btDbvtNode*> stack;
959                stack.resize(DOUBLE_STACKSIZE);
960                stack[0]=root;
961                btVector3 bounds[2];
962                do     
963                {
964                        const btDbvtNode*       node=stack[--depth];
965                        bounds[0] = node->volume.Mins()-aabbMax;
966                        bounds[1] = node->volume.Maxs()-aabbMin;
967                        btScalar tmin=1.f,lambda_min=0.f;
968                        unsigned int result1=false;
969                        result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
970                        if(result1)
971                        {
972                                if(node->isinternal())
973                                {
974                                        if(depth>treshold)
975                                        {
976                                                stack.resize(stack.size()*2);
977                                                treshold=stack.size()-2;
978                                        }
979                                        stack[depth++]=node->childs[0];
980                                        stack[depth++]=node->childs[1];
981                                }
982                                else
983                                {
984                                        policy.Process(node);
985                                }
986                        }
987                } while(depth);
988        }
989}
990
991//
992DBVT_PREFIX
993inline void             btDbvt::rayTest(        const btDbvtNode* root,
994                                                                const btVector3& rayFrom,
995                                                                const btVector3& rayTo,
996                                                                DBVT_IPOLICY)
997{
998        DBVT_CHECKTYPE
999                if(root)
1000                {
1001                        btVector3 rayDir = (rayTo-rayFrom);
1002                        rayDir.normalize ();
1003
1004                        ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
1005                        btVector3 rayDirectionInverse;
1006                        rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
1007                        rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
1008                        rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
1009                        unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0};
1010
1011                        btScalar lambda_max = rayDir.dot(rayTo-rayFrom);
1012
1013                        btVector3 resultNormal;
1014
1015                        btAlignedObjectArray<const btDbvtNode*> stack;
1016
1017                        int                                                             depth=1;
1018                        int                                                             treshold=DOUBLE_STACKSIZE-2;
1019
1020                        stack.resize(DOUBLE_STACKSIZE);
1021                        stack[0]=root;
1022                        btVector3 bounds[2];
1023                        do      {
1024                                const btDbvtNode*       node=stack[--depth];
1025
1026                                bounds[0] = node->volume.Mins();
1027                                bounds[1] = node->volume.Maxs();
1028                               
1029                                btScalar tmin=1.f,lambda_min=0.f;
1030                                unsigned int result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
1031
1032#ifdef COMPARE_BTRAY_AABB2
1033                                btScalar param=1.f;
1034                                bool result2 = btRayAabb(rayFrom,rayTo,node->volume.Mins(),node->volume.Maxs(),param,resultNormal);
1035                                btAssert(result1 == result2);
1036#endif //TEST_BTRAY_AABB2
1037
1038                                if(result1)
1039                                {
1040                                        if(node->isinternal())
1041                                        {
1042                                                if(depth>treshold)
1043                                                {
1044                                                        stack.resize(stack.size()*2);
1045                                                        treshold=stack.size()-2;
1046                                                }
1047                                                stack[depth++]=node->childs[0];
1048                                                stack[depth++]=node->childs[1];
1049                                        }
1050                                        else
1051                                        {
1052                                                policy.Process(node);
1053                                        }
1054                                }
1055                        } while(depth);
1056
1057                }
1058}
1059
1060//
1061DBVT_PREFIX
1062inline void             btDbvt::collideKDOP(const btDbvtNode* root,
1063                                                                        const btVector3* normals,
1064                                                                        const btScalar* offsets,
1065                                                                        int count,
1066                                                                        DBVT_IPOLICY)
1067{
1068        DBVT_CHECKTYPE
1069                if(root)
1070                {
1071                        const int                                               inside=(1<<count)-1;
1072                        btAlignedObjectArray<sStkNP>    stack;
1073                        int                                                             signs[sizeof(unsigned)*8];
1074                        btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
1075                        for(int i=0;i<count;++i)
1076                        {
1077                                signs[i]=       ((normals[i].x()>=0)?1:0)+
1078                                        ((normals[i].y()>=0)?2:0)+
1079                                        ((normals[i].z()>=0)?4:0);
1080                        }
1081                        stack.reserve(SIMPLE_STACKSIZE);
1082                        stack.push_back(sStkNP(root,0));
1083                        do      {
1084                                sStkNP  se=stack[stack.size()-1];
1085                                bool    out=false;
1086                                stack.pop_back();
1087                                for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
1088                                {
1089                                        if(0==(se.mask&j))
1090                                        {
1091                                                const int       side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
1092                                                switch(side)
1093                                                {
1094                                                case    -1:     out=true;break;
1095                                                case    +1:     se.mask|=j;break;
1096                                                }
1097                                        }
1098                                }
1099                                if(!out)
1100                                {
1101                                        if((se.mask!=inside)&&(se.node->isinternal()))
1102                                        {
1103                                                stack.push_back(sStkNP(se.node->childs[0],se.mask));
1104                                                stack.push_back(sStkNP(se.node->childs[1],se.mask));
1105                                        }
1106                                        else
1107                                        {
1108                                                if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy);
1109                                        }
1110                                }
1111                        } while(stack.size());
1112                }
1113}
1114
1115//
1116DBVT_PREFIX
1117inline void             btDbvt::collideOCL(     const btDbvtNode* root,
1118                                                                   const btVector3* normals,
1119                                                                   const btScalar* offsets,
1120                                                                   const btVector3& sortaxis,
1121                                                                   int count,
1122                                                                   DBVT_IPOLICY,
1123                                                                   bool fsort)
1124{
1125        DBVT_CHECKTYPE
1126                if(root)
1127                {
1128                        const unsigned                                  srtsgns=(sortaxis[0]>=0?1:0)+
1129                                (sortaxis[1]>=0?2:0)+
1130                                (sortaxis[2]>=0?4:0);
1131                        const int                                               inside=(1<<count)-1;
1132                        btAlignedObjectArray<sStkNPS>   stock;
1133                        btAlignedObjectArray<int>               ifree;
1134                        btAlignedObjectArray<int>               stack;
1135                        int                                                             signs[sizeof(unsigned)*8];
1136                        btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
1137                        for(int i=0;i<count;++i)
1138                        {
1139                                signs[i]=       ((normals[i].x()>=0)?1:0)+
1140                                        ((normals[i].y()>=0)?2:0)+
1141                                        ((normals[i].z()>=0)?4:0);
1142                        }
1143                        stock.reserve(SIMPLE_STACKSIZE);
1144                        stack.reserve(SIMPLE_STACKSIZE);
1145                        ifree.reserve(SIMPLE_STACKSIZE);
1146                        stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns))));
1147                        do      {
1148                                const int       id=stack[stack.size()-1];
1149                                sStkNPS         se=stock[id];
1150                                stack.pop_back();ifree.push_back(id);
1151                                if(se.mask!=inside)
1152                                {
1153                                        bool    out=false;
1154                                        for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
1155                                        {
1156                                                if(0==(se.mask&j))
1157                                                {
1158                                                        const int       side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
1159                                                        switch(side)
1160                                                        {
1161                                                        case    -1:     out=true;break;
1162                                                        case    +1:     se.mask|=j;break;
1163                                                        }
1164                                                }
1165                                        }
1166                                        if(out) continue;
1167                                }
1168                                if(policy.Descent(se.node))
1169                                {
1170                                        if(se.node->isinternal())
1171                                        {
1172                                                const btDbvtNode* pns[]={       se.node->childs[0],se.node->childs[1]};
1173                                                sStkNPS         nes[]={ sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)),
1174                                                        sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))};
1175                                                const int       q=nes[0].value<nes[1].value?1:0;                               
1176                                                int                     j=stack.size();
1177                                                if(fsort&&(j>0))
1178                                                {
1179                                                        /* Insert 0     */ 
1180                                                        j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size());
1181                                                        stack.push_back(0);
1182#if DBVT_USE_MEMMOVE
1183                                                        memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
1184#else
1185                                                        for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
1186#endif
1187                                                        stack[j]=allocate(ifree,stock,nes[q]);
1188                                                        /* Insert 1     */ 
1189                                                        j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size());
1190                                                        stack.push_back(0);
1191#if DBVT_USE_MEMMOVE
1192                                                        memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
1193#else
1194                                                        for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
1195#endif
1196                                                        stack[j]=allocate(ifree,stock,nes[1-q]);
1197                                                }
1198                                                else
1199                                                {
1200                                                        stack.push_back(allocate(ifree,stock,nes[q]));
1201                                                        stack.push_back(allocate(ifree,stock,nes[1-q]));
1202                                                }
1203                                        }
1204                                        else
1205                                        {
1206                                                policy.Process(se.node,se.value);
1207                                        }
1208                                }
1209                        } while(stack.size());
1210                }
1211}
1212
1213//
1214DBVT_PREFIX
1215inline void             btDbvt::collideTU(      const btDbvtNode* root,
1216                                                                  DBVT_IPOLICY)
1217{
1218        DBVT_CHECKTYPE
1219                if(root)
1220                {
1221                        btAlignedObjectArray<const btDbvtNode*> stack;
1222                        stack.reserve(SIMPLE_STACKSIZE);
1223                        stack.push_back(root);
1224                        do      {
1225                                const btDbvtNode*       n=stack[stack.size()-1];
1226                                stack.pop_back();
1227                                if(policy.Descent(n))
1228                                {
1229                                        if(n->isinternal())
1230                                        { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); }
1231                                        else
1232                                        { policy.Process(n); }
1233                                }
1234                        } while(stack.size()>0);
1235                }
1236}
1237
1238//
1239// PP Cleanup
1240//
1241
1242#undef DBVT_USE_MEMMOVE
1243#undef DBVT_USE_TEMPLATE
1244#undef DBVT_VIRTUAL_DTOR
1245#undef DBVT_VIRTUAL
1246#undef DBVT_PREFIX
1247#undef DBVT_IPOLICY
1248#undef DBVT_CHECKTYPE
1249#undef DBVT_IMPL_GENERIC
1250#undef DBVT_IMPL_SSE
1251#undef DBVT_USE_INTRINSIC_SSE
1252#undef DBVT_SELECT_IMPL
1253#undef DBVT_MERGE_IMPL
1254#undef DBVT_INT0_IMPL
1255
1256#endif
Note: See TracBrowser for help on using the repository browser.