 |
Blender V2.61 - r43446
|
btDbvt.h
Go to the documentation of this file.
00001 /* 00002 Bullet Continuous Collision Detection and Physics Library 00003 Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ 00004 00005 This software is provided 'as-is', without any express or implied warranty. 00006 In no event will the authors be held liable for any damages arising from the use of this software. 00007 Permission is granted to anyone to use this software for any purpose, 00008 including commercial applications, and to alter it and redistribute it freely, 00009 subject to the following restrictions: 00010 00011 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. 00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 00013 3. This notice may not be removed or altered from any source distribution. 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 // Compile time configuration 00027 // 00028 00029 00030 // Implementation profiles 00031 #define DBVT_IMPL_GENERIC 0 // Generic implementation 00032 #define DBVT_IMPL_SSE 1 // SSE 00033 00034 // Template implementation of ICollide 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 // Use only intrinsics instead of inline asm 00046 #define DBVT_USE_INTRINSIC_SSE 1 00047 00048 // Using memmov for collideOCL 00049 #define DBVT_USE_MEMMOVE 1 00050 00051 // Enable benchmarking code 00052 #define DBVT_ENABLE_BENCHMARK 0 00053 00054 // Inlining 00055 #define DBVT_INLINE SIMD_FORCE_INLINE 00056 00057 // Specific methods implementation 00058 00059 //SSE gives errors on a MSVC 7.1 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 // Auto config and checks 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 // Defaults volumes 00127 // 00128 00129 /* btDbvtAabbMm */ 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 // Types 00170 typedef btDbvtAabbMm btDbvtVolume; 00171 00172 /* btDbvtNode */ 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 /* Stack element */ 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 // Policies/Interfaces 00221 00222 /* ICollide */ 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 /* IWriter */ 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 /* IClone */ 00241 struct IClone 00242 { 00243 virtual ~IClone() {} 00244 virtual void CloneLeaf(btDbvtNode*) {} 00245 }; 00246 00247 // Constants 00248 enum { 00249 SIMPLE_STACKSIZE = 64, 00250 DOUBLE_STACKSIZE = SIMPLE_STACKSIZE*2 00251 }; 00252 00253 // Fields 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 // Methods 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 // DBVT_IPOLICY must support ICollide policy/interface 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 // Helpers 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 // Inline's 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 // Inline's 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 /* Insert 0 */ 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 /* Insert 1 */ 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 // PP Cleanup 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