2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2007 Erwin Coumans https://bulletphysics.org
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:
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.
15 ///btDbvt implementation by Nathanael Presson
17 #ifndef BT_DYNAMIC_BOUNDING_VOLUME_TREE_H
18 #define BT_DYNAMIC_BOUNDING_VOLUME_TREE_H
20 #include "LinearMath/btAlignedObjectArray.h"
21 #include "LinearMath/btVector3.h"
22 #include "LinearMath/btTransform.h"
23 #include "LinearMath/btAabbUtil2.h"
25 // Compile time configuration
28 // Implementation profiles
29 #define DBVT_IMPL_GENERIC 0 // Generic implementation
30 #define DBVT_IMPL_SSE 1 // SSE
32 // Template implementation of ICollide
34 #if (defined(_MSC_VER) && _MSC_VER >= 1400)
35 #define DBVT_USE_TEMPLATE 1
37 #define DBVT_USE_TEMPLATE 0
40 #define DBVT_USE_TEMPLATE 0
43 // Use only intrinsics instead of inline asm
44 #define DBVT_USE_INTRINSIC_SSE 1
46 // Using memmov for collideOCL
47 #define DBVT_USE_MEMMOVE 1
49 // Enable benchmarking code
50 #define DBVT_ENABLE_BENCHMARK 0
53 #define DBVT_INLINE SIMD_FORCE_INLINE
55 // Specific methods implementation
57 //SSE gives errors on a MSVC 7.1
58 #if defined(BT_USE_SSE) //&& defined (_WIN32)
59 #define DBVT_SELECT_IMPL DBVT_IMPL_SSE
60 #define DBVT_MERGE_IMPL DBVT_IMPL_SSE
61 #define DBVT_INT0_IMPL DBVT_IMPL_SSE
63 #define DBVT_SELECT_IMPL DBVT_IMPL_GENERIC
64 #define DBVT_MERGE_IMPL DBVT_IMPL_GENERIC
65 #define DBVT_INT0_IMPL DBVT_IMPL_GENERIC
68 #if (DBVT_SELECT_IMPL == DBVT_IMPL_SSE) || \
69 (DBVT_MERGE_IMPL == DBVT_IMPL_SSE) || \
70 (DBVT_INT0_IMPL == DBVT_IMPL_SSE)
71 #include <emmintrin.h>
75 // Auto config and checks
80 #define DBVT_VIRTUAL_DTOR(a)
81 #define DBVT_PREFIX template <typename T>
82 #define DBVT_IPOLICY T& policy
83 #define DBVT_CHECKTYPE \
84 static const ICollide& typechecker = *(T*)1; \
87 #define DBVT_VIRTUAL_DTOR(a) \
89 #define DBVT_VIRTUAL virtual
91 #define DBVT_IPOLICY ICollide& policy
92 #define DBVT_CHECKTYPE
96 #if !defined(__CELLOS_LV2__) && !defined(__MWERKS__)
102 #ifndef DBVT_USE_TEMPLATE
103 #error "DBVT_USE_TEMPLATE undefined"
106 #ifndef DBVT_USE_MEMMOVE
107 #error "DBVT_USE_MEMMOVE undefined"
110 #ifndef DBVT_ENABLE_BENCHMARK
111 #error "DBVT_ENABLE_BENCHMARK undefined"
114 #ifndef DBVT_SELECT_IMPL
115 #error "DBVT_SELECT_IMPL undefined"
118 #ifndef DBVT_MERGE_IMPL
119 #error "DBVT_MERGE_IMPL undefined"
122 #ifndef DBVT_INT0_IMPL
123 #error "DBVT_INT0_IMPL undefined"
133 DBVT_INLINE btDbvtAabbMm(){}
134 DBVT_INLINE btVector3 Center() const { return ((mi + mx) / 2); }
135 DBVT_INLINE btVector3 Lengths() const { return (mx - mi); }
136 DBVT_INLINE btVector3 Extents() const { return ((mx - mi) / 2); }
137 DBVT_INLINE const btVector3& Mins() const { return (mi); }
138 DBVT_INLINE const btVector3& Maxs() const { return (mx); }
139 static inline btDbvtAabbMm FromCE(const btVector3& c, const btVector3& e);
140 static inline btDbvtAabbMm FromCR(const btVector3& c, btScalar r);
141 static inline btDbvtAabbMm FromMM(const btVector3& mi, const btVector3& mx);
142 static inline btDbvtAabbMm FromPoints(const btVector3* pts, int n);
143 static inline btDbvtAabbMm FromPoints(const btVector3** ppts, int n);
144 DBVT_INLINE void Expand(const btVector3& e);
145 DBVT_INLINE void SignedExpand(const btVector3& e);
146 DBVT_INLINE bool Contain(const btDbvtAabbMm& a) const;
147 DBVT_INLINE int Classify(const btVector3& n, btScalar o, int s) const;
148 DBVT_INLINE btScalar ProjectMinimum(const btVector3& v, unsigned signs) const;
149 DBVT_INLINE friend bool Intersect(const btDbvtAabbMm& a,
150 const btDbvtAabbMm& b);
152 DBVT_INLINE friend bool Intersect(const btDbvtAabbMm& a,
155 DBVT_INLINE friend btScalar Proximity(const btDbvtAabbMm& a,
156 const btDbvtAabbMm& b);
157 DBVT_INLINE friend int Select(const btDbvtAabbMm& o,
158 const btDbvtAabbMm& a,
159 const btDbvtAabbMm& b);
160 DBVT_INLINE friend void Merge(const btDbvtAabbMm& a,
161 const btDbvtAabbMm& b,
163 DBVT_INLINE friend bool NotEqual(const btDbvtAabbMm& a,
164 const btDbvtAabbMm& b);
166 DBVT_INLINE btVector3& tMins() { return (mi); }
167 DBVT_INLINE btVector3& tMaxs() { return (mx); }
170 DBVT_INLINE void AddSpan(const btVector3& d, btScalar& smi, btScalar& smx) const;
177 typedef btDbvtAabbMm btDbvtVolume;
184 DBVT_INLINE bool isleaf() const { return (childs[1] == 0); }
185 DBVT_INLINE bool isinternal() const { return (!isleaf()); }
187 btDbvtNode* childs[2];
193 /* btDbv(normal)tNode */
199 DBVT_INLINE bool isleaf() const { return (childs[1] == 0); }
200 DBVT_INLINE bool isinternal() const { return (!isleaf()); }
201 btDbvntNode* childs[2];
204 btDbvntNode(const btDbvtNode* n)
223 typedef btAlignedObjectArray<const btDbvtNode*> btNodeStack;
225 ///The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree).
226 ///This btDbvt is used for soft body collision detection and for the btDbvtBroadphase. It has a fast insert, remove and update of nodes.
227 ///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure.
236 sStkNN(const btDbvtNode* na, const btDbvtNode* nb) : a(na), b(nb) {}
240 const btDbvtNode* node;
242 sStkNP(const btDbvtNode* n, unsigned m) : node(n), mask(m) {}
246 const btDbvtNode* node;
250 sStkNPS(const btDbvtNode* n, unsigned m, btScalar v) : node(n), mask(m), value(v) {}
254 const btDbvtNode* node;
256 sStkCLN(const btDbvtNode* n, btDbvtNode* p) : node(n), parent(p) {}
261 const btDbvntNode* a;
262 const btDbvntNode* b;
264 sStknNN(const btDbvntNode* na, const btDbvntNode* nb) : a(na), b(nb) {}
266 // Policies/Interfaces
271 DBVT_VIRTUAL_DTOR(ICollide)
272 DBVT_VIRTUAL void Process(const btDbvtNode*, const btDbvtNode*) {}
273 DBVT_VIRTUAL void Process(const btDbvtNode*) {}
274 DBVT_VIRTUAL void Process(const btDbvtNode* n, btScalar) { Process(n); }
275 DBVT_VIRTUAL void Process(const btDbvntNode*, const btDbvntNode*) {}
276 DBVT_VIRTUAL bool Descent(const btDbvtNode*) { return (true); }
277 DBVT_VIRTUAL bool AllLeaves(const btDbvtNode*) { return (true); }
282 virtual ~IWriter() {}
283 virtual void Prepare(const btDbvtNode* root, int numnodes) = 0;
284 virtual void WriteNode(const btDbvtNode*, int index, int parent, int child0, int child1) = 0;
285 virtual void WriteLeaf(const btDbvtNode*, int index, int parent) = 0;
291 virtual void CloneLeaf(btDbvtNode*) {}
297 SIMPLE_STACKSIZE = 64,
298 DOUBLE_STACKSIZE = SIMPLE_STACKSIZE * 2
308 btAlignedObjectArray<sStkNN> m_stkStack;
314 bool empty() const { return (0 == m_root); }
315 void optimizeBottomUp();
316 void optimizeTopDown(int bu_treshold = 128);
317 void optimizeIncremental(int passes);
318 btDbvtNode* insert(const btDbvtVolume& box, void* data);
319 void update(btDbvtNode* leaf, int lookahead = -1);
320 void update(btDbvtNode* leaf, btDbvtVolume& volume);
321 bool update(btDbvtNode* leaf, btDbvtVolume& volume, const btVector3& velocity, btScalar margin);
322 bool update(btDbvtNode* leaf, btDbvtVolume& volume, const btVector3& velocity);
323 bool update(btDbvtNode* leaf, btDbvtVolume& volume, btScalar margin);
324 void remove(btDbvtNode* leaf);
325 void write(IWriter* iwriter) const;
326 void clone(btDbvt& dest, IClone* iclone = 0) const;
327 static int maxdepth(const btDbvtNode* node);
328 static int countLeaves(const btDbvtNode* node);
329 static void extractLeaves(const btDbvtNode* node, btAlignedObjectArray<const btDbvtNode*>& leaves);
330 #if DBVT_ENABLE_BENCHMARK
331 static void benchmark();
333 static void benchmark()
337 // DBVT_IPOLICY must support ICollide policy/interface
339 static void enumNodes(const btDbvtNode* root,
342 static void enumLeaves(const btDbvtNode* root,
345 void collideTT(const btDbvtNode* root0,
346 const btDbvtNode* root1,
349 void selfCollideT(const btDbvntNode* root,
352 void selfCollideTT(const btDbvtNode* root,
356 void collideTTpersistentStack(const btDbvtNode* root0,
357 const btDbvtNode* root1,
361 void collideTT( const btDbvtNode* root0,
362 const btDbvtNode* root1,
363 const btTransform& xform,
366 void collideTT( const btDbvtNode* root0,
367 const btTransform& xform0,
368 const btDbvtNode* root1,
369 const btTransform& xform1,
374 void collideTV(const btDbvtNode* root,
375 const btDbvtVolume& volume,
379 void collideTVNoStackAlloc(const btDbvtNode* root,
380 const btDbvtVolume& volume,
384 ///rayTest is a re-entrant ray test, and can be called in parallel as long as the btAlignedAlloc is thread-safe (uses locking etc)
385 ///rayTest is slower than rayTestInternal, because it builds a local stack, using memory allocations, and it recomputes signs/rayDirectionInverses each time
387 static void rayTest(const btDbvtNode* root,
388 const btVector3& rayFrom,
389 const btVector3& rayTo,
391 ///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
392 ///rayTestInternal is used by btDbvtBroadphase to accelerate world ray casts
394 void rayTestInternal(const btDbvtNode* root,
395 const btVector3& rayFrom,
396 const btVector3& rayTo,
397 const btVector3& rayDirectionInverse,
398 unsigned int signs[3],
400 const btVector3& aabbMin,
401 const btVector3& aabbMax,
402 btAlignedObjectArray<const btDbvtNode*>& stack,
406 static void collideKDOP(const btDbvtNode* root,
407 const btVector3* normals,
408 const btScalar* offsets,
412 static void collideOCL(const btDbvtNode* root,
413 const btVector3* normals,
414 const btScalar* offsets,
415 const btVector3& sortaxis,
418 bool fullsort = true);
420 static void collideTU(const btDbvtNode* root,
423 static DBVT_INLINE int nearest(const int* i, const btDbvt::sStkNPS* a, btScalar v, int l, int h)
429 if (a[i[m]].value >= v)
436 static DBVT_INLINE int allocate(btAlignedObjectArray<int>& ifree,
437 btAlignedObjectArray<sStkNPS>& stock,
438 const sStkNPS& value)
441 if (ifree.size() > 0)
443 i = ifree[ifree.size() - 1];
450 stock.push_back(value);
456 btDbvt(const btDbvt&) {}
464 inline btDbvtAabbMm btDbvtAabbMm::FromCE(const btVector3& c, const btVector3& e)
473 inline btDbvtAabbMm btDbvtAabbMm::FromCR(const btVector3& c, btScalar r)
475 return (FromCE(c, btVector3(r, r, r)));
479 inline btDbvtAabbMm btDbvtAabbMm::FromMM(const btVector3& mi, const btVector3& mx)
488 inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3* pts, int n)
491 box.mi = box.mx = pts[0];
492 for (int i = 1; i < n; ++i)
494 box.mi.setMin(pts[i]);
495 box.mx.setMax(pts[i]);
501 inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3** ppts, int n)
504 box.mi = box.mx = *ppts[0];
505 for (int i = 1; i < n; ++i)
507 box.mi.setMin(*ppts[i]);
508 box.mx.setMax(*ppts[i]);
514 DBVT_INLINE void btDbvtAabbMm::Expand(const btVector3& e)
521 DBVT_INLINE void btDbvtAabbMm::SignedExpand(const btVector3& e)
524 mx.setX(mx.x() + e[0]);
526 mi.setX(mi.x() + e[0]);
528 mx.setY(mx.y() + e[1]);
530 mi.setY(mi.y() + e[1]);
532 mx.setZ(mx.z() + e[2]);
534 mi.setZ(mi.z() + e[2]);
538 DBVT_INLINE bool btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const
540 return ((mi.x() <= a.mi.x()) &&
541 (mi.y() <= a.mi.y()) &&
542 (mi.z() <= a.mi.z()) &&
543 (mx.x() >= a.mx.x()) &&
544 (mx.y() >= a.mx.y()) &&
545 (mx.z() >= a.mx.z()));
549 DBVT_INLINE int btDbvtAabbMm::Classify(const btVector3& n, btScalar o, int s) const
555 px = btVector3(mi.x(), mi.y(), mi.z());
556 pi = btVector3(mx.x(), mx.y(), mx.z());
559 px = btVector3(mx.x(), mi.y(), mi.z());
560 pi = btVector3(mi.x(), mx.y(), mx.z());
563 px = btVector3(mi.x(), mx.y(), mi.z());
564 pi = btVector3(mx.x(), mi.y(), mx.z());
567 px = btVector3(mx.x(), mx.y(), mi.z());
568 pi = btVector3(mi.x(), mi.y(), mx.z());
571 px = btVector3(mi.x(), mi.y(), mx.z());
572 pi = btVector3(mx.x(), mx.y(), mi.z());
575 px = btVector3(mx.x(), mi.y(), mx.z());
576 pi = btVector3(mi.x(), mx.y(), mi.z());
579 px = btVector3(mi.x(), mx.y(), mx.z());
580 pi = btVector3(mx.x(), mi.y(), mi.z());
583 px = btVector3(mx.x(), mx.y(), mx.z());
584 pi = btVector3(mi.x(), mi.y(), mi.z());
587 if ((btDot(n, px) + o) < 0) return (-1);
588 if ((btDot(n, pi) + o) >= 0) return (+1);
593 DBVT_INLINE btScalar btDbvtAabbMm::ProjectMinimum(const btVector3& v, unsigned signs) const
595 const btVector3* b[] = {&mx, &mi};
596 const btVector3 p(b[(signs >> 0) & 1]->x(),
597 b[(signs >> 1) & 1]->y(),
598 b[(signs >> 2) & 1]->z());
599 return (btDot(p, v));
603 DBVT_INLINE void btDbvtAabbMm::AddSpan(const btVector3& d, btScalar& smi, btScalar& smx) const
605 for (int i = 0; i < 3; ++i)
621 DBVT_INLINE bool Intersect(const btDbvtAabbMm& a,
622 const btDbvtAabbMm& b)
624 #if DBVT_INT0_IMPL == DBVT_IMPL_SSE
625 const __m128 rt(_mm_or_ps(_mm_cmplt_ps(_mm_load_ps(b.mx), _mm_load_ps(a.mi)),
626 _mm_cmplt_ps(_mm_load_ps(a.mx), _mm_load_ps(b.mi))));
628 const __int32* pu((const __int32*)&rt);
630 const int* pu((const int*)&rt);
632 return ((pu[0] | pu[1] | pu[2]) == 0);
634 return ((a.mi.x() <= b.mx.x()) &&
635 (a.mx.x() >= b.mi.x()) &&
636 (a.mi.y() <= b.mx.y()) &&
637 (a.mx.y() >= b.mi.y()) &&
638 (a.mi.z() <= b.mx.z()) &&
639 (a.mx.z() >= b.mi.z()));
644 DBVT_INLINE bool Intersect(const btDbvtAabbMm& a,
647 return ((b.x() >= a.mi.x()) &&
648 (b.y() >= a.mi.y()) &&
649 (b.z() >= a.mi.z()) &&
650 (b.x() <= a.mx.x()) &&
651 (b.y() <= a.mx.y()) &&
652 (b.z() <= a.mx.z()));
655 //////////////////////////////////////
658 DBVT_INLINE btScalar Proximity(const btDbvtAabbMm& a,
659 const btDbvtAabbMm& b)
661 const btVector3 d = (a.mi + a.mx) - (b.mi + b.mx);
662 return (btFabs(d.x()) + btFabs(d.y()) + btFabs(d.z()));
666 DBVT_INLINE int Select(const btDbvtAabbMm& o,
667 const btDbvtAabbMm& a,
668 const btDbvtAabbMm& b)
670 #if DBVT_SELECT_IMPL == DBVT_IMPL_SSE
673 static ATTRIBUTE_ALIGNED16(const unsigned __int32) mask[] = {0x7fffffff, 0x7fffffff, 0x7fffffff, 0x7fffffff};
675 static ATTRIBUTE_ALIGNED16(const unsigned int) mask[] = {0x7fffffff, 0x7fffffff, 0x7fffffff, 0x00000000 /*0x7fffffff*/};
677 ///@todo: the intrinsic version is 11% slower
678 #if DBVT_USE_INTRINSIC_SSE
680 union btSSEUnion ///NOTE: if we use more intrinsics, move btSSEUnion into the LinearMath directory
687 __m128 omi(_mm_load_ps(o.mi));
688 omi = _mm_add_ps(omi, _mm_load_ps(o.mx));
689 __m128 ami(_mm_load_ps(a.mi));
690 ami = _mm_add_ps(ami, _mm_load_ps(a.mx));
691 ami = _mm_sub_ps(ami, omi);
692 ami = _mm_and_ps(ami, _mm_load_ps((const float*)mask));
693 __m128 bmi(_mm_load_ps(b.mi));
694 bmi = _mm_add_ps(bmi, _mm_load_ps(b.mx));
695 bmi = _mm_sub_ps(bmi, omi);
696 bmi = _mm_and_ps(bmi, _mm_load_ps((const float*)mask));
697 __m128 t0(_mm_movehl_ps(ami, ami));
698 ami = _mm_add_ps(ami, t0);
699 ami = _mm_add_ss(ami, _mm_shuffle_ps(ami, ami, 1));
700 __m128 t1(_mm_movehl_ps(bmi, bmi));
701 bmi = _mm_add_ps(bmi, t1);
702 bmi = _mm_add_ss(bmi, _mm_shuffle_ps(bmi, bmi, 1));
705 tmp.ssereg = _mm_cmple_ss(bmi, ami);
706 return tmp.ints[0] & 1;
709 ATTRIBUTE_ALIGNED16(__int32 r[1]);
740 return (Proximity(o, a) < Proximity(o, b) ? 0 : 1);
745 DBVT_INLINE void Merge(const btDbvtAabbMm& a,
746 const btDbvtAabbMm& b,
749 #if DBVT_MERGE_IMPL == DBVT_IMPL_SSE
750 __m128 ami(_mm_load_ps(a.mi));
751 __m128 amx(_mm_load_ps(a.mx));
752 __m128 bmi(_mm_load_ps(b.mi));
753 __m128 bmx(_mm_load_ps(b.mx));
754 ami = _mm_min_ps(ami, bmi);
755 amx = _mm_max_ps(amx, bmx);
756 _mm_store_ps(r.mi, ami);
757 _mm_store_ps(r.mx, amx);
759 for (int i = 0; i < 3; ++i)
761 if (a.mi[i] < b.mi[i])
765 if (a.mx[i] > b.mx[i])
774 DBVT_INLINE bool NotEqual(const btDbvtAabbMm& a,
775 const btDbvtAabbMm& b)
777 return ((a.mi.x() != b.mi.x()) ||
778 (a.mi.y() != b.mi.y()) ||
779 (a.mi.z() != b.mi.z()) ||
780 (a.mx.x() != b.mx.x()) ||
781 (a.mx.y() != b.mx.y()) ||
782 (a.mx.z() != b.mx.z()));
791 inline void btDbvt::enumNodes(const btDbvtNode* root,
795 policy.Process(root);
796 if (root->isinternal())
798 enumNodes(root->childs[0], policy);
799 enumNodes(root->childs[1], policy);
805 inline void btDbvt::enumLeaves(const btDbvtNode* root,
809 if (root->isinternal())
811 enumLeaves(root->childs[0], policy);
812 enumLeaves(root->childs[1], policy);
816 policy.Process(root);
822 inline void btDbvt::collideTT(const btDbvtNode* root0,
823 const btDbvtNode* root1,
830 int treshold = DOUBLE_STACKSIZE - 4;
831 btAlignedObjectArray<sStkNN> stkStack;
832 stkStack.resize(DOUBLE_STACKSIZE);
833 stkStack[0] = sStkNN(root0, root1);
836 sStkNN p = stkStack[--depth];
837 if (depth > treshold)
839 stkStack.resize(stkStack.size() * 2);
840 treshold = stkStack.size() - 4;
844 if (p.a->isinternal())
846 stkStack[depth++] = sStkNN(p.a->childs[0], p.a->childs[0]);
847 stkStack[depth++] = sStkNN(p.a->childs[1], p.a->childs[1]);
848 stkStack[depth++] = sStkNN(p.a->childs[0], p.a->childs[1]);
851 else if (Intersect(p.a->volume, p.b->volume))
853 if (p.a->isinternal())
855 if (p.b->isinternal())
857 stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[0]);
858 stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[0]);
859 stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[1]);
860 stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[1]);
864 stkStack[depth++] = sStkNN(p.a->childs[0], p.b);
865 stkStack[depth++] = sStkNN(p.a->childs[1], p.b);
870 if (p.b->isinternal())
872 stkStack[depth++] = sStkNN(p.a, p.b->childs[0]);
873 stkStack[depth++] = sStkNN(p.a, p.b->childs[1]);
877 policy.Process(p.a, p.b);
887 inline void btDbvt::selfCollideT(const btDbvntNode* root,
894 int treshold = DOUBLE_STACKSIZE - 4;
895 btAlignedObjectArray<sStknNN> stkStack;
896 stkStack.resize(DOUBLE_STACKSIZE);
897 stkStack[0] = sStknNN(root, root);
900 sStknNN p = stkStack[--depth];
901 if (depth > treshold)
903 stkStack.resize(stkStack.size() * 2);
904 treshold = stkStack.size() - 4;
908 if (p.a->isinternal() && p.a->angle > SIMD_PI)
910 stkStack[depth++] = sStknNN(p.a->childs[0], p.a->childs[0]);
911 stkStack[depth++] = sStknNN(p.a->childs[1], p.a->childs[1]);
912 stkStack[depth++] = sStknNN(p.a->childs[0], p.a->childs[1]);
915 else if (Intersect(p.a->volume, p.b->volume))
917 if (p.a->isinternal())
919 if (p.b->isinternal())
921 stkStack[depth++] = sStknNN(p.a->childs[0], p.b->childs[0]);
922 stkStack[depth++] = sStknNN(p.a->childs[1], p.b->childs[0]);
923 stkStack[depth++] = sStknNN(p.a->childs[0], p.b->childs[1]);
924 stkStack[depth++] = sStknNN(p.a->childs[1], p.b->childs[1]);
928 stkStack[depth++] = sStknNN(p.a->childs[0], p.b);
929 stkStack[depth++] = sStknNN(p.a->childs[1], p.b);
934 if (p.b->isinternal())
936 stkStack[depth++] = sStknNN(p.a, p.b->childs[0]);
937 stkStack[depth++] = sStknNN(p.a, p.b->childs[1]);
941 policy.Process(p.a, p.b);
951 inline void btDbvt::selfCollideTT(const btDbvtNode* root,
958 int treshold = DOUBLE_STACKSIZE - 4;
959 btAlignedObjectArray<sStkNN> stkStack;
960 stkStack.resize(DOUBLE_STACKSIZE);
961 stkStack[0] = sStkNN(root, root);
964 sStkNN p = stkStack[--depth];
965 if (depth > treshold)
967 stkStack.resize(stkStack.size() * 2);
968 treshold = stkStack.size() - 4;
972 if (p.a->isinternal())
974 stkStack[depth++] = sStkNN(p.a->childs[0], p.a->childs[0]);
975 stkStack[depth++] = sStkNN(p.a->childs[1], p.a->childs[1]);
976 stkStack[depth++] = sStkNN(p.a->childs[0], p.a->childs[1]);
979 else if (Intersect(p.a->volume, p.b->volume))
981 if (p.a->isinternal())
983 if (p.b->isinternal())
985 stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[0]);
986 stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[0]);
987 stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[1]);
988 stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[1]);
992 stkStack[depth++] = sStkNN(p.a->childs[0], p.b);
993 stkStack[depth++] = sStkNN(p.a->childs[1], p.b);
998 if (p.b->isinternal())
1000 stkStack[depth++] = sStkNN(p.a, p.b->childs[0]);
1001 stkStack[depth++] = sStkNN(p.a, p.b->childs[1]);
1005 policy.Process(p.a, p.b);
1015 inline void btDbvt::collideTTpersistentStack(const btDbvtNode* root0,
1016 const btDbvtNode* root1,
1023 int treshold = DOUBLE_STACKSIZE - 4;
1025 m_stkStack.resize(DOUBLE_STACKSIZE);
1026 m_stkStack[0] = sStkNN(root0, root1);
1029 sStkNN p = m_stkStack[--depth];
1030 if (depth > treshold)
1032 m_stkStack.resize(m_stkStack.size() * 2);
1033 treshold = m_stkStack.size() - 4;
1037 if (p.a->isinternal())
1039 m_stkStack[depth++] = sStkNN(p.a->childs[0], p.a->childs[0]);
1040 m_stkStack[depth++] = sStkNN(p.a->childs[1], p.a->childs[1]);
1041 m_stkStack[depth++] = sStkNN(p.a->childs[0], p.a->childs[1]);
1044 else if (Intersect(p.a->volume, p.b->volume))
1046 if (p.a->isinternal())
1048 if (p.b->isinternal())
1050 m_stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[0]);
1051 m_stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[0]);
1052 m_stkStack[depth++] = sStkNN(p.a->childs[0], p.b->childs[1]);
1053 m_stkStack[depth++] = sStkNN(p.a->childs[1], p.b->childs[1]);
1057 m_stkStack[depth++] = sStkNN(p.a->childs[0], p.b);
1058 m_stkStack[depth++] = sStkNN(p.a->childs[1], p.b);
1063 if (p.b->isinternal())
1065 m_stkStack[depth++] = sStkNN(p.a, p.b->childs[0]);
1066 m_stkStack[depth++] = sStkNN(p.a, p.b->childs[1]);
1070 policy.Process(p.a, p.b);
1081 inline void btDbvt::collideTT( const btDbvtNode* root0,
1082 const btDbvtNode* root1,
1083 const btTransform& xform,
1090 int treshold=DOUBLE_STACKSIZE-4;
1091 btAlignedObjectArray<sStkNN> stkStack;
1092 stkStack.resize(DOUBLE_STACKSIZE);
1093 stkStack[0]=sStkNN(root0,root1);
1095 sStkNN p=stkStack[--depth];
1096 if(Intersect(p.a->volume,p.b->volume,xform))
1100 stkStack.resize(stkStack.size()*2);
1101 treshold=stkStack.size()-4;
1103 if(p.a->isinternal())
1105 if(p.b->isinternal())
1107 stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
1108 stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
1109 stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
1110 stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
1114 stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
1115 stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
1120 if(p.b->isinternal())
1122 stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
1123 stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
1127 policy.Process(p.a,p.b);
1136 inline void btDbvt::collideTT( const btDbvtNode* root0,
1137 const btTransform& xform0,
1138 const btDbvtNode* root1,
1139 const btTransform& xform1,
1142 const btTransform xform=xform0.inverse()*xform1;
1143 collideTT(root0,root1,xform,policy);
1148 inline void btDbvt::collideTV(const btDbvtNode* root,
1149 const btDbvtVolume& vol,
1155 ATTRIBUTE_ALIGNED16(btDbvtVolume)
1157 btAlignedObjectArray<const btDbvtNode*> stack;
1159 #ifndef BT_DISABLE_STACK_TEMP_MEMORY
1160 char tempmemory[SIMPLE_STACKSIZE * sizeof(const btDbvtNode*)];
1161 stack.initializeFromBuffer(tempmemory, 0, SIMPLE_STACKSIZE);
1163 stack.reserve(SIMPLE_STACKSIZE);
1164 #endif //BT_DISABLE_STACK_TEMP_MEMORY
1166 stack.push_back(root);
1169 const btDbvtNode* n = stack[stack.size() - 1];
1171 if (Intersect(n->volume, volume))
1173 if (n->isinternal())
1175 stack.push_back(n->childs[0]);
1176 stack.push_back(n->childs[1]);
1183 } while (stack.size() > 0);
1189 inline void btDbvt::collideTVNoStackAlloc(const btDbvtNode* root,
1190 const btDbvtVolume& vol,
1197 ATTRIBUTE_ALIGNED16(btDbvtVolume)
1200 stack.reserve(SIMPLE_STACKSIZE);
1201 stack.push_back(root);
1204 const btDbvtNode* n = stack[stack.size() - 1];
1206 if (Intersect(n->volume, volume))
1208 if (n->isinternal())
1210 stack.push_back(n->childs[0]);
1211 stack.push_back(n->childs[1]);
1218 } while (stack.size() > 0);
1223 inline void btDbvt::rayTestInternal(const btDbvtNode* root,
1224 const btVector3& rayFrom,
1225 const btVector3& rayTo,
1226 const btVector3& rayDirectionInverse,
1227 unsigned int signs[3],
1228 btScalar lambda_max,
1229 const btVector3& aabbMin,
1230 const btVector3& aabbMax,
1231 btAlignedObjectArray<const btDbvtNode*>& stack,
1238 btVector3 resultNormal;
1241 int treshold = DOUBLE_STACKSIZE - 2;
1242 stack.resize(DOUBLE_STACKSIZE);
1244 btVector3 bounds[2];
1247 const btDbvtNode* node = stack[--depth];
1248 bounds[0] = node->volume.Mins() - aabbMax;
1249 bounds[1] = node->volume.Maxs() - aabbMin;
1250 btScalar tmin = 1.f, lambda_min = 0.f;
1251 unsigned int result1 = false;
1252 result1 = btRayAabb2(rayFrom, rayDirectionInverse, signs, bounds, tmin, lambda_min, lambda_max);
1255 if (node->isinternal())
1257 if (depth > treshold)
1259 stack.resize(stack.size() * 2);
1260 treshold = stack.size() - 2;
1262 stack[depth++] = node->childs[0];
1263 stack[depth++] = node->childs[1];
1267 policy.Process(node);
1276 inline void btDbvt::rayTest(const btDbvtNode* root,
1277 const btVector3& rayFrom,
1278 const btVector3& rayTo,
1284 btVector3 rayDir = (rayTo - rayFrom);
1287 ///what about division by zero? --> just set rayDirection[i] to INF/BT_LARGE_FLOAT
1288 btVector3 rayDirectionInverse;
1289 rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
1290 rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
1291 rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
1292 unsigned int signs[3] = {rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0};
1294 btScalar lambda_max = rayDir.dot(rayTo - rayFrom);
1296 btVector3 resultNormal;
1298 btAlignedObjectArray<const btDbvtNode*> stack;
1301 int treshold = DOUBLE_STACKSIZE - 2;
1303 char tempmemory[DOUBLE_STACKSIZE * sizeof(const btDbvtNode*)];
1304 #ifndef BT_DISABLE_STACK_TEMP_MEMORY
1305 stack.initializeFromBuffer(tempmemory, DOUBLE_STACKSIZE, DOUBLE_STACKSIZE);
1306 #else //BT_DISABLE_STACK_TEMP_MEMORY
1307 stack.resize(DOUBLE_STACKSIZE);
1308 #endif //BT_DISABLE_STACK_TEMP_MEMORY
1310 btVector3 bounds[2];
1313 const btDbvtNode* node = stack[--depth];
1315 bounds[0] = node->volume.Mins();
1316 bounds[1] = node->volume.Maxs();
1318 btScalar tmin = 1.f, lambda_min = 0.f;
1319 unsigned int result1 = btRayAabb2(rayFrom, rayDirectionInverse, signs, bounds, tmin, lambda_min, lambda_max);
1321 #ifdef COMPARE_BTRAY_AABB2
1322 btScalar param = 1.f;
1323 bool result2 = btRayAabb(rayFrom, rayTo, node->volume.Mins(), node->volume.Maxs(), param, resultNormal);
1324 btAssert(result1 == result2);
1325 #endif //TEST_BTRAY_AABB2
1329 if (node->isinternal())
1331 if (depth > treshold)
1333 stack.resize(stack.size() * 2);
1334 treshold = stack.size() - 2;
1336 stack[depth++] = node->childs[0];
1337 stack[depth++] = node->childs[1];
1341 policy.Process(node);
1350 inline void btDbvt::collideKDOP(const btDbvtNode* root,
1351 const btVector3* normals,
1352 const btScalar* offsets,
1359 const int inside = (1 << count) - 1;
1360 btAlignedObjectArray<sStkNP> stack;
1361 int signs[sizeof(unsigned) * 8];
1362 btAssert(count < int(sizeof(signs) / sizeof(signs[0])));
1363 for (int i = 0; i < count; ++i)
1365 signs[i] = ((normals[i].x() >= 0) ? 1 : 0) +
1366 ((normals[i].y() >= 0) ? 2 : 0) +
1367 ((normals[i].z() >= 0) ? 4 : 0);
1369 stack.reserve(SIMPLE_STACKSIZE);
1370 stack.push_back(sStkNP(root, 0));
1373 sStkNP se = stack[stack.size() - 1];
1376 for (int i = 0, j = 1; (!out) && (i < count); ++i, j <<= 1)
1378 if (0 == (se.mask & j))
1380 const int side = se.node->volume.Classify(normals[i], offsets[i], signs[i]);
1394 if ((se.mask != inside) && (se.node->isinternal()))
1396 stack.push_back(sStkNP(se.node->childs[0], se.mask));
1397 stack.push_back(sStkNP(se.node->childs[1], se.mask));
1401 if (policy.AllLeaves(se.node)) enumLeaves(se.node, policy);
1404 } while (stack.size());
1410 inline void btDbvt::collideOCL(const btDbvtNode* root,
1411 const btVector3* normals,
1412 const btScalar* offsets,
1413 const btVector3& sortaxis,
1421 const unsigned srtsgns = (sortaxis[0] >= 0 ? 1 : 0) +
1422 (sortaxis[1] >= 0 ? 2 : 0) +
1423 (sortaxis[2] >= 0 ? 4 : 0);
1424 const int inside = (1 << count) - 1;
1425 btAlignedObjectArray<sStkNPS> stock;
1426 btAlignedObjectArray<int> ifree;
1427 btAlignedObjectArray<int> stack;
1428 int signs[sizeof(unsigned) * 8];
1429 btAssert(count < int(sizeof(signs) / sizeof(signs[0])));
1430 for (int i = 0; i < count; ++i)
1432 signs[i] = ((normals[i].x() >= 0) ? 1 : 0) +
1433 ((normals[i].y() >= 0) ? 2 : 0) +
1434 ((normals[i].z() >= 0) ? 4 : 0);
1436 stock.reserve(SIMPLE_STACKSIZE);
1437 stack.reserve(SIMPLE_STACKSIZE);
1438 ifree.reserve(SIMPLE_STACKSIZE);
1439 stack.push_back(allocate(ifree, stock, sStkNPS(root, 0, root->volume.ProjectMinimum(sortaxis, srtsgns))));
1442 const int id = stack[stack.size() - 1];
1443 sStkNPS se = stock[id];
1445 ifree.push_back(id);
1446 if (se.mask != inside)
1449 for (int i = 0, j = 1; (!out) && (i < count); ++i, j <<= 1)
1451 if (0 == (se.mask & j))
1453 const int side = se.node->volume.Classify(normals[i], offsets[i], signs[i]);
1467 if (policy.Descent(se.node))
1469 if (se.node->isinternal())
1471 const btDbvtNode* pns[] = {se.node->childs[0], se.node->childs[1]};
1472 sStkNPS nes[] = {sStkNPS(pns[0], se.mask, pns[0]->volume.ProjectMinimum(sortaxis, srtsgns)),
1473 sStkNPS(pns[1], se.mask, pns[1]->volume.ProjectMinimum(sortaxis, srtsgns))};
1474 const int q = nes[0].value < nes[1].value ? 1 : 0;
1475 int j = stack.size();
1476 if (fsort && (j > 0))
1479 j = nearest(&stack[0], &stock[0], nes[q].value, 0, stack.size());
1482 //void * memmove ( void * destination, const void * source, size_t num );
1484 #if DBVT_USE_MEMMOVE
1486 int num_items_to_move = stack.size() - 1 - j;
1487 if (num_items_to_move > 0)
1488 memmove(&stack[j + 1], &stack[j], sizeof(int) * num_items_to_move);
1491 for (int k = stack.size() - 1; k > j; --k)
1493 stack[k] = stack[k - 1];
1496 stack[j] = allocate(ifree, stock, nes[q]);
1498 j = nearest(&stack[0], &stock[0], nes[1 - q].value, j, stack.size());
1500 #if DBVT_USE_MEMMOVE
1502 int num_items_to_move = stack.size() - 1 - j;
1503 if (num_items_to_move > 0)
1504 memmove(&stack[j + 1], &stack[j], sizeof(int) * num_items_to_move);
1507 for (int k = stack.size() - 1; k > j; --k)
1509 stack[k] = stack[k - 1];
1512 stack[j] = allocate(ifree, stock, nes[1 - q]);
1516 stack.push_back(allocate(ifree, stock, nes[q]));
1517 stack.push_back(allocate(ifree, stock, nes[1 - q]));
1522 policy.Process(se.node, se.value);
1525 } while (stack.size());
1531 inline void btDbvt::collideTU(const btDbvtNode* root,
1537 btAlignedObjectArray<const btDbvtNode*> stack;
1538 stack.reserve(SIMPLE_STACKSIZE);
1539 stack.push_back(root);
1542 const btDbvtNode* n = stack[stack.size() - 1];
1544 if (policy.Descent(n))
1546 if (n->isinternal())
1548 stack.push_back(n->childs[0]);
1549 stack.push_back(n->childs[1]);
1556 } while (stack.size() > 0);
1564 #undef DBVT_USE_MEMMOVE
1565 #undef DBVT_USE_TEMPLATE
1566 #undef DBVT_VIRTUAL_DTOR
1570 #undef DBVT_CHECKTYPE
1571 #undef DBVT_IMPL_GENERIC
1572 #undef DBVT_IMPL_SSE
1573 #undef DBVT_USE_INTRINSIC_SSE
1574 #undef DBVT_SELECT_IMPL
1575 #undef DBVT_MERGE_IMPL
1576 #undef DBVT_INT0_IMPL