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