extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15 ///btDbvt implementation by Nathanael Presson
  16
  17 #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                 */
 130 struct  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);
 163 private:
 164         DBVT_INLINE void                                AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const;
 165 private:
 166         btVector3       mi,mx;
 167 };
 168
 169 // Types
 170 typedef btDbvtAabbMm    btDbvtVolume;
 171
 172 /* btDbvtNode                           */
 173 struct  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.
 190 struct  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         //
 383 private:
 384         btDbvt(const btDbvt&)   {}
 385 };
 386
 387 //
 388 // Inline's
 389 //
 390
 391 //
 392 inline 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 //
 400 inline btDbvtAabbMm                     btDbvtAabbMm::FromCR(const btVector3& c,btScalar r)
 401 {
 402         return(FromCE(c,btVector3(r,r,r)));
 403 }
 404
 405 //
 406 inline 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 //
 414 inline 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 //
 427 inline 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 //
 440 DBVT_INLINE void                btDbvtAabbMm::Expand(const btVector3& e)
 441 {
 442         mi-=e;mx+=e;
 443 }
 444
 445 //
 446 DBVT_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 //
 454 DBVT_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 //
 465 DBVT_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 //
 493 DBVT_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 //
 503 DBVT_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 //
 515 DBVT_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 //
 536 DBVT_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 //
 555 DBVT_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 //
 565 DBVT_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 //
 639 DBVT_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 //
 662 DBVT_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 //
 678 DBVT_PREFIX
 679 inline 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 //
 692 DBVT_PREFIX
 693 inline 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 //
 709 DBVT_PREFIX
 710 inline 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
 774 DBVT_PREFIX
 775 inline 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 //
 839 DBVT_PREFIX
 840 inline 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 //
 894 DBVT_PREFIX
 895 inline 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 //
 907 DBVT_PREFIX
 908 inline 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
 939 DBVT_PREFIX
 940 inline 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 //
 992 DBVT_PREFIX
 993 inline 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 //
1061 DBVT_PREFIX
1062 inline 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 //
1116 DBVT_PREFIX
1117 inline 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 //
1214 DBVT_PREFIX
1215 inline 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