- Timestamp:
- Oct 20, 2008, 5:40:38 PM (17 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
code/branches/physics/src/bullet/BulletCollision/BroadphaseCollision/btDbvt.h
r1963 r1972 21 21 #include "LinearMath/btVector3.h" 22 22 #include "LinearMath/btTransform.h" 23 #include "LinearMath/btAabbUtil2.h"24 23 25 24 // … … 34 33 // Template implementation of ICollide 35 34 #ifdef WIN32 36 #if (defined (_MSC_VER) && _MSC_VER >= 1400)37 #define DBVT_USE_TEMPLATE 138 #else39 #define DBVT_USE_TEMPLATE 035 #if (defined (_MSC_VER) && _MSC_VER >= 1400) 36 #define DBVT_USE_TEMPLATE 1 37 #else 38 #define DBVT_USE_TEMPLATE 0 40 39 #endif 41 40 #else … … 136 135 struct btDbvtAabbMm 137 136 { 138 DBVT_INLINE btVector3 Center() const { return((mi+mx)/2); } 139 DBVT_INLINE btVector3 Lengths() const { return(mx-mi); } 140 DBVT_INLINE btVector3 Extents() const { return((mx-mi)/2); } 141 DBVT_INLINE const btVector3& Mins() const { return(mi); } 142 DBVT_INLINE const btVector3& Maxs() const { return(mx); } 143 static inline btDbvtAabbMm FromCE(const btVector3& c,const btVector3& e); 144 static inline btDbvtAabbMm FromCR(const btVector3& c,btScalar r); 145 static inline btDbvtAabbMm FromMM(const btVector3& mi,const btVector3& mx); 146 static inline btDbvtAabbMm FromPoints(const btVector3* pts,int n); 147 static inline btDbvtAabbMm FromPoints(const btVector3** ppts,int n); 148 DBVT_INLINE void Expand(const btVector3& e); 149 DBVT_INLINE void SignedExpand(const btVector3& e); 150 DBVT_INLINE bool Contain(const btDbvtAabbMm& a) const; 151 DBVT_INLINE int Classify(const btVector3& n,btScalar o,int s) const; 152 DBVT_INLINE btScalar ProjectMinimum(const btVector3& v,unsigned signs) const; 153 DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, 154 const btDbvtAabbMm& b); 155 DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, 156 const btDbvtAabbMm& b, 157 const btTransform& xform); 158 DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, 159 const btVector3& b); 160 161 DBVT_INLINE friend btScalar Proximity( const btDbvtAabbMm& a, 162 const btDbvtAabbMm& b); 163 DBVT_INLINE friend int Select( const btDbvtAabbMm& o, 164 const btDbvtAabbMm& a, 165 const btDbvtAabbMm& b); 166 DBVT_INLINE friend void Merge( const btDbvtAabbMm& a, 167 const btDbvtAabbMm& b, 168 btDbvtAabbMm& r); 169 DBVT_INLINE friend bool NotEqual( const btDbvtAabbMm& a, 170 const btDbvtAabbMm& b); 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); 171 173 private: 172 174 DBVT_INLINE void AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const; 173 175 private: 174 176 btVector3 mi,mx; 175 177 }; 176 178 … … 186 188 DBVT_INLINE bool isinternal() const { return(!isleaf()); } 187 189 union { 188 btDbvtNode* childs[2];189 void* data;190 int dataAsInt;191 };190 btDbvtNode* childs[2]; 191 void* data; 192 int dataAsInt; 193 }; 192 194 }; 193 195 … … 196 198 ///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure. 197 199 struct btDbvt 198 {200 { 199 201 /* Stack element */ 200 202 struct sStkNN 201 {203 { 202 204 const btDbvtNode* a; 203 205 const btDbvtNode* b; 204 206 sStkNN() {} 205 207 sStkNN(const btDbvtNode* na,const btDbvtNode* nb) : a(na),b(nb) {} 206 };208 }; 207 209 struct sStkNP 208 {210 { 209 211 const btDbvtNode* node; 210 212 int mask; 211 213 sStkNP(const btDbvtNode* n,unsigned m) : node(n),mask(m) {} 212 };214 }; 213 215 struct sStkNPS 214 {216 { 215 217 const btDbvtNode* node; 216 218 int mask; … … 218 220 sStkNPS() {} 219 221 sStkNPS(const btDbvtNode* n,unsigned m,btScalar v) : node(n),mask(m),value(v) {} 220 };222 }; 221 223 struct sStkCLN 222 {224 { 223 225 const btDbvtNode* node; 224 226 btDbvtNode* parent; 225 227 sStkCLN(const btDbvtNode* n,btDbvtNode* p) : node(n),parent(p) {} 226 };228 }; 227 229 // Policies/Interfaces 228 230 229 231 /* ICollide */ 230 232 struct ICollide 231 {233 { 232 234 DBVT_VIRTUAL_DTOR(ICollide) 233 235 DBVT_VIRTUAL void Process(const btDbvtNode*,const btDbvtNode*) {} 234 236 DBVT_VIRTUAL void Process(const btDbvtNode*) {} 235 237 DBVT_VIRTUAL void Process(const btDbvtNode* n,btScalar) { Process(n); } 236 238 DBVT_VIRTUAL bool Descent(const btDbvtNode*) { return(true); } 237 239 DBVT_VIRTUAL bool AllLeaves(const btDbvtNode*) { return(true); } 238 };240 }; 239 241 /* IWriter */ 240 242 struct IWriter 241 {243 { 242 244 virtual ~IWriter() {} 243 245 virtual void Prepare(const btDbvtNode* root,int numnodes)=0; 244 246 virtual void WriteNode(const btDbvtNode*,int index,int parent,int child0,int child1)=0; 245 247 virtual void WriteLeaf(const btDbvtNode*,int index,int parent)=0; 246 };248 }; 247 249 /* IClone */ 248 250 struct IClone 249 {251 { 250 252 virtual ~IClone() {} 251 253 virtual void CloneLeaf(btDbvtNode*) {} 252 };253 254 }; 255 254 256 // Constants 255 257 enum { 256 SIMPLE_STACKSIZE = 64,257 DOUBLE_STACKSIZE = SIMPLE_STACKSIZE*2258 };259 258 SIMPLE_STACKSIZE = 64, 259 DOUBLE_STACKSIZE = SIMPLE_STACKSIZE*2 260 }; 261 260 262 // Fields 261 263 btDbvtNode* m_root; … … 265 267 unsigned m_opath; 266 268 // Methods 267 btDbvt();268 ~btDbvt();269 btDbvt(); 270 ~btDbvt(); 269 271 void clear(); 270 272 bool empty() const { return(0==m_root); } … … 284 286 static int countLeaves(const btDbvtNode* node); 285 287 static void extractLeaves(const btDbvtNode* node,btAlignedObjectArray<const btDbvtNode*>& leaves); 286 #if DBVT_ENABLE_BENCHMARK288 #if DBVT_ENABLE_BENCHMARK 287 289 static void benchmark(); 288 #else290 #else 289 291 static void benchmark(){} 290 #endif292 #endif 291 293 // DBVT_IPOLICY must support ICollide policy/interface 292 294 DBVT_PREFIX 293 294 DBVT_IPOLICY);295 static void enumNodes( const btDbvtNode* root, 296 DBVT_IPOLICY); 295 297 DBVT_PREFIX 296 297 DBVT_IPOLICY);298 static void enumLeaves( const btDbvtNode* root, 299 DBVT_IPOLICY); 298 300 DBVT_PREFIX 299 300 const btDbvtNode* root1,301 DBVT_IPOLICY);301 static void collideTT( const btDbvtNode* root0, 302 const btDbvtNode* root1, 303 DBVT_IPOLICY); 302 304 DBVT_PREFIX 303 304 const btDbvtNode* root1,305 const btTransform& xform,306 DBVT_IPOLICY);305 static void collideTT( const btDbvtNode* root0, 306 const btDbvtNode* root1, 307 const btTransform& xform, 308 DBVT_IPOLICY); 307 309 DBVT_PREFIX 308 309 const btTransform& xform0,310 const btDbvtNode* root1,311 const btTransform& xform1,312 DBVT_IPOLICY);310 static void collideTT( const btDbvtNode* root0, 311 const btTransform& xform0, 312 const btDbvtNode* root1, 313 const btTransform& xform1, 314 DBVT_IPOLICY); 313 315 DBVT_PREFIX 314 315 const btDbvtVolume& volume,316 DBVT_IPOLICY);316 static void collideTV( const btDbvtNode* root, 317 const btDbvtVolume& volume, 318 DBVT_IPOLICY); 317 319 DBVT_PREFIX 318 static void rayTest( const btDbvtNode* root,319 const btVector3& rayFrom,320 const btVector3& rayTo,321 DBVT_IPOLICY);320 static void collideRAY( const btDbvtNode* root, 321 const btVector3& origin, 322 const btVector3& direction, 323 DBVT_IPOLICY); 322 324 DBVT_PREFIX 323 324 const btVector3* normals,325 const btScalar* offsets,326 int count,327 DBVT_IPOLICY);325 static void collideKDOP(const btDbvtNode* root, 326 const btVector3* normals, 327 const btScalar* offsets, 328 int count, 329 DBVT_IPOLICY); 328 330 DBVT_PREFIX 329 330 const btVector3* normals,331 const btScalar* offsets,332 const btVector3& sortaxis,333 int count,334 DBVT_IPOLICY,335 bool fullsort=true);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); 336 338 DBVT_PREFIX 337 338 DBVT_IPOLICY);339 static void collideTU( const btDbvtNode* root, 340 DBVT_IPOLICY); 339 341 // Helpers 340 342 static DBVT_INLINE int nearest(const int* i,const btDbvt::sStkNPS* a,btScalar v,int l,int h) 341 {343 { 342 344 int m=0; 343 345 while(l<h) 344 {346 { 345 347 m=(l+h)>>1; 346 348 if(a[i[m]].value>=v) l=m+1; else h=m; 349 } 350 return(h); 347 351 } 348 return(h);349 }350 352 static DBVT_INLINE int allocate( btAlignedObjectArray<int>& ifree, 351 btAlignedObjectArray<sStkNPS>& stock,352 const sStkNPS& value)353 {353 btAlignedObjectArray<sStkNPS>& stock, 354 const sStkNPS& value) 355 { 354 356 int i; 355 357 if(ifree.size()>0) 356 { i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; }357 else358 { i=stock.size();stock.push_back(value); }358 { i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; } 359 else 360 { i=stock.size();stock.push_back(value); } 359 361 return(i); 360 }362 } 361 363 // 362 private:363 btDbvt(const btDbvt&) {}364 };364 private: 365 btDbvt(const btDbvt&) {} 366 }; 365 367 366 368 // … … 371 373 inline btDbvtAabbMm btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e) 372 374 { 373 374 375 376 } 377 375 btDbvtAabbMm box; 376 box.mi=c-e;box.mx=c+e; 377 return(box); 378 } 379 378 380 // 379 381 inline btDbvtAabbMm btDbvtAabbMm::FromCR(const btVector3& c,btScalar r) 380 382 { 381 382 } 383 383 return(FromCE(c,btVector3(r,r,r))); 384 } 385 384 386 // 385 387 inline btDbvtAabbMm btDbvtAabbMm::FromMM(const btVector3& mi,const btVector3& mx) 386 388 { 387 388 389 390 } 391 389 btDbvtAabbMm box; 390 box.mi=mi;box.mx=mx; 391 return(box); 392 } 393 392 394 // 393 395 inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3* pts,int n) 394 396 { 395 396 397 398 { 399 400 401 } 402 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]); 403 } 404 return(box); 403 405 } 404 406 … … 406 408 inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3** ppts,int n) 407 409 { 408 409 410 411 { 412 413 414 } 415 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]); 416 } 417 return(box); 416 418 } 417 419 … … 419 421 DBVT_INLINE void btDbvtAabbMm::Expand(const btVector3& e) 420 422 { 421 422 } 423 423 mi-=e;mx+=e; 424 } 425 424 426 // 425 427 DBVT_INLINE void btDbvtAabbMm::SignedExpand(const btVector3& e) 426 428 { 427 428 429 430 } 431 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 432 434 // 433 435 DBVT_INLINE bool btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const 434 436 { 435 437 return( (mi.x()<=a.mi.x())&& 436 438 (mi.y()<=a.mi.y())&& 437 439 (mi.z()<=a.mi.z())&& … … 444 446 DBVT_INLINE int btDbvtAabbMm::Classify(const btVector3& n,btScalar o,int s) const 445 447 { 446 447 448 btVector3 pi,px; 449 switch(s) 448 450 { 449 451 case (0+0+0): px=btVector3(mi.x(),mi.y(),mi.z()); 450 pi=btVector3(mx.x(),mx.y(),mx.z());break;452 pi=btVector3(mx.x(),mx.y(),mx.z());break; 451 453 case (1+0+0): px=btVector3(mx.x(),mi.y(),mi.z()); 452 pi=btVector3(mi.x(),mx.y(),mx.z());break;454 pi=btVector3(mi.x(),mx.y(),mx.z());break; 453 455 case (0+2+0): px=btVector3(mi.x(),mx.y(),mi.z()); 454 pi=btVector3(mx.x(),mi.y(),mx.z());break;456 pi=btVector3(mx.x(),mi.y(),mx.z());break; 455 457 case (1+2+0): px=btVector3(mx.x(),mx.y(),mi.z()); 456 pi=btVector3(mi.x(),mi.y(),mx.z());break;458 pi=btVector3(mi.x(),mi.y(),mx.z());break; 457 459 case (0+0+4): px=btVector3(mi.x(),mi.y(),mx.z()); 458 pi=btVector3(mx.x(),mx.y(),mi.z());break;460 pi=btVector3(mx.x(),mx.y(),mi.z());break; 459 461 case (1+0+4): px=btVector3(mx.x(),mi.y(),mx.z()); 460 pi=btVector3(mi.x(),mx.y(),mi.z());break;462 pi=btVector3(mi.x(),mx.y(),mi.z());break; 461 463 case (0+2+4): px=btVector3(mi.x(),mx.y(),mx.z()); 462 pi=btVector3(mx.x(),mi.y(),mi.z());break;464 pi=btVector3(mx.x(),mi.y(),mi.z());break; 463 465 case (1+2+4): px=btVector3(mx.x(),mx.y(),mx.z()); 464 pi=btVector3(mi.x(),mi.y(),mi.z());break;465 } 466 467 468 466 pi=btVector3(mi.x(),mi.y(),mi.z());break; 467 } 468 if((dot(n,px)+o)<0) return(-1); 469 if((dot(n,pi)+o)>=0) return(+1); 470 return(0); 469 471 } 470 472 … … 472 474 DBVT_INLINE btScalar btDbvtAabbMm::ProjectMinimum(const btVector3& v,unsigned signs) const 473 475 { 474 475 476 b[(signs>>1)&1]->y(),477 b[(signs>>2)&1]->z());478 476 const btVector3* b[]={&mx,&mi}; 477 const btVector3 p( b[(signs>>0)&1]->x(), 478 b[(signs>>1)&1]->y(), 479 b[(signs>>2)&1]->z()); 480 return(dot(p,v)); 479 481 } 480 482 … … 482 484 DBVT_INLINE void btDbvtAabbMm::AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const 483 485 { 484 485 { 486 486 for(int i=0;i<3;++i) 487 { 488 if(d[i]<0) 487 489 { smi+=mx[i]*d[i];smx+=mi[i]*d[i]; } 488 490 else … … 490 492 } 491 493 } 492 494 493 495 // 494 496 DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, 495 497 const btDbvtAabbMm& b) 496 498 { 497 499 #if DBVT_INT0_IMPL == DBVT_IMPL_SSE 498 499 _mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi))));500 501 500 const __m128 rt(_mm_or_ps( _mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)), 501 _mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi)))); 502 const __int32* pu((const __int32*)&rt); 503 return((pu[0]|pu[1]|pu[2])==0); 502 504 #else 503 505 return( (a.mi.x()<=b.mx.x())&& 504 506 (a.mx.x()>=b.mi.x())&& 505 507 (a.mi.y()<=b.mx.y())&& … … 512 514 // 513 515 DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, 514 515 516 { 517 518 519 520 521 522 523 524 525 516 const btDbvtAabbMm& b, 517 const btTransform& xform) 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); 526 528 } 527 529 528 530 // 529 531 DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, 530 531 { 532 532 const btVector3& b) 533 { 534 return( (b.x()>=a.mi.x())&& 533 535 (b.y()>=a.mi.y())&& 534 536 (b.z()>=a.mi.z())&& … … 538 540 } 539 541 540 541 542 543 544 ////////////////////////////////////// 545 546 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 547 574 // 548 575 DBVT_INLINE btScalar Proximity( const btDbvtAabbMm& a, 549 550 { 551 552 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())); 553 580 } 554 581 555 582 // 556 583 DBVT_INLINE int Select( const btDbvtAabbMm& o, 557 558 584 const btDbvtAabbMm& a, 585 const btDbvtAabbMm& b) 559 586 { 560 587 #if DBVT_SELECT_IMPL == DBVT_IMPL_SSE 561 588 static DBVT_ALIGN const unsigned __int32 mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff}; 562 589 // TODO: the intrinsic version is 11% slower 563 #if DBVT_USE_INTRINSIC_SSE590 #if DBVT_USE_INTRINSIC_SSE 564 591 __m128 omi(_mm_load_ps(o.mi)); 565 592 omi=_mm_add_ps(omi,_mm_load_ps(o.mx)); … … 579 606 bmi=_mm_add_ss(bmi,_mm_shuffle_ps(bmi,bmi,1)); 580 607 return(_mm_cmple_ss(bmi,ami).m128_u32[0]&1); 581 #else608 #else 582 609 DBVT_ALIGN __int32 r[1]; 583 610 __asm 584 {611 { 585 612 mov eax,o 586 587 588 613 mov ecx,a 614 mov edx,b 615 movaps xmm0,[eax] 589 616 movaps xmm5,mask 590 617 addps xmm0,[eax+16] 591 618 movaps xmm1,[ecx] 592 619 movaps xmm2,[edx] … … 594 621 addps xmm2,[edx+16] 595 622 subps xmm1,xmm0 596 597 598 599 600 601 602 603 604 605 606 607 608 609 }623 subps xmm2,xmm0 624 andps xmm1,xmm5 625 andps xmm2,xmm5 626 movhlps xmm3,xmm1 627 movhlps xmm4,xmm2 628 addps xmm1,xmm3 629 addps xmm2,xmm4 630 pshufd xmm3,xmm1,1 631 pshufd xmm4,xmm2,1 632 addss xmm1,xmm3 633 addss xmm2,xmm4 634 cmpless xmm2,xmm1 635 movss r,xmm2 636 } 610 637 return(r[0]&1); 611 #endif638 #endif 612 639 #else 613 640 return(Proximity(o,a)<Proximity(o,b)?0:1); 614 641 #endif 615 642 } … … 617 644 // 618 645 DBVT_INLINE void Merge( const btDbvtAabbMm& a, 619 620 646 const btDbvtAabbMm& b, 647 btDbvtAabbMm& r) 621 648 { 622 649 #if DBVT_MERGE_IMPL==DBVT_IMPL_SSE 623 624 625 626 627 628 629 630 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); 631 658 #else 632 633 { 634 635 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]; 636 663 } 637 664 #endif … … 640 667 // 641 668 DBVT_INLINE bool NotEqual( const btDbvtAabbMm& a, 642 643 { 644 669 const btDbvtAabbMm& b) 670 { 671 return( (a.mi.x()!=b.mi.x())|| 645 672 (a.mi.y()!=b.mi.y())|| 646 673 (a.mi.z()!=b.mi.z())|| … … 657 684 DBVT_PREFIX 658 685 inline void btDbvt::enumNodes( const btDbvtNode* root, 659 660 { 661 662 663 664 { 665 666 686 DBVT_IPOLICY) 687 { 688 DBVT_CHECKTYPE 689 policy.Process(root); 690 if(root->isinternal()) 691 { 692 enumNodes(root->childs[0],policy); 693 enumNodes(root->childs[1],policy); 667 694 } 668 695 } … … 671 698 DBVT_PREFIX 672 699 inline void btDbvt::enumLeaves( const btDbvtNode* root, 673 674 { 675 676 677 678 679 680 681 682 683 684 700 DBVT_IPOLICY) 701 { 702 DBVT_CHECKTYPE 703 if(root->isinternal()) 704 { 705 enumLeaves(root->childs[0],policy); 706 enumLeaves(root->childs[1],policy); 707 } 708 else 709 { 710 policy.Process(root); 711 } 685 712 } 686 713 … … 688 715 DBVT_PREFIX 689 716 inline void btDbvt::collideTT( const btDbvtNode* root0, 690 const btDbvtNode* root1, 691 DBVT_IPOLICY) 692 { 693 DBVT_CHECKTYPE 694 if(root0&&root1) 695 { 696 btAlignedObjectArray<sStkNN> stack; 697 int depth=1; 698 int treshold=DOUBLE_STACKSIZE-4; 699 stack.resize(DOUBLE_STACKSIZE); 700 stack[0]=sStkNN(root0,root1); 701 do { 702 sStkNN p=stack[--depth]; 703 if(depth>treshold) 704 { 705 stack.resize(stack.size()*2); 706 treshold=stack.size()-4; 707 } 708 if(p.a==p.b) 709 { 710 if(p.a->isinternal()) 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) 731 { 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()) 711 749 { 712 stack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]); 713 stack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]); 714 stack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]); 715 } 716 } 717 else if(Intersect(p.a->volume,p.b->volume)) 718 { 719 if(p.a->isinternal()) 720 { 721 if(p.b->isinternal()) 722 { 723 stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); 724 stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); 725 stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); 726 stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); 727 } 728 else 729 { 730 stack[depth++]=sStkNN(p.a->childs[0],p.b); 731 stack[depth++]=sStkNN(p.a->childs[1],p.b); 732 } 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]); 733 754 } 734 755 else 735 756 { 736 if(p.b->isinternal()) 737 { 738 stack[depth++]=sStkNN(p.a,p.b->childs[0]); 739 stack[depth++]=sStkNN(p.a,p.b->childs[1]); 740 } 741 else 742 { 743 policy.Process(p.a,p.b); 744 } 757 stack[depth++]=sStkNN(p.a->childs[0],p.b); 758 stack[depth++]=sStkNN(p.a->childs[1],p.b); 745 759 } 746 760 } 747 } while(depth); 748 } 749 } 750 751 // 752 DBVT_PREFIX 753 inline void btDbvt::collideTT( const btDbvtNode* root0, 754 const btDbvtNode* root1, 755 const btTransform& xform, 756 DBVT_IPOLICY) 757 { 758 DBVT_CHECKTYPE 759 if(root0&&root1) 760 { 761 btAlignedObjectArray<sStkNN> stack; 762 int depth=1; 763 int treshold=DOUBLE_STACKSIZE-4; 764 stack.resize(DOUBLE_STACKSIZE); 765 stack[0]=sStkNN(root0,root1); 766 do { 767 sStkNN p=stack[--depth]; 768 if(Intersect(p.a->volume,p.b->volume,xform)) 769 { 770 if(depth>treshold) 761 else 762 { 763 if(p.b->isinternal()) 771 764 { 772 stack.resize(stack.size()*2); 773 treshold=stack.size()-4; 774 } 775 if(p.a->isinternal()) 776 { 777 if(p.b->isinternal()) 778 { 779 stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); 780 stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); 781 stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); 782 stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); 783 } 784 else 785 { 786 stack[depth++]=sStkNN(p.a->childs[0],p.b); 787 stack[depth++]=sStkNN(p.a->childs[1],p.b); 788 } 765 stack[depth++]=sStkNN(p.a,p.b->childs[0]); 766 stack[depth++]=sStkNN(p.a,p.b->childs[1]); 789 767 } 790 768 else 791 769 { 792 if(p.b->isinternal()) 793 { 794 stack[depth++]=sStkNN(p.a,p.b->childs[0]); 795 stack[depth++]=sStkNN(p.a,p.b->childs[1]); 796 } 797 else 798 { 799 policy.Process(p.a,p.b); 800 } 770 policy.Process(p.a,p.b); 801 771 } 802 772 } 803 } while(depth); 804 } 773 } 774 } while(depth); 775 } 805 776 } 806 777 … … 808 779 DBVT_PREFIX 809 780 inline void btDbvt::collideTT( const btDbvtNode* root0, 810 const btTransform& xform0, 811 const btDbvtNode* root1, 812 const btTransform& xform1, 813 DBVT_IPOLICY) 814 { 815 const btTransform xform=xform0.inverse()*xform1; 816 collideTT(root0,root1,xform,policy); 817 } 818 819 // 820 DBVT_PREFIX 821 inline void btDbvt::collideTV( const btDbvtNode* root, 822 const btDbvtVolume& vol, 823 DBVT_IPOLICY) 824 { 825 DBVT_CHECKTYPE 826 if(root) 827 { 828 ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol); 829 btAlignedObjectArray<const btDbvtNode*> stack; 830 stack.reserve(SIMPLE_STACKSIZE); 831 stack.push_back(root); 832 do { 833 const btDbvtNode* n=stack[stack.size()-1]; 834 stack.pop_back(); 835 if(Intersect(n->volume,volume)) 836 { 837 if(n->isinternal()) 838 { 839 stack.push_back(n->childs[0]); 840 stack.push_back(n->childs[1]); 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]); 841 810 } 842 811 else 843 812 { 844 policy.Process(n); 813 stack[depth++]=sStkNN(p.a->childs[0],p.b); 814 stack[depth++]=sStkNN(p.a->childs[1],p.b); 845 815 } 846 816 } 847 } while(stack.size()>0); 848 } 849 } 850 851 852 // 853 DBVT_PREFIX 854 inline void btDbvt::rayTest( const btDbvtNode* root, 855 const btVector3& rayFrom, 856 const btVector3& rayTo, 857 DBVT_IPOLICY) 858 { 859 DBVT_CHECKTYPE 860 if(root) 861 { 862 btVector3 rayDir = (rayTo-rayFrom); 863 rayDir.normalize (); 864 865 ///what about division by zero? --> just set rayDirection[i] to INF/1e30 866 btVector3 rayDirectionInverse; 867 rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; 868 rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; 869 rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; 870 unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0}; 871 872 873 btVector3 resultNormal; 874 875 876 btAlignedObjectArray<const btDbvtNode*> stack; 877 stack.reserve(SIMPLE_STACKSIZE); 878 stack.push_back(root); 879 do { 880 const btDbvtNode* node=stack[stack.size()-1]; 881 stack.pop_back(); 882 883 btVector3 bounds[2] = {node->volume.Mins(),node->volume.Maxs()}; 884 btScalar lambda_max = rayDir.dot(rayTo-rayFrom); 885 btScalar tmin=1.f,lambda_min=0.f; 886 bool result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max); 887 888 #ifdef COMPARE_BTRAY_AABB2 889 btScalar param=1.f; 890 bool result2 = btRayAabb(rayFrom,rayTo,node->volume.Mins(),node->volume.Maxs(),param,resultNormal); 891 btAssert(result1 == result2); 892 #endif //TEST_BTRAY_AABB2 893 894 if(result1) 895 { 896 if(node->isinternal()) 817 else 818 { 819 if(p.b->isinternal()) 897 820 { 898 stack.push_back(node->childs[0]);899 stack.push_back(node->childs[1]);821 stack[depth++]=sStkNN(p.a,p.b->childs[0]); 822 stack[depth++]=sStkNN(p.a,p.b->childs[1]); 900 823 } 901 824 else 902 825 { 903 policy.Process(node);826 policy.Process(p.a,p.b); 904 827 } 905 828 } 906 } while(stack.size()); 907 } 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 } 908 915 } 909 916 … … 916 923 DBVT_IPOLICY) 917 924 { 918 DBVT_CHECKTYPE 919 if(root) 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) 920 933 { 921 const int inside=(1<<count)-1; 922 btAlignedObjectArray<sStkNP> stack; 923 int signs[sizeof(unsigned)*8]; 924 btAssert(count<int (sizeof(signs)/sizeof(signs[0]))); 925 for(int i=0;i<count;++i) 926 { 927 signs[i]= ((normals[i].x()>=0)?1:0)+ 934 signs[i]= ((normals[i].x()>=0)?1:0)+ 928 935 ((normals[i].y()>=0)?2:0)+ 929 936 ((normals[i].z()>=0)?4:0); 937 } 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 } 930 955 } 931 stack.reserve(SIMPLE_STACKSIZE); 932 stack.push_back(sStkNP(root,0)); 933 do { 934 sStkNP se=stack[stack.size()-1]; 935 bool out=false; 936 stack.pop_back(); 937 for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1) 938 { 939 if(0==(se.mask&j)) 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 else 964 { 965 if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy); 966 } 967 } 968 } while(stack.size()); 969 } 970 } 971 972 // 973 DBVT_PREFIX 974 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) 995 { 996 signs[i]= ((normals[i].x()>=0)?1:0)+ 997 ((normals[i].y()>=0)?2:0)+ 998 ((normals[i].z()>=0)?4:0); 999 } 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)) 940 1014 { 941 942 1015 const int side=se.node->volume.Classify(normals[i],offsets[i],signs[i]); 1016 switch(side) 943 1017 { 944 1018 case -1: out=true;break; … … 947 1021 } 948 1022 } 949 if(!out) 950 { 951 if((se.mask!=inside)&&(se.node->isinternal())) 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)) 952 1035 { 953 stack.push_back(sStkNP(se.node->childs[0],se.mask)); 954 stack.push_back(sStkNP(se.node->childs[1],se.mask)); 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_MEMMOVE 1040 memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1)); 1041 #else 1042 for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1]; 1043 #endif 1044 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_MEMMOVE 1049 memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1)); 1050 #else 1051 for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1]; 1052 #endif 1053 stack[j]=allocate(ifree,stock,nes[1-q]); 955 1054 } 956 1055 else 957 1056 { 958 if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy); 1057 stack.push_back(allocate(ifree,stock,nes[q])); 1058 stack.push_back(allocate(ifree,stock,nes[1-q])); 959 1059 } 960 1060 } 961 } while(stack.size()); 962 } 963 } 964 965 // 966 DBVT_PREFIX 967 inline void btDbvt::collideOCL( const btDbvtNode* root, 968 const btVector3* normals, 969 const btScalar* offsets, 970 const btVector3& sortaxis, 971 int count, 972 DBVT_IPOLICY, 973 bool fsort) 974 { 975 DBVT_CHECKTYPE 976 if(root) 977 { 978 const unsigned srtsgns=(sortaxis[0]>=0?1:0)+ 979 (sortaxis[1]>=0?2:0)+ 980 (sortaxis[2]>=0?4:0); 981 const int inside=(1<<count)-1; 982 btAlignedObjectArray<sStkNPS> stock; 983 btAlignedObjectArray<int> ifree; 984 btAlignedObjectArray<int> stack; 985 int signs[sizeof(unsigned)*8]; 986 btAssert(count<int (sizeof(signs)/sizeof(signs[0]))); 987 for(int i=0;i<count;++i) 988 { 989 signs[i]= ((normals[i].x()>=0)?1:0)+ 990 ((normals[i].y()>=0)?2:0)+ 991 ((normals[i].z()>=0)?4:0); 1061 else 1062 { 1063 policy.Process(se.node,se.value); 1064 } 992 1065 } 993 stock.reserve(SIMPLE_STACKSIZE); 994 stack.reserve(SIMPLE_STACKSIZE); 995 ifree.reserve(SIMPLE_STACKSIZE); 996 stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns)))); 997 do { 998 const int id=stack[stack.size()-1]; 999 sStkNPS se=stock[id]; 1000 stack.pop_back();ifree.push_back(id); 1001 if(se.mask!=inside) 1002 { 1003 bool out=false; 1004 for(int i=0,j=1;(!out)&&(i<count);++i,j<<=1) 1005 { 1006 if(0==(se.mask&j)) 1007 { 1008 const int side=se.node->volume.Classify(normals[i],offsets[i],signs[i]); 1009 switch(side) 1010 { 1011 case -1: out=true;break; 1012 case +1: se.mask|=j;break; 1013 } 1014 } 1015 } 1016 if(out) continue; 1017 } 1018 if(policy.Descent(se.node)) 1019 { 1020 if(se.node->isinternal()) 1021 { 1022 const btDbvtNode* pns[]={ se.node->childs[0],se.node->childs[1]}; 1023 sStkNPS nes[]={ sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)), 1024 sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))}; 1025 const int q=nes[0].value<nes[1].value?1:0; 1026 int j=stack.size(); 1027 if(fsort&&(j>0)) 1028 { 1029 /* Insert 0 */ 1030 j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size()); 1031 stack.push_back(0); 1032 #if DBVT_USE_MEMMOVE 1033 memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1)); 1034 #else 1035 for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1]; 1036 #endif 1037 stack[j]=allocate(ifree,stock,nes[q]); 1038 /* Insert 1 */ 1039 j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size()); 1040 stack.push_back(0); 1041 #if DBVT_USE_MEMMOVE 1042 memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1)); 1043 #else 1044 for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1]; 1045 #endif 1046 stack[j]=allocate(ifree,stock,nes[1-q]); 1047 } 1048 else 1049 { 1050 stack.push_back(allocate(ifree,stock,nes[q])); 1051 stack.push_back(allocate(ifree,stock,nes[1-q])); 1052 } 1053 } 1054 else 1055 { 1056 policy.Process(se.node,se.value); 1057 } 1058 } 1059 } while(stack.size()); 1060 } 1066 } while(stack.size()); 1067 } 1061 1068 } 1062 1069 … … 1064 1071 DBVT_PREFIX 1065 1072 inline void btDbvt::collideTU( const btDbvtNode* root, 1066 1067 { 1068 1069 1070 1071 1072 1073 1074 1075 1076 1077 1078 1079 1080 1081 1082 1083 1084 1085 1073 DBVT_IPOLICY) 1074 { 1075 DBVT_CHECKTYPE 1076 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 else 1089 { policy.Process(n); } 1090 } 1091 } while(stack.size()>0); 1092 } 1086 1093 } 1087 1094
Note: See TracChangeset
for help on using the changeset viewer.
