dali-physics/third-party/bullet3/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h

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