Bullet Collision Detection & Physics Library: btDbvt.h Source File

Bullet Collision Detection & Physics Library

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