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