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 {
Centerb3DbvtAabbMm134 B3_DBVT_INLINE b3Vector3 Center() const { return ((mi + mx) / 2); }
Lengthsb3DbvtAabbMm135 B3_DBVT_INLINE b3Vector3 Lengths() const { return (mx - mi); }
Extentsb3DbvtAabbMm136 B3_DBVT_INLINE b3Vector3 Extents() const { return ((mx - mi) / 2); }
Minsb3DbvtAabbMm137 B3_DBVT_INLINE const b3Vector3& Mins() const { return (mi); }
Maxsb3DbvtAabbMm138 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
tMinsb3DbvtAabbMm166 B3_DBVT_INLINE b3Vector3& tMins() { return (mi); }
tMaxsb3DbvtAabbMm167 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;
isleafb3DbvtNode184 B3_DBVT_INLINE bool isleaf() const { return (childs[1] == 0); }
isinternalb3DbvtNode185 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;
sStkNNb3DynamicBvh::sStkNN203 sStkNN() {}
sStkNNb3DynamicBvh::sStkNN204 sStkNN(const b3DbvtNode* na, const b3DbvtNode* nb) : a(na), b(nb) {}
205 };
206 struct sStkNP
207 {
208 const b3DbvtNode* node;
209 int mask;
sStkNPb3DynamicBvh::sStkNP210 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;
sStkNPSb3DynamicBvh::sStkNPS217 sStkNPS() {}
sStkNPSb3DynamicBvh::sStkNPS218 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;
sStkCLNb3DynamicBvh::sStkCLN224 sStkCLN(const b3DbvtNode* n, b3DbvtNode* p) : node(n), parent(p) {}
225 };
226 // Policies/Interfaces
227
228 /* ICollide */
229 struct ICollide
230 {
B3_DBVT_VIRTUAL_DTORb3DynamicBvh::ICollide231 B3_DBVT_VIRTUAL_DTOR(ICollide)
232 B3_DBVT_VIRTUAL void Process(const b3DbvtNode*, const b3DbvtNode*) {}
Processb3DynamicBvh::ICollide233 B3_DBVT_VIRTUAL void Process(const b3DbvtNode*) {}
Processb3DynamicBvh::ICollide234 B3_DBVT_VIRTUAL void Process(const b3DbvtNode* n, b3Scalar) { Process(n); }
Descentb3DynamicBvh::ICollide235 B3_DBVT_VIRTUAL bool Descent(const b3DbvtNode*) { return (true); }
AllLeavesb3DynamicBvh::ICollide236 B3_DBVT_VIRTUAL bool AllLeaves(const b3DbvtNode*) { return (true); }
237 };
238 /* IWriter */
239 struct IWriter
240 {
~IWriterb3DynamicBvh::IWriter241 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 {
~ICloneb3DynamicBvh::IClone249 virtual ~IClone() {}
CloneLeafb3DynamicBvh::IClone250 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();
emptyb3DynamicBvh274 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
benchmarkb3DynamicBvh293 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
nearestb3DynamicBvh369 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 }
allocateb3DynamicBvh382 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:
b3DynamicBvhb3DynamicBvh402 b3DynamicBvh(const b3DynamicBvh&) {}
403 };
404
405 //
406 // Inline's
407 //
408
409 //
FromCE(const b3Vector3 & c,const b3Vector3 & e)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 //
FromCR(const b3Vector3 & c,b3Scalar r)419 inline b3DbvtAabbMm b3DbvtAabbMm::FromCR(const b3Vector3& c, b3Scalar r)
420 {
421 return (FromCE(c, b3MakeVector3(r, r, r)));
422 }
423
424 //
FromMM(const b3Vector3 & mi,const b3Vector3 & mx)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 //
FromPoints(const b3Vector3 * pts,int n)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 //
FromPoints(const b3Vector3 ** ppts,int n)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 //
Expand(const b3Vector3 & e)460 B3_DBVT_INLINE void b3DbvtAabbMm::Expand(const b3Vector3& e)
461 {
462 mi -= e;
463 mx += e;
464 }
465
466 //
SignedExpand(const b3Vector3 & e)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 //
Contain(const b3DbvtAabbMm & a)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 //
Classify(const b3Vector3 & n,b3Scalar o,int s)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 //
ProjectMinimum(const b3Vector3 & v,unsigned signs)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 //
AddSpan(const b3Vector3 & d,b3Scalar & smi,b3Scalar & smx)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 //
b3Intersect(const b3DbvtAabbMm & a,const b3DbvtAabbMm & b)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 //
b3Intersect(const b3DbvtAabbMm & a,const b3Vector3 & b)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 //
b3Proximity(const b3DbvtAabbMm & a,const b3DbvtAabbMm & b)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 //
b3Select(const b3DbvtAabbMm & o,const b3DbvtAabbMm & a,const b3DbvtAabbMm & b)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 //
b3Merge(const b3DbvtAabbMm & a,const b3DbvtAabbMm & b,b3DbvtAabbMm & r)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 //
b3NotEqual(const b3DbvtAabbMm & a,const b3DbvtAabbMm & b)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
enumNodes(const b3DbvtNode * root,B3_DBVT_IPOLICY)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
enumLeaves(const b3DbvtNode * root,B3_DBVT_IPOLICY)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
collideTT(const b3DbvtNode * root0,const b3DbvtNode * root1,B3_DBVT_IPOLICY)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
collideTTpersistentStack(const b3DbvtNode * root0,const b3DbvtNode * root1,B3_DBVT_IPOLICY)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
collideTV(const b3DbvtNode * root,const b3DbvtVolume & vol,B3_DBVT_IPOLICY)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
rayTestInternal(const b3DbvtNode * root,const b3Vector3 & rayFrom,const b3Vector3 & rayTo,const b3Vector3 & rayDirectionInverse,unsigned int signs[3],b3Scalar lambda_max,const b3Vector3 & aabbMin,const b3Vector3 & aabbMax,B3_DBVT_IPOLICY)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
rayTest(const b3DbvtNode * root,const b3Vector3 & rayFrom,const b3Vector3 & rayTo,B3_DBVT_IPOLICY)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
collideKDOP(const b3DbvtNode * root,const b3Vector3 * normals,const b3Scalar * offsets,int count,B3_DBVT_IPOLICY)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
collideOCL(const b3DbvtNode * root,const b3Vector3 * normals,const b3Scalar * offsets,const b3Vector3 & sortaxis,int count,B3_DBVT_IPOLICY,bool fsort)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
collideTU(const b3DbvtNode * root,B3_DBVT_IPOLICY)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
1333