btDbvt.h

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00016 
00017 #ifndef BT_DYNAMIC_BOUNDING_VOLUME_TREE_H
00018 #define BT_DYNAMIC_BOUNDING_VOLUME_TREE_H
00019 
00020 #include "LinearMath/btAlignedObjectArray.h"
00021 #include "LinearMath/btVector3.h"
00022 #include "LinearMath/btTransform.h"
00023 #include "LinearMath/btAabbUtil2.h"
00024 
00025 //
00026 // Compile time configuration
00027 //
00028 
00029 
00030 // Implementation profiles
00031 #define DBVT_IMPL_GENERIC               0       // Generic implementation       
00032 #define DBVT_IMPL_SSE                   1       // SSE
00033 
00034 // Template implementation of ICollide
00035 #ifdef _WIN32
00036 #if (defined (_MSC_VER) && _MSC_VER >= 1400)
00037 #define DBVT_USE_TEMPLATE               1
00038 #else
00039 #define DBVT_USE_TEMPLATE               0
00040 #endif
00041 #else
00042 #define DBVT_USE_TEMPLATE               0
00043 #endif
00044 
00045 // Use only intrinsics instead of inline asm
00046 #define DBVT_USE_INTRINSIC_SSE  1
00047 
00048 // Using memmov for collideOCL
00049 #define DBVT_USE_MEMMOVE                1
00050 
00051 // Enable benchmarking code
00052 #define DBVT_ENABLE_BENCHMARK   0
00053 
00054 // Inlining
00055 #define DBVT_INLINE                             SIMD_FORCE_INLINE
00056 
00057 // Specific methods implementation
00058 
00059 //SSE gives errors on a MSVC 7.1
00060 #if defined (BT_USE_SSE) //&& defined (_WIN32)
00061 #define DBVT_SELECT_IMPL                DBVT_IMPL_SSE
00062 #define DBVT_MERGE_IMPL                 DBVT_IMPL_SSE
00063 #define DBVT_INT0_IMPL                  DBVT_IMPL_SSE
00064 #else
00065 #define DBVT_SELECT_IMPL                DBVT_IMPL_GENERIC
00066 #define DBVT_MERGE_IMPL                 DBVT_IMPL_GENERIC
00067 #define DBVT_INT0_IMPL                  DBVT_IMPL_GENERIC
00068 #endif
00069 
00070 #if     (DBVT_SELECT_IMPL==DBVT_IMPL_SSE)||     \
00071         (DBVT_MERGE_IMPL==DBVT_IMPL_SSE)||      \
00072         (DBVT_INT0_IMPL==DBVT_IMPL_SSE)
00073 #include <emmintrin.h>
00074 #endif
00075 
00076 //
00077 // Auto config and checks
00078 //
00079 
00080 #if DBVT_USE_TEMPLATE
00081 #define DBVT_VIRTUAL
00082 #define DBVT_VIRTUAL_DTOR(a)
00083 #define DBVT_PREFIX                                     template <typename T>
00084 #define DBVT_IPOLICY                            T& policy
00085 #define DBVT_CHECKTYPE                          static const ICollide&  typechecker=*(T*)1;(void)typechecker;
00086 #else
00087 #define DBVT_VIRTUAL_DTOR(a)            virtual ~a() {}
00088 #define DBVT_VIRTUAL                            virtual
00089 #define DBVT_PREFIX
00090 #define DBVT_IPOLICY                            ICollide& policy
00091 #define DBVT_CHECKTYPE
00092 #endif
00093 
00094 #if DBVT_USE_MEMMOVE
00095 #if !defined( __CELLOS_LV2__) && !defined(__MWERKS__)
00096 #include <memory.h>
00097 #endif
00098 #include <string.h>
00099 #endif
00100 
00101 #ifndef DBVT_USE_TEMPLATE
00102 #error "DBVT_USE_TEMPLATE undefined"
00103 #endif
00104 
00105 #ifndef DBVT_USE_MEMMOVE
00106 #error "DBVT_USE_MEMMOVE undefined"
00107 #endif
00108 
00109 #ifndef DBVT_ENABLE_BENCHMARK
00110 #error "DBVT_ENABLE_BENCHMARK undefined"
00111 #endif
00112 
00113 #ifndef DBVT_SELECT_IMPL
00114 #error "DBVT_SELECT_IMPL undefined"
00115 #endif
00116 
00117 #ifndef DBVT_MERGE_IMPL
00118 #error "DBVT_MERGE_IMPL undefined"
00119 #endif
00120 
00121 #ifndef DBVT_INT0_IMPL
00122 #error "DBVT_INT0_IMPL undefined"
00123 #endif
00124 
00125 //
00126 // Defaults volumes
00127 //
00128 
00129 /* btDbvtAabbMm                 */ 
00130 struct  btDbvtAabbMm
00131 {
00132         DBVT_INLINE btVector3                   Center() const  { return((mi+mx)/2); }
00133         DBVT_INLINE btVector3                   Lengths() const { return(mx-mi); }
00134         DBVT_INLINE btVector3                   Extents() const { return((mx-mi)/2); }
00135         DBVT_INLINE const btVector3&    Mins() const    { return(mi); }
00136         DBVT_INLINE const btVector3&    Maxs() const    { return(mx); }
00137         static inline btDbvtAabbMm              FromCE(const btVector3& c,const btVector3& e);
00138         static inline btDbvtAabbMm              FromCR(const btVector3& c,btScalar r);
00139         static inline btDbvtAabbMm              FromMM(const btVector3& mi,const btVector3& mx);
00140         static inline btDbvtAabbMm              FromPoints(const btVector3* pts,int n);
00141         static inline btDbvtAabbMm              FromPoints(const btVector3** ppts,int n);
00142         DBVT_INLINE void                                Expand(const btVector3& e);
00143         DBVT_INLINE void                                SignedExpand(const btVector3& e);
00144         DBVT_INLINE bool                                Contain(const btDbvtAabbMm& a) const;
00145         DBVT_INLINE int                                 Classify(const btVector3& n,btScalar o,int s) const;
00146         DBVT_INLINE btScalar                    ProjectMinimum(const btVector3& v,unsigned signs) const;
00147         DBVT_INLINE friend bool                 Intersect(      const btDbvtAabbMm& a,
00148                 const btDbvtAabbMm& b);
00149         
00150         DBVT_INLINE friend bool                 Intersect(      const btDbvtAabbMm& a,
00151                 const btVector3& b);
00152 
00153         DBVT_INLINE friend btScalar             Proximity(      const btDbvtAabbMm& a,
00154                 const btDbvtAabbMm& b);
00155         DBVT_INLINE friend int                  Select(         const btDbvtAabbMm& o,
00156                 const btDbvtAabbMm& a,
00157                 const btDbvtAabbMm& b);
00158         DBVT_INLINE friend void                 Merge(          const btDbvtAabbMm& a,
00159                 const btDbvtAabbMm& b,
00160                 btDbvtAabbMm& r);
00161         DBVT_INLINE friend bool                 NotEqual(       const btDbvtAabbMm& a,
00162                 const btDbvtAabbMm& b);
00163     
00164     DBVT_INLINE btVector3&      tMins() { return(mi); }
00165         DBVT_INLINE btVector3&  tMaxs() { return(mx); }
00166     
00167 private:
00168         DBVT_INLINE void                                AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const;
00169 private:
00170         btVector3       mi,mx;
00171 };
00172 
00173 // Types        
00174 typedef btDbvtAabbMm    btDbvtVolume;
00175 
00176 /* btDbvtNode                           */ 
00177 struct  btDbvtNode
00178 {
00179         btDbvtVolume    volume;
00180         btDbvtNode*             parent;
00181         DBVT_INLINE bool        isleaf() const          { return(childs[1]==0); }
00182         DBVT_INLINE bool        isinternal() const      { return(!isleaf()); }
00183         union
00184         {
00185                 btDbvtNode*     childs[2];
00186                 void*   data;
00187                 int             dataAsInt;
00188         };
00189 };
00190 
00194 struct  btDbvt
00195 {
00196         /* Stack element        */ 
00197         struct  sStkNN
00198         {
00199                 const btDbvtNode*       a;
00200                 const btDbvtNode*       b;
00201                 sStkNN() {}
00202                 sStkNN(const btDbvtNode* na,const btDbvtNode* nb) : a(na),b(nb) {}
00203         };
00204         struct  sStkNP
00205         {
00206                 const btDbvtNode*       node;
00207                 int                     mask;
00208                 sStkNP(const btDbvtNode* n,unsigned m) : node(n),mask(m) {}
00209         };
00210         struct  sStkNPS
00211         {
00212                 const btDbvtNode*       node;
00213                 int                     mask;
00214                 btScalar        value;
00215                 sStkNPS() {}
00216                 sStkNPS(const btDbvtNode* n,unsigned m,btScalar v) : node(n),mask(m),value(v) {}
00217         };
00218         struct  sStkCLN
00219         {
00220                 const btDbvtNode*       node;
00221                 btDbvtNode*             parent;
00222                 sStkCLN(const btDbvtNode* n,btDbvtNode* p) : node(n),parent(p) {}
00223         };
00224         // Policies/Interfaces
00225 
00226         /* ICollide     */ 
00227         struct  ICollide
00228         {               
00229                 DBVT_VIRTUAL_DTOR(ICollide)
00230                         DBVT_VIRTUAL void       Process(const btDbvtNode*,const btDbvtNode*)            {}
00231                 DBVT_VIRTUAL void       Process(const btDbvtNode*)                                      {}
00232                 DBVT_VIRTUAL void       Process(const btDbvtNode* n,btScalar)                   { Process(n); }
00233                 DBVT_VIRTUAL bool       Descent(const btDbvtNode*)                                      { return(true); }
00234                 DBVT_VIRTUAL bool       AllLeaves(const btDbvtNode*)                                    { return(true); }
00235         };
00236         /* IWriter      */ 
00237         struct  IWriter
00238         {
00239                 virtual ~IWriter() {}
00240                 virtual void            Prepare(const btDbvtNode* root,int numnodes)=0;
00241                 virtual void            WriteNode(const btDbvtNode*,int index,int parent,int child0,int child1)=0;
00242                 virtual void            WriteLeaf(const btDbvtNode*,int index,int parent)=0;
00243         };
00244         /* IClone       */ 
00245         struct  IClone
00246         {
00247                 virtual ~IClone()       {}
00248                 virtual void            CloneLeaf(btDbvtNode*) {}
00249         };
00250 
00251         // Constants
00252         enum    {
00253                 SIMPLE_STACKSIZE        =       64,
00254                 DOUBLE_STACKSIZE        =       SIMPLE_STACKSIZE*2
00255         };
00256 
00257         // Fields
00258         btDbvtNode*             m_root;
00259         btDbvtNode*             m_free;
00260         int                             m_lkhd;
00261         int                             m_leaves;
00262         unsigned                m_opath;
00263 
00264         
00265         btAlignedObjectArray<sStkNN>    m_stkStack;
00266         mutable btAlignedObjectArray<const btDbvtNode*> m_rayTestStack;
00267 
00268 
00269         // Methods
00270         btDbvt();
00271         ~btDbvt();
00272         void                    clear();
00273         bool                    empty() const { return(0==m_root); }
00274         void                    optimizeBottomUp();
00275         void                    optimizeTopDown(int bu_treshold=128);
00276         void                    optimizeIncremental(int passes);
00277         btDbvtNode*             insert(const btDbvtVolume& box,void* data);
00278         void                    update(btDbvtNode* leaf,int lookahead=-1);
00279         void                    update(btDbvtNode* leaf,btDbvtVolume& volume);
00280         bool                    update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity,btScalar margin);
00281         bool                    update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity);
00282         bool                    update(btDbvtNode* leaf,btDbvtVolume& volume,btScalar margin);  
00283         void                    remove(btDbvtNode* leaf);
00284         void                    write(IWriter* iwriter) const;
00285         void                    clone(btDbvt& dest,IClone* iclone=0) const;
00286         static int              maxdepth(const btDbvtNode* node);
00287         static int              countLeaves(const btDbvtNode* node);
00288         static void             extractLeaves(const btDbvtNode* node,btAlignedObjectArray<const btDbvtNode*>& leaves);
00289 #if DBVT_ENABLE_BENCHMARK
00290         static void             benchmark();
00291 #else
00292         static void             benchmark(){}
00293 #endif
00294         // DBVT_IPOLICY must support ICollide policy/interface
00295         DBVT_PREFIX
00296                 static void             enumNodes(      const btDbvtNode* root,
00297                 DBVT_IPOLICY);
00298         DBVT_PREFIX
00299                 static void             enumLeaves(     const btDbvtNode* root,
00300                 DBVT_IPOLICY);
00301         DBVT_PREFIX
00302                 void            collideTT(      const btDbvtNode* root0,
00303                 const btDbvtNode* root1,
00304                 DBVT_IPOLICY);
00305 
00306         DBVT_PREFIX
00307                 void            collideTTpersistentStack(       const btDbvtNode* root0,
00308                   const btDbvtNode* root1,
00309                   DBVT_IPOLICY);
00310 #if 0
00311         DBVT_PREFIX
00312                 void            collideTT(      const btDbvtNode* root0,
00313                 const btDbvtNode* root1,
00314                 const btTransform& xform,
00315                 DBVT_IPOLICY);
00316         DBVT_PREFIX
00317                 void            collideTT(      const btDbvtNode* root0,
00318                 const btTransform& xform0,
00319                 const btDbvtNode* root1,
00320                 const btTransform& xform1,
00321                 DBVT_IPOLICY);
00322 #endif
00323 
00324         DBVT_PREFIX
00325                 void            collideTV(      const btDbvtNode* root,
00326                 const btDbvtVolume& volume,
00327                 DBVT_IPOLICY) const;
00330         DBVT_PREFIX
00331                 static void             rayTest(        const btDbvtNode* root,
00332                 const btVector3& rayFrom,
00333                 const btVector3& rayTo,
00334                 DBVT_IPOLICY);
00337         DBVT_PREFIX
00338                 void            rayTestInternal(        const btDbvtNode* root,
00339                                                                 const btVector3& rayFrom,
00340                                                                 const btVector3& rayTo,
00341                                                                 const btVector3& rayDirectionInverse,
00342                                                                 unsigned int signs[3],
00343                                                                 btScalar lambda_max,
00344                                                                 const btVector3& aabbMin,
00345                                                                 const btVector3& aabbMax,
00346                                                                 DBVT_IPOLICY) const;
00347 
00348         DBVT_PREFIX
00349                 static void             collideKDOP(const btDbvtNode* root,
00350                 const btVector3* normals,
00351                 const btScalar* offsets,
00352                 int count,
00353                 DBVT_IPOLICY);
00354         DBVT_PREFIX
00355                 static void             collideOCL(     const btDbvtNode* root,
00356                 const btVector3* normals,
00357                 const btScalar* offsets,
00358                 const btVector3& sortaxis,
00359                 int count,                                                              
00360                 DBVT_IPOLICY,
00361                 bool fullsort=true);
00362         DBVT_PREFIX
00363                 static void             collideTU(      const btDbvtNode* root,
00364                 DBVT_IPOLICY);
00365         // Helpers      
00366         static DBVT_INLINE int  nearest(const int* i,const btDbvt::sStkNPS* a,btScalar v,int l,int h)
00367         {
00368                 int     m=0;
00369                 while(l<h)
00370                 {
00371                         m=(l+h)>>1;
00372                         if(a[i[m]].value>=v) l=m+1; else h=m;
00373                 }
00374                 return(h);
00375         }
00376         static DBVT_INLINE int  allocate(       btAlignedObjectArray<int>& ifree,
00377                 btAlignedObjectArray<sStkNPS>& stock,
00378                 const sStkNPS& value)
00379         {
00380                 int     i;
00381                 if(ifree.size()>0)
00382                 { i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; }
00383                 else
00384                 { i=stock.size();stock.push_back(value); }
00385                 return(i); 
00386         }
00387         //
00388 private:
00389         btDbvt(const btDbvt&)   {}      
00390 };
00391 
00392 //
00393 // Inline's
00394 //
00395 
00396 //
00397 inline btDbvtAabbMm                     btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e)
00398 {
00399         btDbvtAabbMm box;
00400         box.mi=c-e;box.mx=c+e;
00401         return(box);
00402 }
00403 
00404 //
00405 inline btDbvtAabbMm                     btDbvtAabbMm::FromCR(const btVector3& c,btScalar r)
00406 {
00407         return(FromCE(c,btVector3(r,r,r)));
00408 }
00409 
00410 //
00411 inline btDbvtAabbMm                     btDbvtAabbMm::FromMM(const btVector3& mi,const btVector3& mx)
00412 {
00413         btDbvtAabbMm box;
00414         box.mi=mi;box.mx=mx;
00415         return(box);
00416 }
00417 
00418 //
00419 inline btDbvtAabbMm                     btDbvtAabbMm::FromPoints(const btVector3* pts,int n)
00420 {
00421         btDbvtAabbMm box;
00422         box.mi=box.mx=pts[0];
00423         for(int i=1;i<n;++i)
00424         {
00425                 box.mi.setMin(pts[i]);
00426                 box.mx.setMax(pts[i]);
00427         }
00428         return(box);
00429 }
00430 
00431 //
00432 inline btDbvtAabbMm                     btDbvtAabbMm::FromPoints(const btVector3** ppts,int n)
00433 {
00434         btDbvtAabbMm box;
00435         box.mi=box.mx=*ppts[0];
00436         for(int i=1;i<n;++i)
00437         {
00438                 box.mi.setMin(*ppts[i]);
00439                 box.mx.setMax(*ppts[i]);
00440         }
00441         return(box);
00442 }
00443 
00444 //
00445 DBVT_INLINE void                btDbvtAabbMm::Expand(const btVector3& e)
00446 {
00447         mi-=e;mx+=e;
00448 }
00449 
00450 //
00451 DBVT_INLINE void                btDbvtAabbMm::SignedExpand(const btVector3& e)
00452 {
00453         if(e.x()>0) mx.setX(mx.x()+e[0]); else mi.setX(mi.x()+e[0]);
00454         if(e.y()>0) mx.setY(mx.y()+e[1]); else mi.setY(mi.y()+e[1]);
00455         if(e.z()>0) mx.setZ(mx.z()+e[2]); else mi.setZ(mi.z()+e[2]);
00456 }
00457 
00458 //
00459 DBVT_INLINE bool                btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const
00460 {
00461         return( (mi.x()<=a.mi.x())&&
00462                 (mi.y()<=a.mi.y())&&
00463                 (mi.z()<=a.mi.z())&&
00464                 (mx.x()>=a.mx.x())&&
00465                 (mx.y()>=a.mx.y())&&
00466                 (mx.z()>=a.mx.z()));
00467 }
00468 
00469 //
00470 DBVT_INLINE int         btDbvtAabbMm::Classify(const btVector3& n,btScalar o,int s) const
00471 {
00472         btVector3                       pi,px;
00473         switch(s)
00474         {
00475         case    (0+0+0):        px=btVector3(mi.x(),mi.y(),mi.z());
00476                 pi=btVector3(mx.x(),mx.y(),mx.z());break;
00477         case    (1+0+0):        px=btVector3(mx.x(),mi.y(),mi.z());
00478                 pi=btVector3(mi.x(),mx.y(),mx.z());break;
00479         case    (0+2+0):        px=btVector3(mi.x(),mx.y(),mi.z());
00480                 pi=btVector3(mx.x(),mi.y(),mx.z());break;
00481         case    (1+2+0):        px=btVector3(mx.x(),mx.y(),mi.z());
00482                 pi=btVector3(mi.x(),mi.y(),mx.z());break;
00483         case    (0+0+4):        px=btVector3(mi.x(),mi.y(),mx.z());
00484                 pi=btVector3(mx.x(),mx.y(),mi.z());break;
00485         case    (1+0+4):        px=btVector3(mx.x(),mi.y(),mx.z());
00486                 pi=btVector3(mi.x(),mx.y(),mi.z());break;
00487         case    (0+2+4):        px=btVector3(mi.x(),mx.y(),mx.z());
00488                 pi=btVector3(mx.x(),mi.y(),mi.z());break;
00489         case    (1+2+4):        px=btVector3(mx.x(),mx.y(),mx.z());
00490                 pi=btVector3(mi.x(),mi.y(),mi.z());break;
00491         }
00492         if((btDot(n,px)+o)<0)           return(-1);
00493         if((btDot(n,pi)+o)>=0)  return(+1);
00494         return(0);
00495 }
00496 
00497 //
00498 DBVT_INLINE btScalar    btDbvtAabbMm::ProjectMinimum(const btVector3& v,unsigned signs) const
00499 {
00500         const btVector3*        b[]={&mx,&mi};
00501         const btVector3         p(      b[(signs>>0)&1]->x(),
00502                 b[(signs>>1)&1]->y(),
00503                 b[(signs>>2)&1]->z());
00504         return(btDot(p,v));
00505 }
00506 
00507 //
00508 DBVT_INLINE void                btDbvtAabbMm::AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const
00509 {
00510         for(int i=0;i<3;++i)
00511         {
00512                 if(d[i]<0)
00513                 { smi+=mx[i]*d[i];smx+=mi[i]*d[i]; }
00514                 else
00515                 { smi+=mi[i]*d[i];smx+=mx[i]*d[i]; }
00516         }
00517 }
00518 
00519 //
00520 DBVT_INLINE bool                Intersect(      const btDbvtAabbMm& a,
00521                                                                   const btDbvtAabbMm& b)
00522 {
00523 #if     DBVT_INT0_IMPL == DBVT_IMPL_SSE
00524         const __m128    rt(_mm_or_ps(   _mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)),
00525                 _mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi))));
00526 #if defined (_WIN32)
00527         const __int32*  pu((const __int32*)&rt);
00528 #else
00529     const int*  pu((const int*)&rt);
00530 #endif
00531         return((pu[0]|pu[1]|pu[2])==0);
00532 #else
00533         return( (a.mi.x()<=b.mx.x())&&
00534                 (a.mx.x()>=b.mi.x())&&
00535                 (a.mi.y()<=b.mx.y())&&
00536                 (a.mx.y()>=b.mi.y())&&
00537                 (a.mi.z()<=b.mx.z())&&          
00538                 (a.mx.z()>=b.mi.z()));
00539 #endif
00540 }
00541 
00542 
00543 
00544 //
00545 DBVT_INLINE bool                Intersect(      const btDbvtAabbMm& a,
00546                                                                   const btVector3& b)
00547 {
00548         return( (b.x()>=a.mi.x())&&
00549                 (b.y()>=a.mi.y())&&
00550                 (b.z()>=a.mi.z())&&
00551                 (b.x()<=a.mx.x())&&
00552                 (b.y()<=a.mx.y())&&
00553                 (b.z()<=a.mx.z()));
00554 }
00555 
00556 
00557 
00558 
00559 
00561 
00562 
00563 //
00564 DBVT_INLINE btScalar    Proximity(      const btDbvtAabbMm& a,
00565                                                                   const btDbvtAabbMm& b)
00566 {
00567         const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx);
00568         return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z()));
00569 }
00570 
00571 
00572 
00573 //
00574 DBVT_INLINE int                 Select( const btDbvtAabbMm& o,
00575                                                            const btDbvtAabbMm& a,
00576                                                            const btDbvtAabbMm& b)
00577 {
00578 #if     DBVT_SELECT_IMPL == DBVT_IMPL_SSE
00579     
00580 #if defined (_WIN32)
00581         static ATTRIBUTE_ALIGNED16(const unsigned __int32)      mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff};
00582 #else
00583     static ATTRIBUTE_ALIGNED16(const unsigned int)      mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x00000000 /*0x7fffffff*/};
00584 #endif
00585 
00586 #if DBVT_USE_INTRINSIC_SSE
00587 
00588         union btSSEUnion 
00589         {
00590            __m128               ssereg;
00591            float                floats[4];
00592            int                  ints[4];
00593         };
00594 
00595         __m128  omi(_mm_load_ps(o.mi));
00596         omi=_mm_add_ps(omi,_mm_load_ps(o.mx));
00597         __m128  ami(_mm_load_ps(a.mi));
00598         ami=_mm_add_ps(ami,_mm_load_ps(a.mx));
00599         ami=_mm_sub_ps(ami,omi);
00600         ami=_mm_and_ps(ami,_mm_load_ps((const float*)mask));
00601         __m128  bmi(_mm_load_ps(b.mi));
00602         bmi=_mm_add_ps(bmi,_mm_load_ps(b.mx));
00603         bmi=_mm_sub_ps(bmi,omi);
00604         bmi=_mm_and_ps(bmi,_mm_load_ps((const float*)mask));
00605         __m128  t0(_mm_movehl_ps(ami,ami));
00606         ami=_mm_add_ps(ami,t0);
00607         ami=_mm_add_ss(ami,_mm_shuffle_ps(ami,ami,1));
00608         __m128 t1(_mm_movehl_ps(bmi,bmi));
00609         bmi=_mm_add_ps(bmi,t1);
00610         bmi=_mm_add_ss(bmi,_mm_shuffle_ps(bmi,bmi,1));
00611         
00612         btSSEUnion tmp;
00613         tmp.ssereg = _mm_cmple_ss(bmi,ami);
00614         return tmp.ints[0]&1;
00615 
00616 #else
00617         ATTRIBUTE_ALIGNED16(__int32     r[1]);
00618         __asm
00619         {
00620                 mov             eax,o
00621                         mov             ecx,a
00622                         mov             edx,b
00623                         movaps  xmm0,[eax]
00624                 movaps  xmm5,mask
00625                         addps   xmm0,[eax+16]   
00626                 movaps  xmm1,[ecx]
00627                 movaps  xmm2,[edx]
00628                 addps   xmm1,[ecx+16]
00629                 addps   xmm2,[edx+16]
00630                 subps   xmm1,xmm0
00631                         subps   xmm2,xmm0
00632                         andps   xmm1,xmm5
00633                         andps   xmm2,xmm5
00634                         movhlps xmm3,xmm1
00635                         movhlps xmm4,xmm2
00636                         addps   xmm1,xmm3
00637                         addps   xmm2,xmm4
00638                         pshufd  xmm3,xmm1,1
00639                         pshufd  xmm4,xmm2,1
00640                         addss   xmm1,xmm3
00641                         addss   xmm2,xmm4
00642                         cmpless xmm2,xmm1
00643                         movss   r,xmm2
00644         }
00645         return(r[0]&1);
00646 #endif
00647 #else
00648         return(Proximity(o,a)<Proximity(o,b)?0:1);
00649 #endif
00650 }
00651 
00652 //
00653 DBVT_INLINE void                Merge(  const btDbvtAabbMm& a,
00654                                                           const btDbvtAabbMm& b,
00655                                                           btDbvtAabbMm& r)
00656 {
00657 #if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
00658         __m128  ami(_mm_load_ps(a.mi));
00659         __m128  amx(_mm_load_ps(a.mx));
00660         __m128  bmi(_mm_load_ps(b.mi));
00661         __m128  bmx(_mm_load_ps(b.mx));
00662         ami=_mm_min_ps(ami,bmi);
00663         amx=_mm_max_ps(amx,bmx);
00664         _mm_store_ps(r.mi,ami);
00665         _mm_store_ps(r.mx,amx);
00666 #else
00667         for(int i=0;i<3;++i)
00668         {
00669                 if(a.mi[i]<b.mi[i]) r.mi[i]=a.mi[i]; else r.mi[i]=b.mi[i];
00670                 if(a.mx[i]>b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i];
00671         }
00672 #endif
00673 }
00674 
00675 //
00676 DBVT_INLINE bool                NotEqual(       const btDbvtAabbMm& a,
00677                                                                  const btDbvtAabbMm& b)
00678 {
00679         return( (a.mi.x()!=b.mi.x())||
00680                 (a.mi.y()!=b.mi.y())||
00681                 (a.mi.z()!=b.mi.z())||
00682                 (a.mx.x()!=b.mx.x())||
00683                 (a.mx.y()!=b.mx.y())||
00684                 (a.mx.z()!=b.mx.z()));
00685 }
00686 
00687 //
00688 // Inline's
00689 //
00690 
00691 //
00692 DBVT_PREFIX
00693 inline void             btDbvt::enumNodes(      const btDbvtNode* root,
00694                                                                   DBVT_IPOLICY)
00695 {
00696         DBVT_CHECKTYPE
00697                 policy.Process(root);
00698         if(root->isinternal())
00699         {
00700                 enumNodes(root->childs[0],policy);
00701                 enumNodes(root->childs[1],policy);
00702         }
00703 }
00704 
00705 //
00706 DBVT_PREFIX
00707 inline void             btDbvt::enumLeaves(     const btDbvtNode* root,
00708                                                                    DBVT_IPOLICY)
00709 {
00710         DBVT_CHECKTYPE
00711                 if(root->isinternal())
00712                 {
00713                         enumLeaves(root->childs[0],policy);
00714                         enumLeaves(root->childs[1],policy);
00715                 }
00716                 else
00717                 {
00718                         policy.Process(root);
00719                 }
00720 }
00721 
00722 //
00723 DBVT_PREFIX
00724 inline void             btDbvt::collideTT(      const btDbvtNode* root0,
00725                                                                   const btDbvtNode* root1,
00726                                                                   DBVT_IPOLICY)
00727 {
00728         DBVT_CHECKTYPE
00729                 if(root0&&root1)
00730                 {
00731                         int                                                             depth=1;
00732                         int                                                             treshold=DOUBLE_STACKSIZE-4;
00733                         btAlignedObjectArray<sStkNN>    stkStack;
00734                         stkStack.resize(DOUBLE_STACKSIZE);
00735                         stkStack[0]=sStkNN(root0,root1);
00736                         do      {               
00737                                 sStkNN  p=stkStack[--depth];
00738                                 if(depth>treshold)
00739                                 {
00740                                         stkStack.resize(stkStack.size()*2);
00741                                         treshold=stkStack.size()-4;
00742                                 }
00743                                 if(p.a==p.b)
00744                                 {
00745                                         if(p.a->isinternal())
00746                                         {
00747                                                 stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]);
00748                                                 stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]);
00749                                                 stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]);
00750                                         }
00751                                 }
00752                                 else if(Intersect(p.a->volume,p.b->volume))
00753                                 {
00754                                         if(p.a->isinternal())
00755                                         {
00756                                                 if(p.b->isinternal())
00757                                                 {
00758                                                         stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
00759                                                         stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
00760                                                         stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
00761                                                         stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
00762                                                 }
00763                                                 else
00764                                                 {
00765                                                         stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
00766                                                         stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
00767                                                 }
00768                                         }
00769                                         else
00770                                         {
00771                                                 if(p.b->isinternal())
00772                                                 {
00773                                                         stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
00774                                                         stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
00775                                                 }
00776                                                 else
00777                                                 {
00778                                                         policy.Process(p.a,p.b);
00779                                                 }
00780                                         }
00781                                 }
00782                         } while(depth);
00783                 }
00784 }
00785 
00786 
00787 
00788 DBVT_PREFIX
00789 inline void             btDbvt::collideTTpersistentStack(       const btDbvtNode* root0,
00790                                                                   const btDbvtNode* root1,
00791                                                                   DBVT_IPOLICY)
00792 {
00793         DBVT_CHECKTYPE
00794                 if(root0&&root1)
00795                 {
00796                         int                                                             depth=1;
00797                         int                                                             treshold=DOUBLE_STACKSIZE-4;
00798                         
00799                         m_stkStack.resize(DOUBLE_STACKSIZE);
00800                         m_stkStack[0]=sStkNN(root0,root1);
00801                         do      {               
00802                                 sStkNN  p=m_stkStack[--depth];
00803                                 if(depth>treshold)
00804                                 {
00805                                         m_stkStack.resize(m_stkStack.size()*2);
00806                                         treshold=m_stkStack.size()-4;
00807                                 }
00808                                 if(p.a==p.b)
00809                                 {
00810                                         if(p.a->isinternal())
00811                                         {
00812                                                 m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]);
00813                                                 m_stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]);
00814                                                 m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]);
00815                                         }
00816                                 }
00817                                 else if(Intersect(p.a->volume,p.b->volume))
00818                                 {
00819                                         if(p.a->isinternal())
00820                                         {
00821                                                 if(p.b->isinternal())
00822                                                 {
00823                                                         m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
00824                                                         m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
00825                                                         m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
00826                                                         m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
00827                                                 }
00828                                                 else
00829                                                 {
00830                                                         m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
00831                                                         m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
00832                                                 }
00833                                         }
00834                                         else
00835                                         {
00836                                                 if(p.b->isinternal())
00837                                                 {
00838                                                         m_stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
00839                                                         m_stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
00840                                                 }
00841                                                 else
00842                                                 {
00843                                                         policy.Process(p.a,p.b);
00844                                                 }
00845                                         }
00846                                 }
00847                         } while(depth);
00848                 }
00849 }
00850 
00851 #if 0
00852 //
00853 DBVT_PREFIX
00854 inline void             btDbvt::collideTT(      const btDbvtNode* root0,
00855                                                                   const btDbvtNode* root1,
00856                                                                   const btTransform& xform,
00857                                                                   DBVT_IPOLICY)
00858 {
00859         DBVT_CHECKTYPE
00860                 if(root0&&root1)
00861                 {
00862                         int                                                             depth=1;
00863                         int                                                             treshold=DOUBLE_STACKSIZE-4;
00864                         btAlignedObjectArray<sStkNN>    stkStack;
00865                         stkStack.resize(DOUBLE_STACKSIZE);
00866                         stkStack[0]=sStkNN(root0,root1);
00867                         do      {
00868                                 sStkNN  p=stkStack[--depth];
00869                                 if(Intersect(p.a->volume,p.b->volume,xform))
00870                                 {
00871                                         if(depth>treshold)
00872                                         {
00873                                                 stkStack.resize(stkStack.size()*2);
00874                                                 treshold=stkStack.size()-4;
00875                                         }
00876                                         if(p.a->isinternal())
00877                                         {
00878                                                 if(p.b->isinternal())
00879                                                 {                                       
00880                                                         stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
00881                                                         stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
00882                                                         stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
00883                                                         stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
00884                                                 }
00885                                                 else
00886                                                 {
00887                                                         stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
00888                                                         stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
00889                                                 }
00890                                         }
00891                                         else
00892                                         {
00893                                                 if(p.b->isinternal())
00894                                                 {
00895                                                         stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
00896                                                         stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
00897                                                 }
00898                                                 else
00899                                                 {
00900                                                         policy.Process(p.a,p.b);
00901                                                 }
00902                                         }
00903                                 }
00904                         } while(depth);
00905                 }
00906 }
00907 //
00908 DBVT_PREFIX
00909 inline void             btDbvt::collideTT(      const btDbvtNode* root0,
00910                                                                   const btTransform& xform0,
00911                                                                   const btDbvtNode* root1,
00912                                                                   const btTransform& xform1,
00913                                                                   DBVT_IPOLICY)
00914 {
00915         const btTransform       xform=xform0.inverse()*xform1;
00916         collideTT(root0,root1,xform,policy);
00917 }
00918 #endif 
00919 
00920 //
00921 DBVT_PREFIX
00922 inline void             btDbvt::collideTV(      const btDbvtNode* root,
00923                                                                   const btDbvtVolume& vol,
00924                                                                   DBVT_IPOLICY) const
00925 {
00926         DBVT_CHECKTYPE
00927                 if(root)
00928                 {
00929                         ATTRIBUTE_ALIGNED16(btDbvtVolume)               volume(vol);
00930                         btAlignedObjectArray<const btDbvtNode*> stack;
00931                         stack.resize(0);
00932                         stack.reserve(SIMPLE_STACKSIZE);
00933                         stack.push_back(root);
00934                         do      {
00935                                 const btDbvtNode*       n=stack[stack.size()-1];
00936                                 stack.pop_back();
00937                                 if(Intersect(n->volume,volume))
00938                                 {
00939                                         if(n->isinternal())
00940                                         {
00941                                                 stack.push_back(n->childs[0]);
00942                                                 stack.push_back(n->childs[1]);
00943                                         }
00944                                         else
00945                                         {
00946                                                 policy.Process(n);
00947                                         }
00948                                 }
00949                         } while(stack.size()>0);
00950                 }
00951 }
00952 
00953 DBVT_PREFIX
00954 inline void             btDbvt::rayTestInternal(        const btDbvtNode* root,
00955                                                                 const btVector3& rayFrom,
00956                                                                 const btVector3& rayTo,
00957                                                                 const btVector3& rayDirectionInverse,
00958                                                                 unsigned int signs[3],
00959                                                                 btScalar lambda_max,
00960                                                                 const btVector3& aabbMin,
00961                                                                 const btVector3& aabbMax,
00962                                                                 DBVT_IPOLICY) const
00963 {
00964         (void) rayTo;
00965         DBVT_CHECKTYPE
00966         if(root)
00967         {
00968                 btVector3 resultNormal;
00969 
00970                 int                                                             depth=1;
00971                 int                                                             treshold=DOUBLE_STACKSIZE-2;
00972                 btAlignedObjectArray<const btDbvtNode*>&        stack = m_rayTestStack;
00973                 stack.resize(DOUBLE_STACKSIZE);
00974                 stack[0]=root;
00975                 btVector3 bounds[2];
00976                 do      
00977                 {
00978                         const btDbvtNode*       node=stack[--depth];
00979                         bounds[0] = node->volume.Mins()-aabbMax;
00980                         bounds[1] = node->volume.Maxs()-aabbMin;
00981                         btScalar tmin=1.f,lambda_min=0.f;
00982                         unsigned int result1=false;
00983                         result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
00984                         if(result1)
00985                         {
00986                                 if(node->isinternal())
00987                                 {
00988                                         if(depth>treshold)
00989                                         {
00990                                                 stack.resize(stack.size()*2);
00991                                                 treshold=stack.size()-2;
00992                                         }
00993                                         stack[depth++]=node->childs[0];
00994                                         stack[depth++]=node->childs[1];
00995                                 }
00996                                 else
00997                                 {
00998                                         policy.Process(node);
00999                                 }
01000                         }
01001                 } while(depth);
01002         }
01003 }
01004 
01005 //
01006 DBVT_PREFIX
01007 inline void             btDbvt::rayTest(        const btDbvtNode* root,
01008                                                                 const btVector3& rayFrom,
01009                                                                 const btVector3& rayTo,
01010                                                                 DBVT_IPOLICY)
01011 {
01012         DBVT_CHECKTYPE
01013                 if(root)
01014                 {
01015                         btVector3 rayDir = (rayTo-rayFrom);
01016                         rayDir.normalize ();
01017 
01019                         btVector3 rayDirectionInverse;
01020                         rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
01021                         rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
01022                         rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
01023                         unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0};
01024 
01025                         btScalar lambda_max = rayDir.dot(rayTo-rayFrom);
01026 
01027                         btVector3 resultNormal;
01028 
01029                         btAlignedObjectArray<const btDbvtNode*> stack;
01030 
01031                         int                                                             depth=1;
01032                         int                                                             treshold=DOUBLE_STACKSIZE-2;
01033 
01034                         stack.resize(DOUBLE_STACKSIZE);
01035                         stack[0]=root;
01036                         btVector3 bounds[2];
01037                         do      {
01038                                 const btDbvtNode*       node=stack[--depth];
01039 
01040                                 bounds[0] = node->volume.Mins();
01041                                 bounds[1] = node->volume.Maxs();
01042                                 
01043                                 btScalar tmin=1.f,lambda_min=0.f;
01044                                 unsigned int result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
01045 
01046 #ifdef COMPARE_BTRAY_AABB2
01047                                 btScalar param=1.f;
01048                                 bool result2 = btRayAabb(rayFrom,rayTo,node->volume.Mins(),node->volume.Maxs(),param,resultNormal);
01049                                 btAssert(result1 == result2);
01050 #endif //TEST_BTRAY_AABB2
01051 
01052                                 if(result1)
01053                                 {
01054                                         if(node->isinternal())
01055                                         {
01056                                                 if(depth>treshold)
01057                                                 {
01058                                                         stack.resize(stack.size()*2);
01059                                                         treshold=stack.size()-2;
01060                                                 }
01061                                                 stack[depth++]=node->childs[0];
01062                                                 stack[depth++]=node->childs[1];
01063                                         }
01064                                         else
01065                                         {
01066                                                 policy.Process(node);
01067                                         }
01068                                 }
01069                         } while(depth);
01070 
01071                 }
01072 }
01073 
01074 //
01075 DBVT_PREFIX
01076 inline void             btDbvt::collideKDOP(const btDbvtNode* root,
01077                                                                         const btVector3* normals,
01078                                                                         const btScalar* offsets,
01079                                                                         int count,
01080                                                                         DBVT_IPOLICY)
01081 {
01082         DBVT_CHECKTYPE
01083                 if(root)
01084                 {
01085                         const int                                               inside=(1<<count)-1;
01086                         btAlignedObjectArray<sStkNP>    stack;
01087                         int                                                             signs[sizeof(unsigned)*8];
01088                         btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
01089                         for(int i=0;i<count;++i)
01090                         {
01091                                 signs[i]=       ((normals[i].x()>=0)?1:0)+
01092                                         ((normals[i].y()>=0)?2:0)+
01093                                         ((normals[i].z()>=0)?4:0);
01094                         }
01095                         stack.reserve(SIMPLE_STACKSIZE);
01096                         stack.push_back(sStkNP(root,0));
01097                         do      {
01098                                 sStkNP  se=stack[stack.size()-1];
01099                                 bool    out=false;
01100                                 stack.pop_back();
01101                                 for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
01102                                 {
01103                                         if(0==(se.mask&j))
01104                                         {
01105                                                 const int       side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
01106                                                 switch(side)
01107                                                 {
01108                                                 case    -1:     out=true;break;
01109                                                 case    +1:     se.mask|=j;break;
01110                                                 }
01111                                         }
01112                                 }
01113                                 if(!out)
01114                                 {
01115                                         if((se.mask!=inside)&&(se.node->isinternal()))
01116                                         {
01117                                                 stack.push_back(sStkNP(se.node->childs[0],se.mask));
01118                                                 stack.push_back(sStkNP(se.node->childs[1],se.mask));
01119                                         }
01120                                         else
01121                                         {
01122                                                 if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy);
01123                                         }
01124                                 }
01125                         } while(stack.size());
01126                 }
01127 }
01128 
01129 //
01130 DBVT_PREFIX
01131 inline void             btDbvt::collideOCL(     const btDbvtNode* root,
01132                                                                    const btVector3* normals,
01133                                                                    const btScalar* offsets,
01134                                                                    const btVector3& sortaxis,
01135                                                                    int count,
01136                                                                    DBVT_IPOLICY,
01137                                                                    bool fsort)
01138 {
01139         DBVT_CHECKTYPE
01140                 if(root)
01141                 {
01142                         const unsigned                                  srtsgns=(sortaxis[0]>=0?1:0)+
01143                                 (sortaxis[1]>=0?2:0)+
01144                                 (sortaxis[2]>=0?4:0);
01145                         const int                                               inside=(1<<count)-1;
01146                         btAlignedObjectArray<sStkNPS>   stock;
01147                         btAlignedObjectArray<int>               ifree;
01148                         btAlignedObjectArray<int>               stack;
01149                         int                                                             signs[sizeof(unsigned)*8];
01150                         btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
01151                         for(int i=0;i<count;++i)
01152                         {
01153                                 signs[i]=       ((normals[i].x()>=0)?1:0)+
01154                                         ((normals[i].y()>=0)?2:0)+
01155                                         ((normals[i].z()>=0)?4:0);
01156                         }
01157                         stock.reserve(SIMPLE_STACKSIZE);
01158                         stack.reserve(SIMPLE_STACKSIZE);
01159                         ifree.reserve(SIMPLE_STACKSIZE);
01160                         stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns))));
01161                         do      {
01162                                 const int       id=stack[stack.size()-1];
01163                                 sStkNPS         se=stock[id];
01164                                 stack.pop_back();ifree.push_back(id);
01165                                 if(se.mask!=inside)
01166                                 {
01167                                         bool    out=false;
01168                                         for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
01169                                         {
01170                                                 if(0==(se.mask&j))
01171                                                 {
01172                                                         const int       side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
01173                                                         switch(side)
01174                                                         {
01175                                                         case    -1:     out=true;break;
01176                                                         case    +1:     se.mask|=j;break;
01177                                                         }
01178                                                 }
01179                                         }
01180                                         if(out) continue;
01181                                 }
01182                                 if(policy.Descent(se.node))
01183                                 {
01184                                         if(se.node->isinternal())
01185                                         {
01186                                                 const btDbvtNode* pns[]={       se.node->childs[0],se.node->childs[1]};
01187                                                 sStkNPS         nes[]={ sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)),
01188                                                         sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))};
01189                                                 const int       q=nes[0].value<nes[1].value?1:0;                                
01190                                                 int                     j=stack.size();
01191                                                 if(fsort&&(j>0))
01192                                                 {
01193                                                         /* Insert 0     */ 
01194                                                         j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size());
01195                                                         stack.push_back(0);
01196 #if DBVT_USE_MEMMOVE
01197                                                         memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
01198 #else
01199                                                         for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
01200 #endif
01201                                                         stack[j]=allocate(ifree,stock,nes[q]);
01202                                                         /* Insert 1     */ 
01203                                                         j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size());
01204                                                         stack.push_back(0);
01205 #if DBVT_USE_MEMMOVE
01206                                                         memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
01207 #else
01208                                                         for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
01209 #endif
01210                                                         stack[j]=allocate(ifree,stock,nes[1-q]);
01211                                                 }
01212                                                 else
01213                                                 {
01214                                                         stack.push_back(allocate(ifree,stock,nes[q]));
01215                                                         stack.push_back(allocate(ifree,stock,nes[1-q]));
01216                                                 }
01217                                         }
01218                                         else
01219                                         {
01220                                                 policy.Process(se.node,se.value);
01221                                         }
01222                                 }
01223                         } while(stack.size());
01224                 }
01225 }
01226 
01227 //
01228 DBVT_PREFIX
01229 inline void             btDbvt::collideTU(      const btDbvtNode* root,
01230                                                                   DBVT_IPOLICY)
01231 {
01232         DBVT_CHECKTYPE
01233                 if(root)
01234                 {
01235                         btAlignedObjectArray<const btDbvtNode*> stack;
01236                         stack.reserve(SIMPLE_STACKSIZE);
01237                         stack.push_back(root);
01238                         do      {
01239                                 const btDbvtNode*       n=stack[stack.size()-1];
01240                                 stack.pop_back();
01241                                 if(policy.Descent(n))
01242                                 {
01243                                         if(n->isinternal())
01244                                         { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); }
01245                                         else
01246                                         { policy.Process(n); }
01247                                 }
01248                         } while(stack.size()>0);
01249                 }
01250 }
01251 
01252 //
01253 // PP Cleanup
01254 //
01255 
01256 #undef DBVT_USE_MEMMOVE
01257 #undef DBVT_USE_TEMPLATE
01258 #undef DBVT_VIRTUAL_DTOR
01259 #undef DBVT_VIRTUAL
01260 #undef DBVT_PREFIX
01261 #undef DBVT_IPOLICY
01262 #undef DBVT_CHECKTYPE
01263 #undef DBVT_IMPL_GENERIC
01264 #undef DBVT_IMPL_SSE
01265 #undef DBVT_USE_INTRINSIC_SSE
01266 #undef DBVT_SELECT_IMPL
01267 #undef DBVT_MERGE_IMPL
01268 #undef DBVT_INT0_IMPL
01269 
01270 #endif