- Timestamp:
- Dec 13, 2008, 11:45:51 PM (17 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.h
r2192 r2430 21 21 #include "LinearMath/btVector3.h" 22 22 #include "LinearMath/btTransform.h" 23 #include "LinearMath/btAabbUtil2.h" 23 24 24 25 // … … 33 34 // Template implementation of ICollide 34 35 #ifdef WIN32 35 #if (defined (_MSC_VER) && _MSC_VER >= 1400) 36 #define DBVT_USE_TEMPLATE 1 37 #else 38 #define DBVT_USE_TEMPLATE 0 39 #endif 36 #if (defined (_MSC_VER) && _MSC_VER >= 1400) 37 #define DBVT_USE_TEMPLATE 1 40 38 #else 41 39 #define DBVT_USE_TEMPLATE 0 42 40 #endif 41 #else 42 #define DBVT_USE_TEMPLATE 0 43 #endif 43 44 44 45 // Use only intrinsics instead of inline asm … … 53 54 // Inlining 54 55 #define DBVT_INLINE SIMD_FORCE_INLINE 55 // Align56 #ifdef WIN3257 #define DBVT_ALIGN __declspec(align(16))58 #else59 #define DBVT_ALIGN60 #endif61 56 62 57 // Specific methods implementation … … 135 130 struct btDbvtAabbMm 136 131 { 137 DBVT_INLINE btVector3 Center() const { return((mi+mx)/2); } 138 DBVT_INLINE btVector3 Lengths() const { return(mx-mi); } 139 DBVT_INLINE btVector3 Extents() const { return((mx-mi)/2); } 140 DBVT_INLINE const btVector3& Mins() const { return(mi); } 141 DBVT_INLINE const btVector3& Maxs() const { return(mx); } 142 static inline btDbvtAabbMm FromCE(const btVector3& c,const btVector3& e); 143 static inline btDbvtAabbMm FromCR(const btVector3& c,btScalar r); 144 static inline btDbvtAabbMm FromMM(const btVector3& mi,const btVector3& mx); 145 static inline btDbvtAabbMm FromPoints(const btVector3* pts,int n); 146 static inline btDbvtAabbMm FromPoints(const btVector3** ppts,int n); 147 DBVT_INLINE void Expand(const btVector3& e); 148 DBVT_INLINE void SignedExpand(const btVector3& e); 149 DBVT_INLINE bool Contain(const btDbvtAabbMm& a) const; 150 DBVT_INLINE int Classify(const btVector3& n,btScalar o,int s) const; 151 DBVT_INLINE btScalar ProjectMinimum(const btVector3& v,unsigned signs) const; 152 DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, 153 const btDbvtAabbMm& b); 154 DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, 155 const btDbvtAabbMm& b, 156 const btTransform& xform); 157 DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, 158 const btVector3& b); 159 DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, 160 const btVector3& org, 161 const btVector3& invdir, 162 const unsigned* signs); 163 DBVT_INLINE friend btScalar Proximity( const btDbvtAabbMm& a, 164 const btDbvtAabbMm& b); 165 DBVT_INLINE friend int Select( const btDbvtAabbMm& o, 166 const btDbvtAabbMm& a, 167 const btDbvtAabbMm& b); 168 DBVT_INLINE friend void Merge( const btDbvtAabbMm& a, 169 const btDbvtAabbMm& b, 170 btDbvtAabbMm& r); 171 DBVT_INLINE friend bool NotEqual( const btDbvtAabbMm& a, 172 const btDbvtAabbMm& b); 132 DBVT_INLINE btVector3 Center() const { return((mi+mx)/2); } 133 DBVT_INLINE btVector3 Lengths() const { return(mx-mi); } 134 DBVT_INLINE btVector3 Extents() const { return((mx-mi)/2); } 135 DBVT_INLINE const btVector3& Mins() const { return(mi); } 136 DBVT_INLINE const btVector3& Maxs() const { return(mx); } 137 static inline btDbvtAabbMm FromCE(const btVector3& c,const btVector3& e); 138 static inline btDbvtAabbMm FromCR(const btVector3& c,btScalar r); 139 static inline btDbvtAabbMm FromMM(const btVector3& mi,const btVector3& mx); 140 static inline btDbvtAabbMm FromPoints(const btVector3* pts,int n); 141 static inline btDbvtAabbMm FromPoints(const btVector3** ppts,int n); 142 DBVT_INLINE void Expand(const btVector3& e); 143 DBVT_INLINE void SignedExpand(const btVector3& e); 144 DBVT_INLINE bool Contain(const btDbvtAabbMm& a) const; 145 DBVT_INLINE int Classify(const btVector3& n,btScalar o,int s) const; 146 DBVT_INLINE btScalar ProjectMinimum(const btVector3& v,unsigned signs) const; 147 DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, 148 const btDbvtAabbMm& b); 149 DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, 150 const btDbvtAabbMm& b, 151 const btTransform& xform); 152 DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, 153 const btVector3& b); 154 155 DBVT_INLINE friend btScalar Proximity( const btDbvtAabbMm& a, 156 const btDbvtAabbMm& b); 157 DBVT_INLINE friend int Select( const btDbvtAabbMm& o, 158 const btDbvtAabbMm& a, 159 const btDbvtAabbMm& b); 160 DBVT_INLINE friend void Merge( const btDbvtAabbMm& a, 161 const btDbvtAabbMm& b, 162 btDbvtAabbMm& r); 163 DBVT_INLINE friend bool NotEqual( const btDbvtAabbMm& a, 164 const btDbvtAabbMm& b); 173 165 private: 174 DBVT_INLINE void AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const;166 DBVT_INLINE void AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const; 175 167 private: 176 btVector3 mi,mx;168 btVector3 mi,mx; 177 169 }; 178 170 … … 187 179 DBVT_INLINE bool isleaf() const { return(childs[1]==0); } 188 180 DBVT_INLINE bool isinternal() const { return(!isleaf()); } 189 union { 190 btDbvtNode* childs[2]; 191 void* data; 192 int dataAsInt; 193 }; 181 union 182 { 183 btDbvtNode* childs[2]; 184 void* data; 185 int dataAsInt; 186 }; 194 187 }; 195 188 … … 198 191 ///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure. 199 192 struct btDbvt 200 193 { 201 194 /* Stack element */ 202 195 struct sStkNN 203 196 { 204 197 const btDbvtNode* a; 205 198 const btDbvtNode* b; 206 199 sStkNN() {} 207 200 sStkNN(const btDbvtNode* na,const btDbvtNode* nb) : a(na),b(nb) {} 208 201 }; 209 202 struct sStkNP 210 203 { 211 204 const btDbvtNode* node; 212 205 int mask; 213 206 sStkNP(const btDbvtNode* n,unsigned m) : node(n),mask(m) {} 214 207 }; 215 208 struct sStkNPS 216 209 { 217 210 const btDbvtNode* node; 218 211 int mask; … … 220 213 sStkNPS() {} 221 214 sStkNPS(const btDbvtNode* n,unsigned m,btScalar v) : node(n),mask(m),value(v) {} 222 215 }; 223 216 struct sStkCLN 224 217 { 225 218 const btDbvtNode* node; 226 219 btDbvtNode* parent; 227 220 sStkCLN(const btDbvtNode* n,btDbvtNode* p) : node(n),parent(p) {} 228 221 }; 229 222 // Policies/Interfaces 230 223 231 224 /* ICollide */ 232 225 struct ICollide 233 226 { 234 227 DBVT_VIRTUAL_DTOR(ICollide) 235 DBVT_VIRTUAL void Process(const btDbvtNode*,const btDbvtNode*) {}228 DBVT_VIRTUAL void Process(const btDbvtNode*,const btDbvtNode*) {} 236 229 DBVT_VIRTUAL void Process(const btDbvtNode*) {} 237 230 DBVT_VIRTUAL void Process(const btDbvtNode* n,btScalar) { Process(n); } 238 231 DBVT_VIRTUAL bool Descent(const btDbvtNode*) { return(true); } 239 232 DBVT_VIRTUAL bool AllLeaves(const btDbvtNode*) { return(true); } 240 233 }; 241 234 /* IWriter */ 242 235 struct IWriter 243 236 { 244 237 virtual ~IWriter() {} 245 238 virtual void Prepare(const btDbvtNode* root,int numnodes)=0; 246 239 virtual void WriteNode(const btDbvtNode*,int index,int parent,int child0,int child1)=0; 247 240 virtual void WriteLeaf(const btDbvtNode*,int index,int parent)=0; 248 241 }; 249 242 /* IClone */ 250 243 struct IClone 251 244 { 252 245 virtual ~IClone() {} 253 246 virtual void CloneLeaf(btDbvtNode*) {} 254 255 247 }; 248 256 249 // Constants 257 250 enum { 258 259 260 261 251 SIMPLE_STACKSIZE = 64, 252 DOUBLE_STACKSIZE = SIMPLE_STACKSIZE*2 253 }; 254 262 255 // Fields 263 256 btDbvtNode* m_root; … … 266 259 int m_leaves; 267 260 unsigned m_opath; 261 262 263 btAlignedObjectArray<sStkNN> m_stkStack; 264 265 268 266 // Methods 269 270 267 btDbvt(); 268 ~btDbvt(); 271 269 void clear(); 272 270 bool empty() const { return(0==m_root); } … … 276 274 btDbvtNode* insert(const btDbvtVolume& box,void* data); 277 275 void update(btDbvtNode* leaf,int lookahead=-1); 278 void update(btDbvtNode* leaf, constbtDbvtVolume& volume);279 bool update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity,btScalar margin);280 bool update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity);281 bool update(btDbvtNode* leaf,btDbvtVolume volume,btScalar margin);276 void update(btDbvtNode* leaf,btDbvtVolume& volume); 277 bool update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity,btScalar margin); 278 bool update(btDbvtNode* leaf,btDbvtVolume& volume,const btVector3& velocity); 279 bool update(btDbvtNode* leaf,btDbvtVolume& volume,btScalar margin); 282 280 void remove(btDbvtNode* leaf); 283 281 void write(IWriter* iwriter) const; … … 286 284 static int countLeaves(const btDbvtNode* node); 287 285 static void extractLeaves(const btDbvtNode* node,btAlignedObjectArray<const btDbvtNode*>& leaves); 288 286 #if DBVT_ENABLE_BENCHMARK 289 287 static void benchmark(); 290 288 #else 291 289 static void benchmark(){} 292 290 #endif 293 291 // DBVT_IPOLICY must support ICollide policy/interface 294 292 DBVT_PREFIX 295 static void enumNodes( const btDbvtNode* root,296 293 static void enumNodes( const btDbvtNode* root, 294 DBVT_IPOLICY); 297 295 DBVT_PREFIX 298 static void enumLeaves( const btDbvtNode* root,299 296 static void enumLeaves( const btDbvtNode* root, 297 DBVT_IPOLICY); 300 298 DBVT_PREFIX 301 static void collideTT( const btDbvtNode* root0, 302 const btDbvtNode* root1, 303 DBVT_IPOLICY); 299 void collideTT( const btDbvtNode* root0, 300 const btDbvtNode* root1, 301 DBVT_IPOLICY); 302 304 303 DBVT_PREFIX 305 static void collideTT( const btDbvtNode* root0,306 307 const btTransform& xform,308 DBVT_IPOLICY); 304 void collideTTpersistentStack( const btDbvtNode* root0, 305 const btDbvtNode* root1, 306 DBVT_IPOLICY); 307 309 308 DBVT_PREFIX 310 static void collideTT( const btDbvtNode* root0, 311 const btTransform& xform0, 312 const btDbvtNode* root1, 313 const btTransform& xform1, 314 DBVT_IPOLICY); 309 void collideTT( const btDbvtNode* root0, 310 const btDbvtNode* root1, 311 const btTransform& xform, 312 DBVT_IPOLICY); 315 313 DBVT_PREFIX 316 static void collideTV( const btDbvtNode* root, 317 const btDbvtVolume& volume, 318 DBVT_IPOLICY); 314 void collideTT( const btDbvtNode* root0, 315 const btTransform& xform0, 316 const btDbvtNode* root1, 317 const btTransform& xform1, 318 DBVT_IPOLICY); 319 319 DBVT_PREFIX 320 static void collideRAY( const btDbvtNode* root, 321 const btVector3& origin, 322 const btVector3& direction, 323 DBVT_IPOLICY); 320 void collideTV( const btDbvtNode* root, 321 const btDbvtVolume& volume, 322 DBVT_IPOLICY); 323 ///rayTest is a re-entrant ray test, and can be called in parallel as long as the btAlignedAlloc is thread-safe (uses locking etc) 324 ///rayTest is slower than rayTestInternal, because it builds a local stack, using memory allocations, and it recomputes signs/rayDirectionInverses each time 324 325 DBVT_PREFIX 325 static void collideKDOP(const btDbvtNode* root, 326 const btVector3* normals, 327 const btScalar* offsets, 328 int count, 329 DBVT_IPOLICY); 326 static void rayTest( const btDbvtNode* root, 327 const btVector3& rayFrom, 328 const btVector3& rayTo, 329 DBVT_IPOLICY); 330 ///rayTestInternal is faster than rayTest, because it uses a persistent stack (to reduce dynamic memory allocations to a minimum) and it uses precomputed signs/rayInverseDirections 331 ///rayTestInternal is used by btDbvtBroadphase to accelerate world ray casts 330 332 DBVT_PREFIX 331 static void collideOCL( const btDbvtNode* root, 332 const btVector3* normals, 333 const btScalar* offsets, 334 const btVector3& sortaxis, 335 int count, 336 DBVT_IPOLICY, 337 bool fullsort=true); 333 void rayTestInternal( const btDbvtNode* root, 334 const btVector3& rayFrom, 335 const btVector3& rayTo, 336 const btVector3& rayDirectionInverse, 337 unsigned int signs[3], 338 btScalar lambda_max, 339 const btVector3& aabbMin, 340 const btVector3& aabbMax, 341 DBVT_IPOLICY) const; 342 338 343 DBVT_PREFIX 339 static void collideTU( const btDbvtNode* root, 340 DBVT_IPOLICY); 344 static void collideKDOP(const btDbvtNode* root, 345 const btVector3* normals, 346 const btScalar* offsets, 347 int count, 348 DBVT_IPOLICY); 349 DBVT_PREFIX 350 static void collideOCL( const btDbvtNode* root, 351 const btVector3* normals, 352 const btScalar* offsets, 353 const btVector3& sortaxis, 354 int count, 355 DBVT_IPOLICY, 356 bool fullsort=true); 357 DBVT_PREFIX 358 static void collideTU( const btDbvtNode* root, 359 DBVT_IPOLICY); 341 360 // Helpers 342 361 static DBVT_INLINE int nearest(const int* i,const btDbvt::sStkNPS* a,btScalar v,int l,int h) 343 362 { 344 363 int m=0; 345 364 while(l<h) 346 365 { 347 366 m=(l+h)>>1; 348 367 if(a[i[m]].value>=v) l=m+1; else h=m; 349 368 } 350 369 return(h); 351 370 } 352 371 static DBVT_INLINE int allocate( btAlignedObjectArray<int>& ifree, 353 354 355 372 btAlignedObjectArray<sStkNPS>& stock, 373 const sStkNPS& value) 374 { 356 375 int i; 357 376 if(ifree.size()>0) 358 359 360 377 { i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; } 378 else 379 { i=stock.size();stock.push_back(value); } 361 380 return(i); 362 381 } 363 382 // 364 365 366 383 private: 384 btDbvt(const btDbvt&) {} 385 }; 367 386 368 387 // … … 373 392 inline btDbvtAabbMm btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e) 374 393 { 375 btDbvtAabbMm box;376 box.mi=c-e;box.mx=c+e;377 return(box);378 } 379 394 btDbvtAabbMm box; 395 box.mi=c-e;box.mx=c+e; 396 return(box); 397 } 398 380 399 // 381 400 inline btDbvtAabbMm btDbvtAabbMm::FromCR(const btVector3& c,btScalar r) 382 401 { 383 return(FromCE(c,btVector3(r,r,r)));384 } 385 402 return(FromCE(c,btVector3(r,r,r))); 403 } 404 386 405 // 387 406 inline btDbvtAabbMm btDbvtAabbMm::FromMM(const btVector3& mi,const btVector3& mx) 388 407 { 389 btDbvtAabbMm box;390 box.mi=mi;box.mx=mx;391 return(box);392 } 393 408 btDbvtAabbMm box; 409 box.mi=mi;box.mx=mx; 410 return(box); 411 } 412 394 413 // 395 414 inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3* pts,int n) 396 415 { 397 btDbvtAabbMm box;398 box.mi=box.mx=pts[0];399 for(int i=1;i<n;++i)400 { 401 box.mi.setMin(pts[i]);402 box.mx.setMax(pts[i]);416 btDbvtAabbMm box; 417 box.mi=box.mx=pts[0]; 418 for(int i=1;i<n;++i) 419 { 420 box.mi.setMin(pts[i]); 421 box.mx.setMax(pts[i]); 403 422 } 404 return(box);423 return(box); 405 424 } 406 425 … … 408 427 inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3** ppts,int n) 409 428 { 410 btDbvtAabbMm box;411 box.mi=box.mx=*ppts[0];412 for(int i=1;i<n;++i)413 { 414 box.mi.setMin(*ppts[i]);415 box.mx.setMax(*ppts[i]);429 btDbvtAabbMm box; 430 box.mi=box.mx=*ppts[0]; 431 for(int i=1;i<n;++i) 432 { 433 box.mi.setMin(*ppts[i]); 434 box.mx.setMax(*ppts[i]); 416 435 } 417 return(box);436 return(box); 418 437 } 419 438 … … 421 440 DBVT_INLINE void btDbvtAabbMm::Expand(const btVector3& e) 422 441 { 423 mi-=e;mx+=e;424 } 425 442 mi-=e;mx+=e; 443 } 444 426 445 // 427 446 DBVT_INLINE void btDbvtAabbMm::SignedExpand(const btVector3& e) 428 447 { 429 if(e.x()>0) mx.setX(mx.x()+e[0]); else mi.setX(mi.x()+e[0]);430 if(e.y()>0) mx.setY(mx.y()+e[1]); else mi.setY(mi.y()+e[1]);431 if(e.z()>0) mx.setZ(mx.z()+e[2]); else mi.setZ(mi.z()+e[2]);432 } 433 448 if(e.x()>0) mx.setX(mx.x()+e[0]); else mi.setX(mi.x()+e[0]); 449 if(e.y()>0) mx.setY(mx.y()+e[1]); else mi.setY(mi.y()+e[1]); 450 if(e.z()>0) mx.setZ(mx.z()+e[2]); else mi.setZ(mi.z()+e[2]); 451 } 452 434 453 // 435 454 DBVT_INLINE bool btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const 436 455 { 437 return( (mi.x()<=a.mi.x())&&456 return( (mi.x()<=a.mi.x())&& 438 457 (mi.y()<=a.mi.y())&& 439 458 (mi.z()<=a.mi.z())&& … … 446 465 DBVT_INLINE int btDbvtAabbMm::Classify(const btVector3& n,btScalar o,int s) const 447 466 { 448 btVector3 pi,px;449 switch(s)467 btVector3 pi,px; 468 switch(s) 450 469 { 451 470 case (0+0+0): px=btVector3(mi.x(),mi.y(),mi.z()); 452 471 pi=btVector3(mx.x(),mx.y(),mx.z());break; 453 472 case (1+0+0): px=btVector3(mx.x(),mi.y(),mi.z()); 454 473 pi=btVector3(mi.x(),mx.y(),mx.z());break; 455 474 case (0+2+0): px=btVector3(mi.x(),mx.y(),mi.z()); 456 475 pi=btVector3(mx.x(),mi.y(),mx.z());break; 457 476 case (1+2+0): px=btVector3(mx.x(),mx.y(),mi.z()); 458 477 pi=btVector3(mi.x(),mi.y(),mx.z());break; 459 478 case (0+0+4): px=btVector3(mi.x(),mi.y(),mx.z()); 460 479 pi=btVector3(mx.x(),mx.y(),mi.z());break; 461 480 case (1+0+4): px=btVector3(mx.x(),mi.y(),mx.z()); 462 481 pi=btVector3(mi.x(),mx.y(),mi.z());break; 463 482 case (0+2+4): px=btVector3(mi.x(),mx.y(),mx.z()); 464 483 pi=btVector3(mx.x(),mi.y(),mi.z());break; 465 484 case (1+2+4): px=btVector3(mx.x(),mx.y(),mx.z()); 466 485 pi=btVector3(mi.x(),mi.y(),mi.z());break; 467 486 } 468 if((dot(n,px)+o)<0) return(-1);469 if((dot(n,pi)+o)>=0) return(+1);470 return(0);487 if((dot(n,px)+o)<0) return(-1); 488 if((dot(n,pi)+o)>=0) return(+1); 489 return(0); 471 490 } 472 491 … … 474 493 DBVT_INLINE btScalar btDbvtAabbMm::ProjectMinimum(const btVector3& v,unsigned signs) const 475 494 { 476 const btVector3* b[]={&mx,&mi};477 const btVector3 p( b[(signs>>0)&1]->x(),478 479 480 return(dot(p,v));495 const btVector3* b[]={&mx,&mi}; 496 const btVector3 p( b[(signs>>0)&1]->x(), 497 b[(signs>>1)&1]->y(), 498 b[(signs>>2)&1]->z()); 499 return(dot(p,v)); 481 500 } 482 501 … … 484 503 DBVT_INLINE void btDbvtAabbMm::AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const 485 504 { 486 for(int i=0;i<3;++i)487 { 488 if(d[i]<0)505 for(int i=0;i<3;++i) 506 { 507 if(d[i]<0) 489 508 { smi+=mx[i]*d[i];smx+=mi[i]*d[i]; } 490 509 else … … 492 511 } 493 512 } 494 513 495 514 // 496 515 DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, 497 516 const btDbvtAabbMm& b) 498 517 { 499 518 #if DBVT_INT0_IMPL == DBVT_IMPL_SSE 500 const __m128 rt(_mm_or_ps( _mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)),501 502 const __int32* pu((const __int32*)&rt);503 return((pu[0]|pu[1]|pu[2])==0);519 const __m128 rt(_mm_or_ps( _mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)), 520 _mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi)))); 521 const __int32* pu((const __int32*)&rt); 522 return((pu[0]|pu[1]|pu[2])==0); 504 523 #else 505 return( (a.mi.x()<=b.mx.x())&&524 return( (a.mi.x()<=b.mx.x())&& 506 525 (a.mx.x()>=b.mi.x())&& 507 526 (a.mi.y()<=b.mx.y())&& … … 514 533 // 515 534 DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, 516 517 518 { 519 const btVector3 d0=xform*b.Center()-a.Center();520 const btVector3 d1=d0*xform.getBasis();521 btScalar s0[2]={0,0};522 btScalar s1[2]={dot(xform.getOrigin(),d0),s1[0]};523 a.AddSpan(d0,s0[0],s0[1]);524 b.AddSpan(d1,s1[0],s1[1]);525 if(s0[0]>(s1[1])) return(false);526 if(s0[1]<(s1[0])) return(false);527 return(true);535 const btDbvtAabbMm& b, 536 const btTransform& xform) 537 { 538 const btVector3 d0=xform*b.Center()-a.Center(); 539 const btVector3 d1=d0*xform.getBasis(); 540 btScalar s0[2]={0,0}; 541 btScalar s1[2]={dot(xform.getOrigin(),d0),s1[0]}; 542 a.AddSpan(d0,s0[0],s0[1]); 543 b.AddSpan(d1,s1[0],s1[1]); 544 if(s0[0]>(s1[1])) return(false); 545 if(s0[1]<(s1[0])) return(false); 546 return(true); 528 547 } 529 548 530 549 // 531 550 DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, 532 533 { 534 return( (b.x()>=a.mi.x())&&551 const btVector3& b) 552 { 553 return( (b.x()>=a.mi.x())&& 535 554 (b.y()>=a.mi.y())&& 536 555 (b.z()>=a.mi.z())&& … … 540 559 } 541 560 542 // 543 DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, 544 const btVector3& org, 545 const btVector3& invdir, 546 const unsigned* signs) 547 { 548 #if 0 549 const btVector3 b0((a.mi-org)*invdir); 550 const btVector3 b1((a.mx-org)*invdir); 551 const btVector3 tmin(btMin(b0[0],b1[0]),btMin(b0[1],b1[1]),btMin(b0[2],b1[2])); 552 const btVector3 tmax(btMax(b0[0],b1[0]),btMax(b0[1],b1[1]),btMax(b0[2],b1[2])); 553 const btScalar tin=btMax(tmin[0],btMax(tmin[1],tmin[2])); 554 const btScalar tout=btMin(tmax[0],btMin(tmax[1],tmax[2])); 555 return(tin<tout); 556 #else 557 const btVector3* bounds[2]={&a.mi,&a.mx}; 558 btScalar txmin=(bounds[ signs[0]]->x()-org[0])*invdir[0]; 559 btScalar txmax=(bounds[1-signs[0]]->x()-org[0])*invdir[0]; 560 const btScalar tymin=(bounds[ signs[1]]->y()-org[1])*invdir[1]; 561 const btScalar tymax=(bounds[1-signs[1]]->y()-org[1])*invdir[1]; 562 if((txmin>tymax)||(tymin>txmax)) return(false); 563 if(tymin>txmin) txmin=tymin; 564 if(tymax<txmax) txmax=tymax; 565 const btScalar tzmin=(bounds[ signs[2]]->z()-org[2])*invdir[2]; 566 const btScalar tzmax=(bounds[1-signs[2]]->z()-org[2])*invdir[2]; 567 if((txmin>tzmax)||(tzmin>txmax)) return(false); 568 if(tzmin>txmin) txmin=tzmin; 569 if(tzmax<txmax) txmax=tzmax; 570 return(txmax>0); 571 #endif 572 } 573 561 562 563 564 565 ////////////////////////////////////// 566 567 574 568 // 575 569 DBVT_INLINE btScalar Proximity( const btDbvtAabbMm& a, 576 const btDbvtAabbMm& b) 577 { 578 const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx); 579 return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z())); 580 } 570 const btDbvtAabbMm& b) 571 { 572 const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx); 573 return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z())); 574 } 575 576 581 577 582 578 // 583 579 DBVT_INLINE int Select( const btDbvtAabbMm& o, 584 585 580 const btDbvtAabbMm& a, 581 const btDbvtAabbMm& b) 586 582 { 587 583 #if DBVT_SELECT_IMPL == DBVT_IMPL_SSE 588 static DBVT_ALIGN const unsigned __int32 mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff}; 589 // TODO: the intrinsic version is 11% slower 590 #if DBVT_USE_INTRINSIC_SSE 584 static ATTRIBUTE_ALIGNED16(const unsigned __int32) mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff}; 585 ///@todo: the intrinsic version is 11% slower 586 #if DBVT_USE_INTRINSIC_SSE 587 588 union btSSEUnion ///NOTE: if we use more intrinsics, move btSSEUnion into the LinearMath directory 589 { 590 __m128 ssereg; 591 float floats[4]; 592 int ints[4]; 593 }; 594 591 595 __m128 omi(_mm_load_ps(o.mi)); 592 596 omi=_mm_add_ps(omi,_mm_load_ps(o.mx)); … … 602 606 ami=_mm_add_ps(ami,t0); 603 607 ami=_mm_add_ss(ami,_mm_shuffle_ps(ami,ami,1)); 604 __m128 608 __m128 t1(_mm_movehl_ps(bmi,bmi)); 605 609 bmi=_mm_add_ps(bmi,t1); 606 610 bmi=_mm_add_ss(bmi,_mm_shuffle_ps(bmi,bmi,1)); 607 return(_mm_cmple_ss(bmi,ami).m128_u32[0]&1); 608 #else 609 DBVT_ALIGN __int32 r[1]; 611 612 btSSEUnion tmp; 613 tmp.ssereg = _mm_cmple_ss(bmi,ami); 614 return tmp.ints[0]&1; 615 616 #else 617 ATTRIBUTE_ALIGNED16(__int32 r[1]); 610 618 __asm 611 619 { 612 620 mov eax,o 613 mov ecx,a614 mov edx,b615 movaps xmm0,[eax]621 mov ecx,a 622 mov edx,b 623 movaps xmm0,[eax] 616 624 movaps xmm5,mask 617 addps xmm0,[eax+16]625 addps xmm0,[eax+16] 618 626 movaps xmm1,[ecx] 619 627 movaps xmm2,[edx] … … 621 629 addps xmm2,[edx+16] 622 630 subps xmm1,xmm0 623 subps xmm2,xmm0624 andps xmm1,xmm5625 andps xmm2,xmm5626 movhlps xmm3,xmm1627 movhlps xmm4,xmm2628 addps xmm1,xmm3629 addps xmm2,xmm4630 pshufd xmm3,xmm1,1631 pshufd xmm4,xmm2,1632 addss xmm1,xmm3633 addss xmm2,xmm4634 cmpless xmm2,xmm1635 movss r,xmm2636 631 subps xmm2,xmm0 632 andps xmm1,xmm5 633 andps xmm2,xmm5 634 movhlps xmm3,xmm1 635 movhlps xmm4,xmm2 636 addps xmm1,xmm3 637 addps xmm2,xmm4 638 pshufd xmm3,xmm1,1 639 pshufd xmm4,xmm2,1 640 addss xmm1,xmm3 641 addss xmm2,xmm4 642 cmpless xmm2,xmm1 643 movss r,xmm2 644 } 637 645 return(r[0]&1); 638 646 #endif 639 647 #else 640 return(Proximity(o,a)<Proximity(o,b)?0:1);648 return(Proximity(o,a)<Proximity(o,b)?0:1); 641 649 #endif 642 650 } … … 644 652 // 645 653 DBVT_INLINE void Merge( const btDbvtAabbMm& a, 646 647 654 const btDbvtAabbMm& b, 655 btDbvtAabbMm& r) 648 656 { 649 657 #if DBVT_MERGE_IMPL==DBVT_IMPL_SSE 650 __m128 ami(_mm_load_ps(a.mi));651 __m128 amx(_mm_load_ps(a.mx));652 __m128 bmi(_mm_load_ps(b.mi));653 __m128 bmx(_mm_load_ps(b.mx));654 ami=_mm_min_ps(ami,bmi);655 amx=_mm_max_ps(amx,bmx);656 _mm_store_ps(r.mi,ami);657 _mm_store_ps(r.mx,amx);658 __m128 ami(_mm_load_ps(a.mi)); 659 __m128 amx(_mm_load_ps(a.mx)); 660 __m128 bmi(_mm_load_ps(b.mi)); 661 __m128 bmx(_mm_load_ps(b.mx)); 662 ami=_mm_min_ps(ami,bmi); 663 amx=_mm_max_ps(amx,bmx); 664 _mm_store_ps(r.mi,ami); 665 _mm_store_ps(r.mx,amx); 658 666 #else 659 for(int i=0;i<3;++i)660 { 661 if(a.mi[i]<b.mi[i]) r.mi[i]=a.mi[i]; else r.mi[i]=b.mi[i];662 if(a.mx[i]>b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i];667 for(int i=0;i<3;++i) 668 { 669 if(a.mi[i]<b.mi[i]) r.mi[i]=a.mi[i]; else r.mi[i]=b.mi[i]; 670 if(a.mx[i]>b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i]; 663 671 } 664 672 #endif … … 667 675 // 668 676 DBVT_INLINE bool NotEqual( const btDbvtAabbMm& a, 669 670 { 671 return( (a.mi.x()!=b.mi.x())||677 const btDbvtAabbMm& b) 678 { 679 return( (a.mi.x()!=b.mi.x())|| 672 680 (a.mi.y()!=b.mi.y())|| 673 681 (a.mi.z()!=b.mi.z())|| … … 684 692 DBVT_PREFIX 685 693 inline void btDbvt::enumNodes( const btDbvtNode* root, 686 687 { 688 DBVT_CHECKTYPE689 policy.Process(root);690 if(root->isinternal())691 { 692 enumNodes(root->childs[0],policy);693 enumNodes(root->childs[1],policy);694 DBVT_IPOLICY) 695 { 696 DBVT_CHECKTYPE 697 policy.Process(root); 698 if(root->isinternal()) 699 { 700 enumNodes(root->childs[0],policy); 701 enumNodes(root->childs[1],policy); 694 702 } 695 703 } … … 698 706 DBVT_PREFIX 699 707 inline void btDbvt::enumLeaves( const btDbvtNode* root, 700 701 { 702 DBVT_CHECKTYPE703 if(root->isinternal())704 {705 enumLeaves(root->childs[0],policy);706 enumLeaves(root->childs[1],policy);707 }708 else709 {710 policy.Process(root);711 }708 DBVT_IPOLICY) 709 { 710 DBVT_CHECKTYPE 711 if(root->isinternal()) 712 { 713 enumLeaves(root->childs[0],policy); 714 enumLeaves(root->childs[1],policy); 715 } 716 else 717 { 718 policy.Process(root); 719 } 712 720 } 713 721 … … 715 723 DBVT_PREFIX 716 724 inline void btDbvt::collideTT( const btDbvtNode* root0, 717 const btDbvtNode* root1, 718 DBVT_IPOLICY) 719 { 720 DBVT_CHECKTYPE 721 if(root0&&root1) 722 { 723 btAlignedObjectArray<sStkNN> stack; 724 int depth=1; 725 int treshold=DOUBLE_STACKSIZE-4; 726 stack.resize(DOUBLE_STACKSIZE); 727 stack[0]=sStkNN(root0,root1); 728 do { 729 sStkNN p=stack[--depth]; 730 if(depth>treshold) 725 const btDbvtNode* root1, 726 DBVT_IPOLICY) 727 { 728 DBVT_CHECKTYPE 729 if(root0&&root1) 730 { 731 int depth=1; 732 int treshold=DOUBLE_STACKSIZE-4; 733 btAlignedObjectArray<sStkNN> stkStack; 734 stkStack.resize(DOUBLE_STACKSIZE); 735 stkStack[0]=sStkNN(root0,root1); 736 do { 737 sStkNN p=stkStack[--depth]; 738 if(depth>treshold) 739 { 740 stkStack.resize(stkStack.size()*2); 741 treshold=stkStack.size()-4; 742 } 743 if(p.a==p.b) 744 { 745 if(p.a->isinternal()) 746 { 747 stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]); 748 stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]); 749 stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]); 750 } 751 } 752 else if(Intersect(p.a->volume,p.b->volume)) 753 { 754 if(p.a->isinternal()) 755 { 756 if(p.b->isinternal()) 757 { 758 stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); 759 stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); 760 stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); 761 stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); 762 } 763 else 764 { 765 stkStack[depth++]=sStkNN(p.a->childs[0],p.b); 766 stkStack[depth++]=sStkNN(p.a->childs[1],p.b); 767 } 768 } 769 else 770 { 771 if(p.b->isinternal()) 772 { 773 stkStack[depth++]=sStkNN(p.a,p.b->childs[0]); 774 stkStack[depth++]=sStkNN(p.a,p.b->childs[1]); 775 } 776 else 777 { 778 policy.Process(p.a,p.b); 779 } 780 } 781 } 782 } while(depth); 783 } 784 } 785 786 787 788 DBVT_PREFIX 789 inline void btDbvt::collideTTpersistentStack( const btDbvtNode* root0, 790 const btDbvtNode* root1, 791 DBVT_IPOLICY) 792 { 793 DBVT_CHECKTYPE 794 if(root0&&root1) 795 { 796 int depth=1; 797 int treshold=DOUBLE_STACKSIZE-4; 798 799 m_stkStack.resize(DOUBLE_STACKSIZE); 800 m_stkStack[0]=sStkNN(root0,root1); 801 do { 802 sStkNN p=m_stkStack[--depth]; 803 if(depth>treshold) 804 { 805 m_stkStack.resize(m_stkStack.size()*2); 806 treshold=m_stkStack.size()-4; 807 } 808 if(p.a==p.b) 809 { 810 if(p.a->isinternal()) 811 { 812 m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]); 813 m_stkStack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]); 814 m_stkStack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]); 815 } 816 } 817 else if(Intersect(p.a->volume,p.b->volume)) 818 { 819 if(p.a->isinternal()) 820 { 821 if(p.b->isinternal()) 822 { 823 m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); 824 m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); 825 m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); 826 m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); 827 } 828 else 829 { 830 m_stkStack[depth++]=sStkNN(p.a->childs[0],p.b); 831 m_stkStack[depth++]=sStkNN(p.a->childs[1],p.b); 832 } 833 } 834 else 835 { 836 if(p.b->isinternal()) 837 { 838 m_stkStack[depth++]=sStkNN(p.a,p.b->childs[0]); 839 m_stkStack[depth++]=sStkNN(p.a,p.b->childs[1]); 840 } 841 else 842 { 843 policy.Process(p.a,p.b); 844 } 845 } 846 } 847 } while(depth); 848 } 849 } 850 851 852 // 853 DBVT_PREFIX 854 inline void btDbvt::collideTT( const btDbvtNode* root0, 855 const btDbvtNode* root1, 856 const btTransform& xform, 857 DBVT_IPOLICY) 858 { 859 DBVT_CHECKTYPE 860 if(root0&&root1) 861 { 862 int depth=1; 863 int treshold=DOUBLE_STACKSIZE-4; 864 btAlignedObjectArray<sStkNN> stkStack; 865 stkStack.resize(DOUBLE_STACKSIZE); 866 stkStack[0]=sStkNN(root0,root1); 867 do { 868 sStkNN p=stkStack[--depth]; 869 if(Intersect(p.a->volume,p.b->volume,xform)) 870 { 871 if(depth>treshold) 872 { 873 stkStack.resize(stkStack.size()*2); 874 treshold=stkStack.size()-4; 875 } 876 if(p.a->isinternal()) 877 { 878 if(p.b->isinternal()) 879 { 880 stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); 881 stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); 882 stkStack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); 883 stkStack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); 884 } 885 else 886 { 887 stkStack[depth++]=sStkNN(p.a->childs[0],p.b); 888 stkStack[depth++]=sStkNN(p.a->childs[1],p.b); 889 } 890 } 891 else 892 { 893 if(p.b->isinternal()) 894 { 895 stkStack[depth++]=sStkNN(p.a,p.b->childs[0]); 896 stkStack[depth++]=sStkNN(p.a,p.b->childs[1]); 897 } 898 else 899 { 900 policy.Process(p.a,p.b); 901 } 902 } 903 } 904 } while(depth); 905 } 906 } 907 908 // 909 DBVT_PREFIX 910 inline void btDbvt::collideTT( const btDbvtNode* root0, 911 const btTransform& xform0, 912 const btDbvtNode* root1, 913 const btTransform& xform1, 914 DBVT_IPOLICY) 915 { 916 const btTransform xform=xform0.inverse()*xform1; 917 collideTT(root0,root1,xform,policy); 918 } 919 920 // 921 DBVT_PREFIX 922 inline void btDbvt::collideTV( const btDbvtNode* root, 923 const btDbvtVolume& vol, 924 DBVT_IPOLICY) 925 { 926 DBVT_CHECKTYPE 927 if(root) 928 { 929 ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol); 930 btAlignedObjectArray<const btDbvtNode*> stack; 931 stack.resize(0); 932 stack.reserve(SIMPLE_STACKSIZE); 933 stack.push_back(root); 934 do { 935 const btDbvtNode* n=stack[stack.size()-1]; 936 stack.pop_back(); 937 if(Intersect(n->volume,volume)) 938 { 939 if(n->isinternal()) 940 { 941 stack.push_back(n->childs[0]); 942 stack.push_back(n->childs[1]); 943 } 944 else 945 { 946 policy.Process(n); 947 } 948 } 949 } while(stack.size()>0); 950 } 951 } 952 953 DBVT_PREFIX 954 inline void btDbvt::rayTestInternal( const btDbvtNode* root, 955 const btVector3& rayFrom, 956 const btVector3& rayTo, 957 const btVector3& rayDirectionInverse, 958 unsigned int signs[3], 959 btScalar lambda_max, 960 const btVector3& aabbMin, 961 const btVector3& aabbMax, 962 DBVT_IPOLICY) const 963 { 964 DBVT_CHECKTYPE 965 if(root) 966 { 967 btVector3 resultNormal; 968 969 int depth=1; 970 int treshold=DOUBLE_STACKSIZE-2; 971 btAlignedObjectArray<const btDbvtNode*> stack; 972 stack.resize(DOUBLE_STACKSIZE); 973 stack[0]=root; 974 btVector3 bounds[2]; 975 do 976 { 977 const btDbvtNode* node=stack[--depth]; 978 bounds[0] = node->volume.Mins()+aabbMin; 979 bounds[1] = node->volume.Maxs()+aabbMax; 980 btScalar tmin=1.f,lambda_min=0.f; 981 unsigned int result1=false; 982 result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max); 983 if(result1) 731 984 { 732 stack.resize(stack.size()*2); 733 treshold=stack.size()-4; 734 } 735 if(p.a==p.b) 736 { 737 if(p.a->isinternal()) 738 { 739 stack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]); 740 stack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]); 741 stack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]); 742 } 743 } 744 else if(Intersect(p.a->volume,p.b->volume)) 745 { 746 if(p.a->isinternal()) 747 { 748 if(p.b->isinternal()) 749 { 750 stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); 751 stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); 752 stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); 753 stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); 754 } 755 else 756 { 757 stack[depth++]=sStkNN(p.a->childs[0],p.b); 758 stack[depth++]=sStkNN(p.a->childs[1],p.b); 759 } 985 if(node->isinternal()) 986 { 987 if(depth>treshold) 988 { 989 stack.resize(stack.size()*2); 990 treshold=stack.size()-2; 991 } 992 stack[depth++]=node->childs[0]; 993 stack[depth++]=node->childs[1]; 760 994 } 761 995 else 762 996 { 763 if(p.b->isinternal()) 764 { 765 stack[depth++]=sStkNN(p.a,p.b->childs[0]); 766 stack[depth++]=sStkNN(p.a,p.b->childs[1]); 767 } 768 else 769 { 770 policy.Process(p.a,p.b); 771 } 997 policy.Process(node); 772 998 } 773 999 } … … 778 1004 // 779 1005 DBVT_PREFIX 780 inline void btDbvt::collideTT( const btDbvtNode* root0, 781 const btDbvtNode* root1, 782 const btTransform& xform, 783 DBVT_IPOLICY) 784 { 785 DBVT_CHECKTYPE 786 if(root0&&root1) 787 { 788 btAlignedObjectArray<sStkNN> stack; 789 int depth=1; 790 int treshold=DOUBLE_STACKSIZE-4; 791 stack.resize(DOUBLE_STACKSIZE); 792 stack[0]=sStkNN(root0,root1); 793 do { 794 sStkNN p=stack[--depth]; 795 if(Intersect(p.a->volume,p.b->volume,xform)) 796 { 797 if(depth>treshold) 798 { 799 stack.resize(stack.size()*2); 800 treshold=stack.size()-4; 801 } 802 if(p.a->isinternal()) 803 { 804 if(p.b->isinternal()) 805 { 806 stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); 807 stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); 808 stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); 809 stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); 1006 inline void btDbvt::rayTest( const btDbvtNode* root, 1007 const btVector3& rayFrom, 1008 const btVector3& rayTo, 1009 DBVT_IPOLICY) 1010 { 1011 DBVT_CHECKTYPE 1012 if(root) 1013 { 1014 btVector3 rayDir = (rayTo-rayFrom); 1015 rayDir.normalize (); 1016 1017 ///what about division by zero? --> just set rayDirection[i] to INF/1e30 1018 btVector3 rayDirectionInverse; 1019 rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; 1020 rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; 1021 rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; 1022 unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0}; 1023 1024 btScalar lambda_max = rayDir.dot(rayTo-rayFrom); 1025 1026 btVector3 resultNormal; 1027 1028 btAlignedObjectArray<const btDbvtNode*> stack; 1029 1030 int depth=1; 1031 int treshold=DOUBLE_STACKSIZE-2; 1032 1033 stack.resize(DOUBLE_STACKSIZE); 1034 stack[0]=root; 1035 btVector3 bounds[2]; 1036 do { 1037 const btDbvtNode* node=stack[--depth]; 1038 1039 bounds[0] = node->volume.Mins(); 1040 bounds[1] = node->volume.Maxs(); 1041 1042 btScalar tmin=1.f,lambda_min=0.f; 1043 unsigned int result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max); 1044 1045 #ifdef COMPARE_BTRAY_AABB2 1046 btScalar param=1.f; 1047 bool result2 = btRayAabb(rayFrom,rayTo,node->volume.Mins(),node->volume.Maxs(),param,resultNormal); 1048 btAssert(result1 == result2); 1049 #endif //TEST_BTRAY_AABB2 1050 1051 if(result1) 1052 { 1053 if(node->isinternal()) 1054 { 1055 if(depth>treshold) 1056 { 1057 stack.resize(stack.size()*2); 1058 treshold=stack.size()-2; 1059 } 1060 stack[depth++]=node->childs[0]; 1061 stack[depth++]=node->childs[1]; 810 1062 } 811 1063 else 812 1064 { 813 stack[depth++]=sStkNN(p.a->childs[0],p.b); 814 stack[depth++]=sStkNN(p.a->childs[1],p.b); 815 } 816 } 817 else 818 { 819 if(p.b->isinternal()) 820 { 821 stack[depth++]=sStkNN(p.a,p.b->childs[0]); 822 stack[depth++]=sStkNN(p.a,p.b->childs[1]); 823 } 824 else 825 { 826 policy.Process(p.a,p.b); 827 } 828 } 829 } 830 } while(depth); 831 } 832 } 833 834 // 835 DBVT_PREFIX 836 inline void btDbvt::collideTT( const btDbvtNode* root0, 837 const btTransform& xform0, 838 const btDbvtNode* root1, 839 const btTransform& xform1, 840 DBVT_IPOLICY) 841 { 842 const btTransform xform=xform0.inverse()*xform1; 843 collideTT(root0,root1,xform,policy); 844 } 845 846 // 847 DBVT_PREFIX 848 inline void btDbvt::collideTV( const btDbvtNode* root, 849 const btDbvtVolume& vol, 850 DBVT_IPOLICY) 851 { 852 DBVT_CHECKTYPE 853 if(root) 854 { 855 ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol); 856 btAlignedObjectArray<const btDbvtNode*> stack; 857 stack.reserve(SIMPLE_STACKSIZE); 858 stack.push_back(root); 859 do { 860 const btDbvtNode* n=stack[stack.size()-1]; 861 stack.pop_back(); 862 if(Intersect(n->volume,volume)) 863 { 864 if(n->isinternal()) 865 { 866 stack.push_back(n->childs[0]); 867 stack.push_back(n->childs[1]); 868 } 869 else 870 { 871 policy.Process(n); 872 } 873 } 874 } while(stack.size()>0); 875 } 876 } 877 878 // 879 DBVT_PREFIX 880 inline void btDbvt::collideRAY( const btDbvtNode* root, 881 const btVector3& origin, 882 const btVector3& direction, 883 DBVT_IPOLICY) 884 { 885 DBVT_CHECKTYPE 886 if(root) 887 { 888 const btVector3 normal=direction.normalized(); 889 const btVector3 invdir( 1/normal.x(), 890 1/normal.y(), 891 1/normal.z()); 892 const unsigned signs[]={ direction.x()<0, 893 direction.y()<0, 894 direction.z()<0}; 895 btAlignedObjectArray<const btDbvtNode*> stack; 896 stack.reserve(SIMPLE_STACKSIZE); 897 stack.push_back(root); 898 do { 899 const btDbvtNode* node=stack[stack.size()-1]; 900 stack.pop_back(); 901 if(Intersect(node->volume,origin,invdir,signs)) 902 { 903 if(node->isinternal()) 904 { 905 stack.push_back(node->childs[0]); 906 stack.push_back(node->childs[1]); 907 } 908 else 909 { 910 policy.Process(node); 911 } 912 } 913 } while(stack.size()); 914 } 1065 policy.Process(node); 1066 } 1067 } 1068 } while(depth); 1069 1070 } 915 1071 } 916 1072 … … 923 1079 DBVT_IPOLICY) 924 1080 { 925 DBVT_CHECKTYPE 926 if(root) 927 { 928 const int inside=(1<<count)-1; 929 btAlignedObjectArray<sStkNP> stack; 930 int signs[sizeof(unsigned)*8]; 931 btAssert(count<int (sizeof(signs)/sizeof(signs[0]))); 932 for(int i=0;i<count;++i) 1081 DBVT_CHECKTYPE 1082 if(root) 933 1083 { 934 signs[i]= ((normals[i].x()>=0)?1:0)+ 1084 const int inside=(1<<count)-1; 1085 btAlignedObjectArray<sStkNP> stack; 1086 int signs[sizeof(unsigned)*8]; 1087 btAssert(count<int (sizeof(signs)/sizeof(signs[0]))); 1088 for(int i=0;i<count;++i) 1089 { 1090 signs[i]= ((normals[i].x()>=0)?1:0)+ 935 1091 ((normals[i].y()>=0)?2:0)+ 936 1092 ((normals[i].z()>=0)?4:0); 1093 } 1094 stack.reserve(SIMPLE_STACKSIZE); 1095 stack.push_back(sStkNP(root,0)); 1096 do { 1097 sStkNP se=stack[stack.size()-1]; 1098 bool out=false; 1099 stack.pop_back(); 1100 for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1) 1101 { 1102 if(0==(se.mask&j)) 1103 { 1104 const int side=se.node->volume.Classify(normals[i],offsets[i],signs[i]); 1105 switch(side) 1106 { 1107 case -1: out=true;break; 1108 case +1: se.mask|=j;break; 1109 } 1110 } 1111 } 1112 if(!out) 1113 { 1114 if((se.mask!=inside)&&(se.node->isinternal())) 1115 { 1116 stack.push_back(sStkNP(se.node->childs[0],se.mask)); 1117 stack.push_back(sStkNP(se.node->childs[1],se.mask)); 1118 } 1119 else 1120 { 1121 if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy); 1122 } 1123 } 1124 } while(stack.size()); 937 1125 } 938 stack.reserve(SIMPLE_STACKSIZE);939 stack.push_back(sStkNP(root,0));940 do {941 sStkNP se=stack[stack.size()-1];942 bool out=false;943 stack.pop_back();944 for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)945 {946 if(0==(se.mask&j))947 {948 const int side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);949 switch(side)950 {951 case -1: out=true;break;952 case +1: se.mask|=j;break;953 }954 }955 }956 if(!out)957 {958 if((se.mask!=inside)&&(se.node->isinternal()))959 {960 stack.push_back(sStkNP(se.node->childs[0],se.mask));961 stack.push_back(sStkNP(se.node->childs[1],se.mask));962 }963 else964 {965 if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy);966 }967 }968 } while(stack.size());969 }970 1126 } 971 1127 … … 973 1129 DBVT_PREFIX 974 1130 inline void btDbvt::collideOCL( const btDbvtNode* root, 975 const btVector3* normals, 976 const btScalar* offsets, 977 const btVector3& sortaxis, 978 int count, 979 DBVT_IPOLICY, 980 bool fsort) 981 { 982 DBVT_CHECKTYPE 983 if(root) 984 { 985 const unsigned srtsgns=(sortaxis[0]>=0?1:0)+ 986 (sortaxis[1]>=0?2:0)+ 987 (sortaxis[2]>=0?4:0); 988 const int inside=(1<<count)-1; 989 btAlignedObjectArray<sStkNPS> stock; 990 btAlignedObjectArray<int> ifree; 991 btAlignedObjectArray<int> stack; 992 int signs[sizeof(unsigned)*8]; 993 btAssert(count<int (sizeof(signs)/sizeof(signs[0]))); 994 for(int i=0;i<count;++i) 1131 const btVector3* normals, 1132 const btScalar* offsets, 1133 const btVector3& sortaxis, 1134 int count, 1135 DBVT_IPOLICY, 1136 bool fsort) 1137 { 1138 DBVT_CHECKTYPE 1139 if(root) 995 1140 { 996 signs[i]= ((normals[i].x()>=0)?1:0)+ 1141 const unsigned srtsgns=(sortaxis[0]>=0?1:0)+ 1142 (sortaxis[1]>=0?2:0)+ 1143 (sortaxis[2]>=0?4:0); 1144 const int inside=(1<<count)-1; 1145 btAlignedObjectArray<sStkNPS> stock; 1146 btAlignedObjectArray<int> ifree; 1147 btAlignedObjectArray<int> stack; 1148 int signs[sizeof(unsigned)*8]; 1149 btAssert(count<int (sizeof(signs)/sizeof(signs[0]))); 1150 for(int i=0;i<count;++i) 1151 { 1152 signs[i]= ((normals[i].x()>=0)?1:0)+ 997 1153 ((normals[i].y()>=0)?2:0)+ 998 1154 ((normals[i].z()>=0)?4:0); 1155 } 1156 stock.reserve(SIMPLE_STACKSIZE); 1157 stack.reserve(SIMPLE_STACKSIZE); 1158 ifree.reserve(SIMPLE_STACKSIZE); 1159 stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns)))); 1160 do { 1161 const int id=stack[stack.size()-1]; 1162 sStkNPS se=stock[id]; 1163 stack.pop_back();ifree.push_back(id); 1164 if(se.mask!=inside) 1165 { 1166 bool out=false; 1167 for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1) 1168 { 1169 if(0==(se.mask&j)) 1170 { 1171 const int side=se.node->volume.Classify(normals[i],offsets[i],signs[i]); 1172 switch(side) 1173 { 1174 case -1: out=true;break; 1175 case +1: se.mask|=j;break; 1176 } 1177 } 1178 } 1179 if(out) continue; 1180 } 1181 if(policy.Descent(se.node)) 1182 { 1183 if(se.node->isinternal()) 1184 { 1185 const btDbvtNode* pns[]={ se.node->childs[0],se.node->childs[1]}; 1186 sStkNPS nes[]={ sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)), 1187 sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))}; 1188 const int q=nes[0].value<nes[1].value?1:0; 1189 int j=stack.size(); 1190 if(fsort&&(j>0)) 1191 { 1192 /* Insert 0 */ 1193 j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size()); 1194 stack.push_back(0); 1195 #if DBVT_USE_MEMMOVE 1196 memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1)); 1197 #else 1198 for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1]; 1199 #endif 1200 stack[j]=allocate(ifree,stock,nes[q]); 1201 /* Insert 1 */ 1202 j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size()); 1203 stack.push_back(0); 1204 #if DBVT_USE_MEMMOVE 1205 memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1)); 1206 #else 1207 for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1]; 1208 #endif 1209 stack[j]=allocate(ifree,stock,nes[1-q]); 1210 } 1211 else 1212 { 1213 stack.push_back(allocate(ifree,stock,nes[q])); 1214 stack.push_back(allocate(ifree,stock,nes[1-q])); 1215 } 1216 } 1217 else 1218 { 1219 policy.Process(se.node,se.value); 1220 } 1221 } 1222 } while(stack.size()); 999 1223 } 1000 stock.reserve(SIMPLE_STACKSIZE);1001 stack.reserve(SIMPLE_STACKSIZE);1002 ifree.reserve(SIMPLE_STACKSIZE);1003 stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns))));1004 do {1005 const int id=stack[stack.size()-1];1006 sStkNPS se=stock[id];1007 stack.pop_back();ifree.push_back(id);1008 if(se.mask!=inside)1009 {1010 bool out=false;1011 for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1)1012 {1013 if(0==(se.mask&j))1014 {1015 const int side=se.node->volume.Classify(normals[i],offsets[i],signs[i]);1016 switch(side)1017 {1018 case -1: out=true;break;1019 case +1: se.mask|=j;break;1020 }1021 }1022 }1023 if(out) continue;1024 }1025 if(policy.Descent(se.node))1026 {1027 if(se.node->isinternal())1028 {1029 const btDbvtNode* pns[]={ se.node->childs[0],se.node->childs[1]};1030 sStkNPS nes[]={ sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)),1031 sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))};1032 const int q=nes[0].value<nes[1].value?1:0;1033 int j=stack.size();1034 if(fsort&&(j>0))1035 {1036 /* Insert 0 */1037 j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size());1038 stack.push_back(0);1039 #if DBVT_USE_MEMMOVE1040 memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));1041 #else1042 for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];1043 #endif1044 stack[j]=allocate(ifree,stock,nes[q]);1045 /* Insert 1 */1046 j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size());1047 stack.push_back(0);1048 #if DBVT_USE_MEMMOVE1049 memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1));1050 #else1051 for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1];1052 #endif1053 stack[j]=allocate(ifree,stock,nes[1-q]);1054 }1055 else1056 {1057 stack.push_back(allocate(ifree,stock,nes[q]));1058 stack.push_back(allocate(ifree,stock,nes[1-q]));1059 }1060 }1061 else1062 {1063 policy.Process(se.node,se.value);1064 }1065 }1066 } while(stack.size());1067 }1068 1224 } 1069 1225 … … 1071 1227 DBVT_PREFIX 1072 1228 inline void btDbvt::collideTU( const btDbvtNode* root, 1073 1074 { 1075 DBVT_CHECKTYPE1076 if(root)1077 {1078 btAlignedObjectArray<const btDbvtNode*> stack;1079 stack.reserve(SIMPLE_STACKSIZE);1080 stack.push_back(root);1081 do {1082 const btDbvtNode* n=stack[stack.size()-1];1083 stack.pop_back();1084 if(policy.Descent(n))1085 {1086 if(n->isinternal())1087 { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); }1088 else1089 { policy.Process(n); }1090 }1091 } while(stack.size()>0);1092 }1229 DBVT_IPOLICY) 1230 { 1231 DBVT_CHECKTYPE 1232 if(root) 1233 { 1234 btAlignedObjectArray<const btDbvtNode*> stack; 1235 stack.reserve(SIMPLE_STACKSIZE); 1236 stack.push_back(root); 1237 do { 1238 const btDbvtNode* n=stack[stack.size()-1]; 1239 stack.pop_back(); 1240 if(policy.Descent(n)) 1241 { 1242 if(n->isinternal()) 1243 { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); } 1244 else 1245 { policy.Process(n); } 1246 } 1247 } while(stack.size()>0); 1248 } 1093 1249 } 1094 1250
Note: See TracChangeset
for help on using the changeset viewer.
