00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00016
00017 #ifndef BT_DYNAMIC_BOUNDING_VOLUME_TREE_H
00018 #define BT_DYNAMIC_BOUNDING_VOLUME_TREE_H
00019
00020 #include "LinearMath/btAlignedObjectArray.h"
00021 #include "LinearMath/btVector3.h"
00022 #include "LinearMath/btTransform.h"
00023 #include "LinearMath/btAabbUtil2.h"
00024
00025
00026
00027
00028
00029
00030
00031 #define DBVT_IMPL_GENERIC 0 // Generic implementation
00032 #define DBVT_IMPL_SSE 1 // SSE
00033
00034
00035 #ifdef _WIN32
00036 #if (defined (_MSC_VER) && _MSC_VER >= 1400)
00037 #define DBVT_USE_TEMPLATE 1
00038 #else
00039 #define DBVT_USE_TEMPLATE 0
00040 #endif
00041 #else
00042 #define DBVT_USE_TEMPLATE 0
00043 #endif
00044
00045
00046 #define DBVT_USE_INTRINSIC_SSE 1
00047
00048
00049 #define DBVT_USE_MEMMOVE 1
00050
00051
00052 #define DBVT_ENABLE_BENCHMARK 0
00053
00054
00055 #define DBVT_INLINE SIMD_FORCE_INLINE
00056
00057
00058
00059
00060 #if defined (BT_USE_SSE) //&& defined (_WIN32)
00061 #define DBVT_SELECT_IMPL DBVT_IMPL_SSE
00062 #define DBVT_MERGE_IMPL DBVT_IMPL_SSE
00063 #define DBVT_INT0_IMPL DBVT_IMPL_SSE
00064 #else
00065 #define DBVT_SELECT_IMPL DBVT_IMPL_GENERIC
00066 #define DBVT_MERGE_IMPL DBVT_IMPL_GENERIC
00067 #define DBVT_INT0_IMPL DBVT_IMPL_GENERIC
00068 #endif
00069
00070 #if (DBVT_SELECT_IMPL==DBVT_IMPL_SSE)|| \
00071 (DBVT_MERGE_IMPL==DBVT_IMPL_SSE)|| \
00072 (DBVT_INT0_IMPL==DBVT_IMPL_SSE)
00073 #include <emmintrin.h>
00074 #endif
00075
00076
00077
00078
00079
00080 #if DBVT_USE_TEMPLATE
00081 #define DBVT_VIRTUAL
00082 #define DBVT_VIRTUAL_DTOR(a)
00083 #define DBVT_PREFIX template <typename T>
00084 #define DBVT_IPOLICY T& policy
00085 #define DBVT_CHECKTYPE static const ICollide& typechecker=*(T*)1;(void)typechecker;
00086 #else
00087 #define DBVT_VIRTUAL_DTOR(a) virtual ~a() {}
00088 #define DBVT_VIRTUAL virtual
00089 #define DBVT_PREFIX
00090 #define DBVT_IPOLICY ICollide& policy
00091 #define DBVT_CHECKTYPE
00092 #endif
00093
00094 #if DBVT_USE_MEMMOVE
00095 #if !defined( __CELLOS_LV2__) && !defined(__MWERKS__)
00096 #include <memory.h>
00097 #endif
00098 #include <string.h>
00099 #endif
00100
00101 #ifndef DBVT_USE_TEMPLATE
00102 #error "DBVT_USE_TEMPLATE undefined"
00103 #endif
00104
00105 #ifndef DBVT_USE_MEMMOVE
00106 #error "DBVT_USE_MEMMOVE undefined"
00107 #endif
00108
00109 #ifndef DBVT_ENABLE_BENCHMARK
00110 #error "DBVT_ENABLE_BENCHMARK undefined"
00111 #endif
00112
00113 #ifndef DBVT_SELECT_IMPL
00114 #error "DBVT_SELECT_IMPL undefined"
00115 #endif
00116
00117 #ifndef DBVT_MERGE_IMPL
00118 #error "DBVT_MERGE_IMPL undefined"
00119 #endif
00120
00121 #ifndef DBVT_INT0_IMPL
00122 #error "DBVT_INT0_IMPL undefined"
00123 #endif
00124
00125
00126
00127
00128
00129
00130 struct btDbvtAabbMm
00131 {
00132 DBVT_INLINE btVector3 Center() const { return((mi+mx)/2); }
00133 DBVT_INLINE btVector3 Lengths() const { return(mx-mi); }
00134 DBVT_INLINE btVector3 Extents() const { return((mx-mi)/2); }
00135 DBVT_INLINE const btVector3& Mins() const { return(mi); }
00136 DBVT_INLINE const btVector3& Maxs() const { return(mx); }
00137 static inline btDbvtAabbMm FromCE(const btVector3& c,const btVector3& e);
00138 static inline btDbvtAabbMm FromCR(const btVector3& c,btScalar r);
00139 static inline btDbvtAabbMm FromMM(const btVector3& mi,const btVector3& mx);
00140 static inline btDbvtAabbMm FromPoints(const btVector3* pts,int n);
00141 static inline btDbvtAabbMm FromPoints(const btVector3** ppts,int n);
00142 DBVT_INLINE void Expand(const btVector3& e);
00143 DBVT_INLINE void SignedExpand(const btVector3& e);
00144 DBVT_INLINE bool Contain(const btDbvtAabbMm& a) const;
00145 DBVT_INLINE int Classify(const btVector3& n,btScalar o,int s) const;
00146 DBVT_INLINE btScalar ProjectMinimum(const btVector3& v,unsigned signs) const;
00147 DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a,
00148 const btDbvtAabbMm& b);
00149
00150 DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a,
00151 const btVector3& b);
00152
00153 DBVT_INLINE friend btScalar Proximity( const btDbvtAabbMm& a,
00154 const btDbvtAabbMm& b);
00155 DBVT_INLINE friend int Select( const btDbvtAabbMm& o,
00156 const btDbvtAabbMm& a,
00157 const btDbvtAabbMm& b);
00158 DBVT_INLINE friend void Merge( const btDbvtAabbMm& a,
00159 const btDbvtAabbMm& b,
00160 btDbvtAabbMm& r);
00161 DBVT_INLINE friend bool NotEqual( const btDbvtAabbMm& a,
00162 const btDbvtAabbMm& b);
00163
00164 DBVT_INLINE btVector3& tMins() { return(mi); }
00165 DBVT_INLINE btVector3& tMaxs() { return(mx); }
00166
00167 private:
00168 DBVT_INLINE void AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const;
00169 private:
00170 btVector3 mi,mx;
00171 };
00172
00173
00174 typedef btDbvtAabbMm btDbvtVolume;
00175
00176
00177 struct btDbvtNode
00178 {
00179 btDbvtVolume volume;
00180 btDbvtNode* parent;
00181 DBVT_INLINE bool isleaf() const { return(childs[1]==0); }
00182 DBVT_INLINE bool isinternal() const { return(!isleaf()); }
00183 union
00184 {
00185 btDbvtNode* childs[2];
00186 void* data;
00187 int dataAsInt;
00188 };
00189 };
00190
00194 struct btDbvt
00195 {
00196
00197 struct sStkNN
00198 {
00199 const btDbvtNode* a;
00200 const btDbvtNode* b;
00201 sStkNN() {}
00202 sStkNN(const btDbvtNode* na,const btDbvtNode* nb) : a(na),b(nb) {}
00203 };
00204 struct sStkNP
00205 {
00206 const btDbvtNode* node;
00207 int mask;
00208 sStkNP(const btDbvtNode* n,unsigned m) : node(n),mask(m) {}
00209 };
00210 struct sStkNPS
00211 {
00212 const btDbvtNode* node;
00213 int mask;
00214 btScalar value;
00215 sStkNPS() {}
00216 sStkNPS(const btDbvtNode* n,unsigned m,btScalar v) : node(n),mask(m),value(v) {}
00217 };
00218 struct sStkCLN
00219 {
00220 const btDbvtNode* node;
00221 btDbvtNode* parent;
00222 sStkCLN(const btDbvtNode* n,btDbvtNode* p) : node(n),parent(p) {}
00223 };
00224
00225
00226
00227 struct ICollide
00228 {
00229 DBVT_VIRTUAL_DTOR(ICollide)
00230 DBVT_VIRTUAL void Process(const btDbvtNode*,const btDbvtNode*) {}
00231 DBVT_VIRTUAL void Process(const btDbvtNode*) {}
00232 DBVT_VIRTUAL void Process(const btDbvtNode* n,btScalar) { Process(n); }
00233 DBVT_VIRTUAL bool Descent(const btDbvtNode*) { return(true); }
00234 DBVT_VIRTUAL bool AllLeaves(const btDbvtNode*) { return(true); }
00235 };
00236
00237 struct IWriter
00238 {
00239 virtual ~IWriter() {}
00240 virtual void Prepare(const btDbvtNode* root,int numnodes)=0;
00241 virtual void WriteNode(const btDbvtNode*,int index,int parent,int child0,int child1)=0;
00242 virtual void WriteLeaf(const btDbvtNode*,int index,int parent)=0;
00243 };
00244
00245 struct IClone
00246 {
00247 virtual ~IClone() {}
00248 virtual void CloneLeaf(btDbvtNode*) {}
00249 };
00250
00251
00252 enum {
00253 SIMPLE_STACKSIZE = 64,
00254 DOUBLE_STACKSIZE = SIMPLE_STACKSIZE*2
00255 };
00256
00257
00258 btDbvtNode* m_root;
00259 btDbvtNode* m_free;
00260 int m_lkhd;
00261 int m_leaves;
00262 unsigned m_opath;
00263
00264
00265 btAlignedObjectArray<sStkNN> m_stkStack;
00266 mutable btAlignedObjectArray<const btDbvtNode*> m_rayTestStack;
00267
00268
00269
00270 btDbvt();
00271 ~btDbvt();
00272 void clear();
00273 bool empty() const { return(0==m_root); }
00274 void optimizeBottomUp();
00275 void optimizeTopDown(int bu_treshold=128);
00276 void optimizeIncremental(int passes);
00277 btDbvtNode* insert(const btDbvtVolume& box,void* data);
00278 void update(btDbvtNode* leaf,int lookahead=-1);
00279 void update(btDbvtNode* leaf,btDbvtVolume& volume);
00280 bool update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity,btScalar margin);
00281 bool update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity);
00282 bool update(btDbvtNode* leaf,btDbvtVolume& volume,btScalar margin);
00283 void remove(btDbvtNode* leaf);
00284 void write(IWriter* iwriter) const;
00285 void clone(btDbvt& dest,IClone* iclone=0) const;
00286 static int maxdepth(const btDbvtNode* node);
00287 static int countLeaves(const btDbvtNode* node);
00288 static void extractLeaves(const btDbvtNode* node,btAlignedObjectArray<const btDbvtNode*>& leaves);
00289 #if DBVT_ENABLE_BENCHMARK
00290 static void benchmark();
00291 #else
00292 static void benchmark(){}
00293 #endif
00294
00295 DBVT_PREFIX
00296 static void enumNodes( const btDbvtNode* root,
00297 DBVT_IPOLICY);
00298 DBVT_PREFIX
00299 static void enumLeaves( const btDbvtNode* root,
00300 DBVT_IPOLICY);
00301 DBVT_PREFIX
00302 void collideTT( const btDbvtNode* root0,
00303 const btDbvtNode* root1,
00304 DBVT_IPOLICY);
00305
00306 DBVT_PREFIX
00307 void collideTTpersistentStack( const btDbvtNode* root0,
00308 const btDbvtNode* root1,
00309 DBVT_IPOLICY);
00310 #if 0
00311 DBVT_PREFIX
00312 void collideTT( const btDbvtNode* root0,
00313 const btDbvtNode* root1,
00314 const btTransform& xform,
00315 DBVT_IPOLICY);
00316 DBVT_PREFIX
00317 void collideTT( const btDbvtNode* root0,
00318 const btTransform& xform0,
00319 const btDbvtNode* root1,
00320 const btTransform& xform1,
00321 DBVT_IPOLICY);
00322 #endif
00323
00324 DBVT_PREFIX
00325 void collideTV( const btDbvtNode* root,
00326 const btDbvtVolume& volume,
00327 DBVT_IPOLICY) const;
00330 DBVT_PREFIX
00331 static void rayTest( const btDbvtNode* root,
00332 const btVector3& rayFrom,
00333 const btVector3& rayTo,
00334 DBVT_IPOLICY);
00337 DBVT_PREFIX
00338 void rayTestInternal( const btDbvtNode* root,
00339 const btVector3& rayFrom,
00340 const btVector3& rayTo,
00341 const btVector3& rayDirectionInverse,
00342 unsigned int signs[3],
00343 btScalar lambda_max,
00344 const btVector3& aabbMin,
00345 const btVector3& aabbMax,
00346 DBVT_IPOLICY) const;
00347
00348 DBVT_PREFIX
00349 static void collideKDOP(const btDbvtNode* root,
00350 const btVector3* normals,
00351 const btScalar* offsets,
00352 int count,
00353 DBVT_IPOLICY);
00354 DBVT_PREFIX
00355 static void collideOCL( const btDbvtNode* root,
00356 const btVector3* normals,
00357 const btScalar* offsets,
00358 const btVector3& sortaxis,
00359 int count,
00360 DBVT_IPOLICY,
00361 bool fullsort=true);
00362 DBVT_PREFIX
00363 static void collideTU( const btDbvtNode* root,
00364 DBVT_IPOLICY);
00365
00366 static DBVT_INLINE int nearest(const int* i,const btDbvt::sStkNPS* a,btScalar v,int l,int h)
00367 {
00368 int m=0;
00369 while(l<h)
00370 {
00371 m=(l+h)>>1;
00372 if(a[i[m]].value>=v) l=m+1; else h=m;
00373 }
00374 return(h);
00375 }
00376 static DBVT_INLINE int allocate( btAlignedObjectArray<int>& ifree,
00377 btAlignedObjectArray<sStkNPS>& stock,
00378 const sStkNPS& value)
00379 {
00380 int i;
00381 if(ifree.size()>0)
00382 { i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; }
00383 else
00384 { i=stock.size();stock.push_back(value); }
00385 return(i);
00386 }
00387
00388 private:
00389 btDbvt(const btDbvt&) {}
00390 };
00391
00392
00393
00394
00395
00396
00397 inline btDbvtAabbMm btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e)
00398 {
00399 btDbvtAabbMm box;
00400 box.mi=c-e;box.mx=c+e;
00401 return(box);
00402 }
00403
00404
00405 inline btDbvtAabbMm btDbvtAabbMm::FromCR(const btVector3& c,btScalar r)
00406 {
00407 return(FromCE(c,btVector3(r,r,r)));
00408 }
00409
00410
00411 inline btDbvtAabbMm btDbvtAabbMm::FromMM(const btVector3& mi,const btVector3& mx)
00412 {
00413 btDbvtAabbMm box;
00414 box.mi=mi;box.mx=mx;
00415 return(box);
00416 }
00417
00418
00419 inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3* pts,int n)
00420 {
00421 btDbvtAabbMm box;
00422 box.mi=box.mx=pts[0];
00423 for(int i=1;i<n;++i)
00424 {
00425 box.mi.setMin(pts[i]);
00426 box.mx.setMax(pts[i]);
00427 }
00428 return(box);
00429 }
00430
00431
00432 inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3** ppts,int n)
00433 {
00434 btDbvtAabbMm box;
00435 box.mi=box.mx=*ppts[0];
00436 for(int i=1;i<n;++i)
00437 {
00438 box.mi.setMin(*ppts[i]);
00439 box.mx.setMax(*ppts[i]);
00440 }
00441 return(box);
00442 }
00443
00444
00445 DBVT_INLINE void btDbvtAabbMm::Expand(const btVector3& e)
00446 {
00447 mi-=e;mx+=e;
00448 }
00449
00450
00451 DBVT_INLINE void btDbvtAabbMm::SignedExpand(const btVector3& e)
00452 {
00453 if(e.x()>0) mx.setX(mx.x()+e[0]); else mi.setX(mi.x()+e[0]);
00454 if(e.y()>0) mx.setY(mx.y()+e[1]); else mi.setY(mi.y()+e[1]);
00455 if(e.z()>0) mx.setZ(mx.z()+e[2]); else mi.setZ(mi.z()+e[2]);
00456 }
00457
00458
00459 DBVT_INLINE bool btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const
00460 {
00461 return( (mi.x()<=a.mi.x())&&
00462 (mi.y()<=a.mi.y())&&
00463 (mi.z()<=a.mi.z())&&
00464 (mx.x()>=a.mx.x())&&
00465 (mx.y()>=a.mx.y())&&
00466 (mx.z()>=a.mx.z()));
00467 }
00468
00469
00470 DBVT_INLINE int btDbvtAabbMm::Classify(const btVector3& n,btScalar o,int s) const
00471 {
00472 btVector3 pi,px;
00473 switch(s)
00474 {
00475 case (0+0+0): px=btVector3(mi.x(),mi.y(),mi.z());
00476 pi=btVector3(mx.x(),mx.y(),mx.z());break;
00477 case (1+0+0): px=btVector3(mx.x(),mi.y(),mi.z());
00478 pi=btVector3(mi.x(),mx.y(),mx.z());break;
00479 case (0+2+0): px=btVector3(mi.x(),mx.y(),mi.z());
00480 pi=btVector3(mx.x(),mi.y(),mx.z());break;
00481 case (1+2+0): px=btVector3(mx.x(),mx.y(),mi.z());
00482 pi=btVector3(mi.x(),mi.y(),mx.z());break;
00483 case (0+0+4): px=btVector3(mi.x(),mi.y(),mx.z());
00484 pi=btVector3(mx.x(),mx.y(),mi.z());break;
00485 case (1+0+4): px=btVector3(mx.x(),mi.y(),mx.z());
00486 pi=btVector3(mi.x(),mx.y(),mi.z());break;
00487 case (0+2+4): px=btVector3(mi.x(),mx.y(),mx.z());
00488 pi=btVector3(mx.x(),mi.y(),mi.z());break;
00489 case (1+2+4): px=btVector3(mx.x(),mx.y(),mx.z());
00490 pi=btVector3(mi.x(),mi.y(),mi.z());break;
00491 }
00492 if((btDot(n,px)+o)<0) return(-1);
00493 if((btDot(n,pi)+o)>=0) return(+1);
00494 return(0);
00495 }
00496
00497
00498 DBVT_INLINE btScalar btDbvtAabbMm::ProjectMinimum(const btVector3& v,unsigned signs) const
00499 {
00500 const btVector3* b[]={&mx,&mi};
00501 const btVector3 p( b[(signs>>0)&1]->x(),
00502 b[(signs>>1)&1]->y(),
00503 b[(signs>>2)&1]->z());
00504 return(btDot(p,v));
00505 }
00506
00507
00508 DBVT_INLINE void btDbvtAabbMm::AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const
00509 {
00510 for(int i=0;i<3;++i)
00511 {
00512 if(d[i]<0)
00513 { smi+=mx[i]*d[i];smx+=mi[i]*d[i]; }
00514 else
00515 { smi+=mi[i]*d[i];smx+=mx[i]*d[i]; }
00516 }
00517 }
00518
00519
00520 DBVT_INLINE bool Intersect( const btDbvtAabbMm& a,
00521 const btDbvtAabbMm& b)
00522 {
00523 #if DBVT_INT0_IMPL == DBVT_IMPL_SSE
00524 const __m128 rt(_mm_or_ps( _mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)),
00525 _mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi))));
00526 #if defined (_WIN32)
00527 const __int32* pu((const __int32*)&rt);
00528 #else
00529 const int* pu((const int*)&rt);
00530 #endif
00531 return((pu[0]|pu[1]|pu[2])==0);
00532 #else
00533 return( (a.mi.x()<=b.mx.x())&&
00534 (a.mx.x()>=b.mi.x())&&
00535 (a.mi.y()<=b.mx.y())&&
00536 (a.mx.y()>=b.mi.y())&&
00537 (a.mi.z()<=b.mx.z())&&
00538 (a.mx.z()>=b.mi.z()));
00539 #endif
00540 }
00541
00542
00543
00544
00545 DBVT_INLINE bool Intersect( const btDbvtAabbMm& a,
00546 const btVector3& b)
00547 {
00548 return( (b.x()>=a.mi.x())&&
00549 (b.y()>=a.mi.y())&&
00550 (b.z()>=a.mi.z())&&
00551 (b.x()<=a.mx.x())&&
00552 (b.y()<=a.mx.y())&&
00553 (b.z()<=a.mx.z()));
00554 }
00555
00556
00557
00558
00559
00561
00562
00563
00564 DBVT_INLINE btScalar Proximity( const btDbvtAabbMm& a,
00565 const btDbvtAabbMm& b)
00566 {
00567 const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx);
00568 return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z()));
00569 }
00570
00571
00572
00573
00574 DBVT_INLINE int Select( const btDbvtAabbMm& o,
00575 const btDbvtAabbMm& a,
00576 const btDbvtAabbMm& b)
00577 {
00578 #if DBVT_SELECT_IMPL == DBVT_IMPL_SSE
00579
00580 #if defined (_WIN32)
00581 static ATTRIBUTE_ALIGNED16(const unsigned __int32) mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff};
00582 #else
00583 static ATTRIBUTE_ALIGNED16(const unsigned int) mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x00000000 };
00584 #endif
00585
00586 #if DBVT_USE_INTRINSIC_SSE
00587
00588 union btSSEUnion
00589 {
00590 __m128 ssereg;
00591 float floats[4];
00592 int ints[4];
00593 };
00594
00595 __m128 omi(_mm_load_ps(o.mi));
00596 omi=_mm_add_ps(omi,_mm_load_ps(o.mx));
00597 __m128 ami(_mm_load_ps(a.mi));
00598 ami=_mm_add_ps(ami,_mm_load_ps(a.mx));
00599 ami=_mm_sub_ps(ami,omi);
00600 ami=_mm_and_ps(ami,_mm_load_ps((const float*)mask));
00601 __m128 bmi(_mm_load_ps(b.mi));
00602 bmi=_mm_add_ps(bmi,_mm_load_ps(b.mx));
00603 bmi=_mm_sub_ps(bmi,omi);
00604 bmi=_mm_and_ps(bmi,_mm_load_ps((const float*)mask));
00605 __m128 t0(_mm_movehl_ps(ami,ami));
00606 ami=_mm_add_ps(ami,t0);
00607 ami=_mm_add_ss(ami,_mm_shuffle_ps(ami,ami,1));
00608 __m128 t1(_mm_movehl_ps(bmi,bmi));
00609 bmi=_mm_add_ps(bmi,t1);
00610 bmi=_mm_add_ss(bmi,_mm_shuffle_ps(bmi,bmi,1));
00611
00612 btSSEUnion tmp;
00613 tmp.ssereg = _mm_cmple_ss(bmi,ami);
00614 return tmp.ints[0]&1;
00615
00616 #else
00617 ATTRIBUTE_ALIGNED16(__int32 r[1]);
00618 __asm
00619 {
00620 mov eax,o
00621 mov ecx,a
00622 mov edx,b
00623 movaps xmm0,[eax]
00624 movaps xmm5,mask
00625 addps xmm0,[eax+16]
00626 movaps xmm1,[ecx]
00627 movaps xmm2,[edx]
00628 addps xmm1,[ecx+16]
00629 addps xmm2,[edx+16]
00630 subps xmm1,xmm0
00631 subps xmm2,xmm0
00632 andps xmm1,xmm5
00633 andps xmm2,xmm5
00634 movhlps xmm3,xmm1
00635 movhlps xmm4,xmm2
00636 addps xmm1,xmm3
00637 addps xmm2,xmm4
00638 pshufd xmm3,xmm1,1
00639 pshufd xmm4,xmm2,1
00640 addss xmm1,xmm3
00641 addss xmm2,xmm4
00642 cmpless xmm2,xmm1
00643 movss r,xmm2
00644 }
00645 return(r[0]&1);
00646 #endif
00647 #else
00648 return(Proximity(o,a)<Proximity(o,b)?0:1);
00649 #endif
00650 }
00651
00652
00653 DBVT_INLINE void Merge( const btDbvtAabbMm& a,
00654 const btDbvtAabbMm& b,
00655 btDbvtAabbMm& r)
00656 {
00657 #if DBVT_MERGE_IMPL==DBVT_IMPL_SSE
00658 __m128 ami(_mm_load_ps(a.mi));
00659 __m128 amx(_mm_load_ps(a.mx));
00660 __m128 bmi(_mm_load_ps(b.mi));
00661 __m128 bmx(_mm_load_ps(b.mx));
00662 ami=_mm_min_ps(ami,bmi);
00663 amx=_mm_max_ps(amx,bmx);
00664 _mm_store_ps(r.mi,ami);
00665 _mm_store_ps(r.mx,amx);
00666 #else
00667 for(int i=0;i<3;++i)
00668 {
00669 if(a.mi[i]<b.mi[i]) r.mi[i]=a.mi[i]; else r.mi[i]=b.mi[i];
00670 if(a.mx[i]>b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i];
00671 }
00672 #endif
00673 }
00674
00675
00676 DBVT_INLINE bool NotEqual( const btDbvtAabbMm& a,
00677 const btDbvtAabbMm& b)
00678 {
00679 return( (a.mi.x()!=b.mi.x())||
00680 (a.mi.y()!=b.mi.y())||
00681 (a.mi.z()!=b.mi.z())||
00682 (a.mx.x()!=b.mx.x())||
00683 (a.mx.y()!=b.mx.y())||
00684 (a.mx.z()!=b.mx.z()));
00685 }
00686
00687
00688
00689
00690
00691
00692 DBVT_PREFIX
00693 inline void btDbvt::enumNodes( const btDbvtNode* root,
00694 DBVT_IPOLICY)
00695 {
00696 DBVT_CHECKTYPE
00697 policy.Process(root);
00698 if(root->isinternal())
00699 {
00700 enumNodes(root->childs[0],policy);
00701 enumNodes(root->childs[1],policy);
00702 }
00703 }
00704
00705
00706 DBVT_PREFIX
00707 inline void btDbvt::enumLeaves( const btDbvtNode* root,
00708 DBVT_IPOLICY)
00709 {
00710 DBVT_CHECKTYPE
00711 if(root->isinternal())
00712 {
00713 enumLeaves(root->childs[0],policy);
00714 enumLeaves(root->childs[1],policy);
00715 }
00716 else
00717 {
00718 policy.Process(root);
00719 }
00720 }
00721
00722
00723 DBVT_PREFIX
00724 inline void btDbvt::collideTT( const btDbvtNode* root0,
00725 const btDbvtNode* root1,
00726 DBVT_IPOLICY)
00727 {
00728 DBVT_CHECKTYPE
00729 if(root0&&root1)
00730 {
00731 int depth=1;
00732 int treshold=DOUBLE_STACKSIZE-4;
00733 btAlignedObjectArray<sStkNN> stkStack;
00734 stkStack.resize(DOUBLE_STACKSIZE);
00735 stkStack[0]=sStkNN(root0,root1);
00736 do {
00737 sStkNN p=stkStack[--depth];
00738 if(depth>treshold)
00739 {
00740 stkStack.resize(stkStack.size()*2);
00741 treshold=stkStack.size()-4;
00742 }
00743 if(p.a==p.b)
00744 {
00745 if(p.a->isinternal())
00746 {
00747 stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]);
00748 stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]);
00749 stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]);
00750 }
00751 }
00752 else if(Intersect(p.a->volume,p.b->volume))
00753 {
00754 if(p.a->isinternal())
00755 {
00756 if(p.b->isinternal())
00757 {
00758 stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
00759 stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
00760 stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
00761 stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
00762 }
00763 else
00764 {
00765 stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
00766 stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
00767 }
00768 }
00769 else
00770 {
00771 if(p.b->isinternal())
00772 {
00773 stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
00774 stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
00775 }
00776 else
00777 {
00778 policy.Process(p.a,p.b);
00779 }
00780 }
00781 }
00782 } while(depth);
00783 }
00784 }
00785
00786
00787
00788 DBVT_PREFIX
00789 inline void btDbvt::collideTTpersistentStack( const btDbvtNode* root0,
00790 const btDbvtNode* root1,
00791 DBVT_IPOLICY)
00792 {
00793 DBVT_CHECKTYPE
00794 if(root0&&root1)
00795 {
00796 int depth=1;
00797 int treshold=DOUBLE_STACKSIZE-4;
00798
00799 m_stkStack.resize(DOUBLE_STACKSIZE);
00800 m_stkStack[0]=sStkNN(root0,root1);
00801 do {
00802 sStkNN p=m_stkStack[--depth];
00803 if(depth>treshold)
00804 {
00805 m_stkStack.resize(m_stkStack.size()*2);
00806 treshold=m_stkStack.size()-4;
00807 }
00808 if(p.a==p.b)
00809 {
00810 if(p.a->isinternal())
00811 {
00812 m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]);
00813 m_stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]);
00814 m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]);
00815 }
00816 }
00817 else if(Intersect(p.a->volume,p.b->volume))
00818 {
00819 if(p.a->isinternal())
00820 {
00821 if(p.b->isinternal())
00822 {
00823 m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
00824 m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
00825 m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
00826 m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
00827 }
00828 else
00829 {
00830 m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
00831 m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
00832 }
00833 }
00834 else
00835 {
00836 if(p.b->isinternal())
00837 {
00838 m_stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
00839 m_stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
00840 }
00841 else
00842 {
00843 policy.Process(p.a,p.b);
00844 }
00845 }
00846 }
00847 } while(depth);
00848 }
00849 }
00850
00851 #if 0
00852
00853 DBVT_PREFIX
00854 inline void btDbvt::collideTT( const btDbvtNode* root0,
00855 const btDbvtNode* root1,
00856 const btTransform& xform,
00857 DBVT_IPOLICY)
00858 {
00859 DBVT_CHECKTYPE
00860 if(root0&&root1)
00861 {
00862 int depth=1;
00863 int treshold=DOUBLE_STACKSIZE-4;
00864 btAlignedObjectArray<sStkNN> stkStack;
00865 stkStack.resize(DOUBLE_STACKSIZE);
00866 stkStack[0]=sStkNN(root0,root1);
00867 do {
00868 sStkNN p=stkStack[--depth];
00869 if(Intersect(p.a->volume,p.b->volume,xform))
00870 {
00871 if(depth>treshold)
00872 {
00873 stkStack.resize(stkStack.size()*2);
00874 treshold=stkStack.size()-4;
00875 }
00876 if(p.a->isinternal())
00877 {
00878 if(p.b->isinternal())
00879 {
00880 stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]);
00881 stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]);
00882 stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]);
00883 stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]);
00884 }
00885 else
00886 {
00887 stkStack[depth++]=sStkNN(p.a->childs[0],p.b);
00888 stkStack[depth++]=sStkNN(p.a->childs[1],p.b);
00889 }
00890 }
00891 else
00892 {
00893 if(p.b->isinternal())
00894 {
00895 stkStack[depth++]=sStkNN(p.a,p.b->childs[0]);
00896 stkStack[depth++]=sStkNN(p.a,p.b->childs[1]);
00897 }
00898 else
00899 {
00900 policy.Process(p.a,p.b);
00901 }
00902 }
00903 }
00904 } while(depth);
00905 }
00906 }
00907
00908 DBVT_PREFIX
00909 inline void btDbvt::collideTT( const btDbvtNode* root0,
00910 const btTransform& xform0,
00911 const btDbvtNode* root1,
00912 const btTransform& xform1,
00913 DBVT_IPOLICY)
00914 {
00915 const btTransform xform=xform0.inverse()*xform1;
00916 collideTT(root0,root1,xform,policy);
00917 }
00918 #endif
00919
00920
00921 DBVT_PREFIX
00922 inline void btDbvt::collideTV( const btDbvtNode* root,
00923 const btDbvtVolume& vol,
00924 DBVT_IPOLICY) const
00925 {
00926 DBVT_CHECKTYPE
00927 if(root)
00928 {
00929 ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol);
00930 btAlignedObjectArray<const btDbvtNode*> stack;
00931 stack.resize(0);
00932 stack.reserve(SIMPLE_STACKSIZE);
00933 stack.push_back(root);
00934 do {
00935 const btDbvtNode* n=stack[stack.size()-1];
00936 stack.pop_back();
00937 if(Intersect(n->volume,volume))
00938 {
00939 if(n->isinternal())
00940 {
00941 stack.push_back(n->childs[0]);
00942 stack.push_back(n->childs[1]);
00943 }
00944 else
00945 {
00946 policy.Process(n);
00947 }
00948 }
00949 } while(stack.size()>0);
00950 }
00951 }
00952
00953 DBVT_PREFIX
00954 inline void btDbvt::rayTestInternal( const btDbvtNode* root,
00955 const btVector3& rayFrom,
00956 const btVector3& rayTo,
00957 const btVector3& rayDirectionInverse,
00958 unsigned int signs[3],
00959 btScalar lambda_max,
00960 const btVector3& aabbMin,
00961 const btVector3& aabbMax,
00962 DBVT_IPOLICY) const
00963 {
00964 (void) rayTo;
00965 DBVT_CHECKTYPE
00966 if(root)
00967 {
00968 btVector3 resultNormal;
00969
00970 int depth=1;
00971 int treshold=DOUBLE_STACKSIZE-2;
00972 btAlignedObjectArray<const btDbvtNode*>& stack = m_rayTestStack;
00973 stack.resize(DOUBLE_STACKSIZE);
00974 stack[0]=root;
00975 btVector3 bounds[2];
00976 do
00977 {
00978 const btDbvtNode* node=stack[--depth];
00979 bounds[0] = node->volume.Mins()-aabbMax;
00980 bounds[1] = node->volume.Maxs()-aabbMin;
00981 btScalar tmin=1.f,lambda_min=0.f;
00982 unsigned int result1=false;
00983 result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
00984 if(result1)
00985 {
00986 if(node->isinternal())
00987 {
00988 if(depth>treshold)
00989 {
00990 stack.resize(stack.size()*2);
00991 treshold=stack.size()-2;
00992 }
00993 stack[depth++]=node->childs[0];
00994 stack[depth++]=node->childs[1];
00995 }
00996 else
00997 {
00998 policy.Process(node);
00999 }
01000 }
01001 } while(depth);
01002 }
01003 }
01004
01005
01006 DBVT_PREFIX
01007 inline void btDbvt::rayTest( const btDbvtNode* root,
01008 const btVector3& rayFrom,
01009 const btVector3& rayTo,
01010 DBVT_IPOLICY)
01011 {
01012 DBVT_CHECKTYPE
01013 if(root)
01014 {
01015 btVector3 rayDir = (rayTo-rayFrom);
01016 rayDir.normalize ();
01017
01019 btVector3 rayDirectionInverse;
01020 rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[0];
01021 rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[1];
01022 rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(BT_LARGE_FLOAT) : btScalar(1.0) / rayDir[2];
01023 unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0};
01024
01025 btScalar lambda_max = rayDir.dot(rayTo-rayFrom);
01026
01027 btVector3 resultNormal;
01028
01029 btAlignedObjectArray<const btDbvtNode*> stack;
01030
01031 int depth=1;
01032 int treshold=DOUBLE_STACKSIZE-2;
01033
01034 stack.resize(DOUBLE_STACKSIZE);
01035 stack[0]=root;
01036 btVector3 bounds[2];
01037 do {
01038 const btDbvtNode* node=stack[--depth];
01039
01040 bounds[0] = node->volume.Mins();
01041 bounds[1] = node->volume.Maxs();
01042
01043 btScalar tmin=1.f,lambda_min=0.f;
01044 unsigned int result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max);
01045
01046 #ifdef COMPARE_BTRAY_AABB2
01047 btScalar param=1.f;
01048 bool result2 = btRayAabb(rayFrom,rayTo,node->volume.Mins(),node->volume.Maxs(),param,resultNormal);
01049 btAssert(result1 == result2);
01050 #endif //TEST_BTRAY_AABB2
01051
01052 if(result1)
01053 {
01054 if(node->isinternal())
01055 {
01056 if(depth>treshold)
01057 {
01058 stack.resize(stack.size()*2);
01059 treshold=stack.size()-2;
01060 }
01061 stack[depth++]=node->childs[0];
01062 stack[depth++]=node->childs[1];
01063 }
01064 else
01065 {
01066 policy.Process(node);
01067 }
01068 }
01069 } while(depth);
01070
01071 }
01072 }
01073
01074
01075 DBVT_PREFIX
01076 inline void btDbvt::collideKDOP(const btDbvtNode* root,
01077 const btVector3* normals,
01078 const btScalar* offsets,
01079 int count,
01080 DBVT_IPOLICY)
01081 {
01082 DBVT_CHECKTYPE
01083 if(root)
01084 {
01085 const int inside=(1<<count)-1;
01086 btAlignedObjectArray<sStkNP> stack;
01087 int signs[sizeof(unsigned)*8];
01088 btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
01089 for(int i=0;i<count;++i)
01090 {
01091 signs[i]= ((normals[i].x()>=0)?1:0)+
01092 ((normals[i].y()>=0)?2:0)+
01093 ((normals[i].z()>=0)?4:0);
01094 }
01095 stack.reserve(SIMPLE_STACKSIZE);
01096 stack.push_back(sStkNP(root,0));
01097 do {
01098 sStkNP se=stack[stack.size()-1];
01099 bool out=false;
01100 stack.pop_back();
01101 for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
01102 {
01103 if(0==(se.mask&j))
01104 {
01105 const int side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
01106 switch(side)
01107 {
01108 case -1: out=true;break;
01109 case +1: se.mask|=j;break;
01110 }
01111 }
01112 }
01113 if(!out)
01114 {
01115 if((se.mask!=inside)&&(se.node->isinternal()))
01116 {
01117 stack.push_back(sStkNP(se.node->childs[0],se.mask));
01118 stack.push_back(sStkNP(se.node->childs[1],se.mask));
01119 }
01120 else
01121 {
01122 if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy);
01123 }
01124 }
01125 } while(stack.size());
01126 }
01127 }
01128
01129
01130 DBVT_PREFIX
01131 inline void btDbvt::collideOCL( const btDbvtNode* root,
01132 const btVector3* normals,
01133 const btScalar* offsets,
01134 const btVector3& sortaxis,
01135 int count,
01136 DBVT_IPOLICY,
01137 bool fsort)
01138 {
01139 DBVT_CHECKTYPE
01140 if(root)
01141 {
01142 const unsigned srtsgns=(sortaxis[0]>=0?1:0)+
01143 (sortaxis[1]>=0?2:0)+
01144 (sortaxis[2]>=0?4:0);
01145 const int inside=(1<<count)-1;
01146 btAlignedObjectArray<sStkNPS> stock;
01147 btAlignedObjectArray<int> ifree;
01148 btAlignedObjectArray<int> stack;
01149 int signs[sizeof(unsigned)*8];
01150 btAssert(count<int (sizeof(signs)/sizeof(signs[0])));
01151 for(int i=0;i<count;++i)
01152 {
01153 signs[i]= ((normals[i].x()>=0)?1:0)+
01154 ((normals[i].y()>=0)?2:0)+
01155 ((normals[i].z()>=0)?4:0);
01156 }
01157 stock.reserve(SIMPLE_STACKSIZE);
01158 stack.reserve(SIMPLE_STACKSIZE);
01159 ifree.reserve(SIMPLE_STACKSIZE);
01160 stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns))));
01161 do {
01162 const int id=stack[stack.size()-1];
01163 sStkNPS se=stock[id];
01164 stack.pop_back();ifree.push_back(id);
01165 if(se.mask!=inside)
01166 {
01167 bool out=false;
01168 for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)
01169 {
01170 if(0==(se.mask&j))
01171 {
01172 const int side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);
01173 switch(side)
01174 {
01175 case -1: out=true;break;
01176 case +1: se.mask|=j;break;
01177 }
01178 }
01179 }
01180 if(out) continue;
01181 }
01182 if(policy.Descent(se.node))
01183 {
01184 if(se.node->isinternal())
01185 {
01186 const btDbvtNode* pns[]={ se.node->childs[0],se.node->childs[1]};
01187 sStkNPS nes[]={ sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)),
01188 sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))};
01189 const int q=nes[0].value<nes[1].value?1:0;
01190 int j=stack.size();
01191 if(fsort&&(j>0))
01192 {
01193
01194 j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size());
01195 stack.push_back(0);
01196 #if DBVT_USE_MEMMOVE
01197 memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
01198 #else
01199 for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
01200 #endif
01201 stack[j]=allocate(ifree,stock,nes[q]);
01202
01203 j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size());
01204 stack.push_back(0);
01205 #if DBVT_USE_MEMMOVE
01206 memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));
01207 #else
01208 for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];
01209 #endif
01210 stack[j]=allocate(ifree,stock,nes[1-q]);
01211 }
01212 else
01213 {
01214 stack.push_back(allocate(ifree,stock,nes[q]));
01215 stack.push_back(allocate(ifree,stock,nes[1-q]));
01216 }
01217 }
01218 else
01219 {
01220 policy.Process(se.node,se.value);
01221 }
01222 }
01223 } while(stack.size());
01224 }
01225 }
01226
01227
01228 DBVT_PREFIX
01229 inline void btDbvt::collideTU( const btDbvtNode* root,
01230 DBVT_IPOLICY)
01231 {
01232 DBVT_CHECKTYPE
01233 if(root)
01234 {
01235 btAlignedObjectArray<const btDbvtNode*> stack;
01236 stack.reserve(SIMPLE_STACKSIZE);
01237 stack.push_back(root);
01238 do {
01239 const btDbvtNode* n=stack[stack.size()-1];
01240 stack.pop_back();
01241 if(policy.Descent(n))
01242 {
01243 if(n->isinternal())
01244 { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); }
01245 else
01246 { policy.Process(n); }
01247 }
01248 } while(stack.size()>0);
01249 }
01250 }
01251
01252
01253
01254
01255
01256 #undef DBVT_USE_MEMMOVE
01257 #undef DBVT_USE_TEMPLATE
01258 #undef DBVT_VIRTUAL_DTOR
01259 #undef DBVT_VIRTUAL
01260 #undef DBVT_PREFIX
01261 #undef DBVT_IPOLICY
01262 #undef DBVT_CHECKTYPE
01263 #undef DBVT_IMPL_GENERIC
01264 #undef DBVT_IMPL_SSE
01265 #undef DBVT_USE_INTRINSIC_SSE
01266 #undef DBVT_SELECT_IMPL
01267 #undef DBVT_MERGE_IMPL
01268 #undef DBVT_INT0_IMPL
01269
01270 #endif