1 //Bullet Continuous Collision Detection and Physics Library 2 //Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ 3 4 // 5 // btAxisSweep3.h 6 // 7 // Copyright (c) 2006 Simon Hobbs 8 // 9 // This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software. 10 // 11 // Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions: 12 // 13 // 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. 14 // 15 // 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 16 // 17 // 3. This notice may not be removed or altered from any source distribution. 18 19 #ifndef BT_AXIS_SWEEP_3_H 20 #define BT_AXIS_SWEEP_3_H 21 22 #include "LinearMath/btVector3.h" 23 #include "btOverlappingPairCache.h" 24 #include "btBroadphaseInterface.h" 25 #include "btBroadphaseProxy.h" 26 #include "btOverlappingPairCallback.h" 27 #include "btDbvtBroadphase.h" 28 29 //#define DEBUG_BROADPHASE 1 30 #define USE_OVERLAP_TEST_ON_REMOVES 1 31 32 /// The internal templace class btAxisSweep3Internal implements the sweep and prune broadphase. 33 /// It uses quantized integers to represent the begin and end points for each of the 3 axis. 34 /// Dont use this class directly, use btAxisSweep3 or bt32BitAxisSweep3 instead. 35 template <typename BP_FP_INT_TYPE> 36 class btAxisSweep3Internal : public btBroadphaseInterface 37 { 38 protected: 39 40 BP_FP_INT_TYPE m_bpHandleMask; 41 BP_FP_INT_TYPE m_handleSentinel; 42 43 public: 44 45 BT_DECLARE_ALIGNED_ALLOCATOR(); 46 47 class Edge 48 { 49 public: 50 BP_FP_INT_TYPE m_pos; // low bit is min/max 51 BP_FP_INT_TYPE m_handle; 52 53 BP_FP_INT_TYPE IsMax() const {return static_cast<BP_FP_INT_TYPE>(m_pos & 1);} 54 }; 55 56 public: 57 class Handle : public btBroadphaseProxy 58 { 59 public: 60 BT_DECLARE_ALIGNED_ALLOCATOR(); 61 62 // indexes into the edge arrays 63 BP_FP_INT_TYPE m_minEdges[3], m_maxEdges[3]; // 6 * 2 = 12 64 // BP_FP_INT_TYPE m_uniqueId; 65 btBroadphaseProxy* m_dbvtProxy;//for faster raycast 66 //void* m_pOwner; this is now in btBroadphaseProxy.m_clientObject 67 68 SIMD_FORCE_INLINE void SetNextFree(BP_FP_INT_TYPE next) {m_minEdges[0] = next;} 69 SIMD_FORCE_INLINE BP_FP_INT_TYPE GetNextFree() const {return m_minEdges[0];} 70 }; // 24 bytes + 24 for Edge structures = 44 bytes total per entry 71 72 73 protected: 74 btVector3 m_worldAabbMin; // overall system bounds 75 btVector3 m_worldAabbMax; // overall system bounds 76 77 btVector3 m_quantize; // scaling factor for quantization 78 79 BP_FP_INT_TYPE m_numHandles; // number of active handles 80 BP_FP_INT_TYPE m_maxHandles; // max number of handles 81 Handle* m_pHandles; // handles pool 82 83 BP_FP_INT_TYPE m_firstFreeHandle; // free handles list 84 85 Edge* m_pEdges[3]; // edge arrays for the 3 axes (each array has m_maxHandles * 2 + 2 sentinel entries) 86 void* m_pEdgesRawPtr[3]; 87 88 btOverlappingPairCache* m_pairCache; 89 90 ///btOverlappingPairCallback is an additional optional user callback for adding/removing overlapping pairs, similar interface to btOverlappingPairCache. 91 btOverlappingPairCallback* m_userPairCallback; 92 93 bool m_ownsPairCache; 94 95 int m_invalidPair; 96 97 ///additional dynamic aabb structure, used to accelerate ray cast queries. 98 ///can be disabled using a optional argument in the constructor 99 btDbvtBroadphase* m_raycastAccelerator; 100 btOverlappingPairCache* m_nullPairCache; 101 102 103 // allocation/deallocation 104 BP_FP_INT_TYPE allocHandle(); 105 void freeHandle(BP_FP_INT_TYPE handle); 106 107 108 bool testOverlap2D(const Handle* pHandleA, const Handle* pHandleB,int axis0,int axis1); 109 110 #ifdef DEBUG_BROADPHASE 111 void debugPrintAxis(int axis,bool checkCardinality=true); 112 #endif //DEBUG_BROADPHASE 113 114 //Overlap* AddOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB); 115 //void RemoveOverlap(BP_FP_INT_TYPE handleA, BP_FP_INT_TYPE handleB); 116 117 118 119 void sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps ); 120 void sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps ); 121 void sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps ); 122 void sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps ); 123 124 public: 125 126 btAxisSweep3Internal(const btVector3& worldAabbMin,const btVector3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel, BP_FP_INT_TYPE maxHandles = 16384, btOverlappingPairCache* pairCache=0,bool disableRaycastAccelerator = false); 127 128 virtual ~btAxisSweep3Internal(); 129 130 BP_FP_INT_TYPE getNumHandles() const 131 { 132 return m_numHandles; 133 } 134 135 virtual void calculateOverlappingPairs(btDispatcher* dispatcher); 136 137 BP_FP_INT_TYPE addHandle(const btVector3& aabbMin,const btVector3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy); 138 void removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher); 139 void updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher); 140 SIMD_FORCE_INLINE Handle* getHandle(BP_FP_INT_TYPE index) const {return m_pHandles + index;} 141 142 virtual void resetPool(btDispatcher* dispatcher); 143 144 void processAllOverlappingPairs(btOverlapCallback* callback); 145 146 //Broadphase Interface 147 virtual btBroadphaseProxy* createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr ,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy); 148 virtual void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); 149 virtual void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher); 150 virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; 151 152 virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin=btVector3(0,0,0), const btVector3& aabbMax = btVector3(0,0,0)); 153 virtual void aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback); 154 155 156 void quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const; 157 ///unQuantize should be conservative: aabbMin/aabbMax should be larger then 'getAabb' result 158 void unQuantize(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; 159 160 bool testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1); 161 162 btOverlappingPairCache* getOverlappingPairCache() 163 { 164 return m_pairCache; 165 } 166 const btOverlappingPairCache* getOverlappingPairCache() const 167 { 168 return m_pairCache; 169 } 170 171 void setOverlappingPairUserCallback(btOverlappingPairCallback* pairCallback) 172 { 173 m_userPairCallback = pairCallback; 174 } 175 const btOverlappingPairCallback* getOverlappingPairUserCallback() const 176 { 177 return m_userPairCallback; 178 } 179 180 ///getAabb returns the axis aligned bounding box in the 'global' coordinate frame 181 ///will add some transform later 182 virtual void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const 183 { 184 aabbMin = m_worldAabbMin; 185 aabbMax = m_worldAabbMax; 186 } 187 188 virtual void printStats() 189 { 190 /* printf("btAxisSweep3.h\n"); 191 printf("numHandles = %d, maxHandles = %d\n",m_numHandles,m_maxHandles); 192 printf("aabbMin=%f,%f,%f,aabbMax=%f,%f,%f\n",m_worldAabbMin.getX(),m_worldAabbMin.getY(),m_worldAabbMin.getZ(), 193 m_worldAabbMax.getX(),m_worldAabbMax.getY(),m_worldAabbMax.getZ()); 194 */ 195 196 } 197 198 }; 199 200 //////////////////////////////////////////////////////////////////// 201 202 203 204 205 #ifdef DEBUG_BROADPHASE 206 #include <stdio.h> 207 208 template <typename BP_FP_INT_TYPE> 209 void btAxisSweep3<BP_FP_INT_TYPE>::debugPrintAxis(int axis, bool checkCardinality) 210 { 211 int numEdges = m_pHandles[0].m_maxEdges[axis]; 212 printf("SAP Axis %d, numEdges=%d\n",axis,numEdges); 213 214 int i; 215 for (i=0;i<numEdges+1;i++) 216 { 217 Edge* pEdge = m_pEdges[axis] + i; 218 Handle* pHandlePrev = getHandle(pEdge->m_handle); 219 int handleIndex = pEdge->IsMax()? pHandlePrev->m_maxEdges[axis] : pHandlePrev->m_minEdges[axis]; 220 char beginOrEnd; 221 beginOrEnd=pEdge->IsMax()?'E':'B'; 222 printf(" [%c,h=%d,p=%x,i=%d]\n",beginOrEnd,pEdge->m_handle,pEdge->m_pos,handleIndex); 223 } 224 225 if (checkCardinality) 226 btAssert(numEdges == m_numHandles*2+1); 227 } 228 #endif //DEBUG_BROADPHASE 229 230 template <typename BP_FP_INT_TYPE> 231 btBroadphaseProxy* btAxisSweep3Internal<BP_FP_INT_TYPE>::createProxy( const btVector3& aabbMin, const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy) 232 { 233 (void)shapeType; 234 BP_FP_INT_TYPE handleId = addHandle(aabbMin,aabbMax, userPtr,collisionFilterGroup,collisionFilterMask,dispatcher,multiSapProxy); 235 236 Handle* handle = getHandle(handleId); 237 238 if (m_raycastAccelerator) 239 { 240 btBroadphaseProxy* rayProxy = m_raycastAccelerator->createProxy(aabbMin,aabbMax,shapeType,userPtr,collisionFilterGroup,collisionFilterMask,dispatcher,0); 241 handle->m_dbvtProxy = rayProxy; 242 } 243 return handle; 244 } 245 246 247 248 template <typename BP_FP_INT_TYPE> 249 void btAxisSweep3Internal<BP_FP_INT_TYPE>::destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher) 250 { 251 Handle* handle = static_cast<Handle*>(proxy); 252 if (m_raycastAccelerator) 253 m_raycastAccelerator->destroyProxy(handle->m_dbvtProxy,dispatcher); 254 removeHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), dispatcher); 255 } 256 257 template <typename BP_FP_INT_TYPE> 258 void btAxisSweep3Internal<BP_FP_INT_TYPE>::setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher) 259 { 260 Handle* handle = static_cast<Handle*>(proxy); 261 handle->m_aabbMin = aabbMin; 262 handle->m_aabbMax = aabbMax; 263 updateHandle(static_cast<BP_FP_INT_TYPE>(handle->m_uniqueId), aabbMin, aabbMax,dispatcher); 264 if (m_raycastAccelerator) 265 m_raycastAccelerator->setAabb(handle->m_dbvtProxy,aabbMin,aabbMax,dispatcher); 266 267 } 268 269 template <typename BP_FP_INT_TYPE> 270 void btAxisSweep3Internal<BP_FP_INT_TYPE>::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback,const btVector3& aabbMin,const btVector3& aabbMax) 271 { 272 if (m_raycastAccelerator) 273 { 274 m_raycastAccelerator->rayTest(rayFrom,rayTo,rayCallback,aabbMin,aabbMax); 275 } else 276 { 277 //choose axis? 278 BP_FP_INT_TYPE axis = 0; 279 //for each proxy 280 for (BP_FP_INT_TYPE i=1;i<m_numHandles*2+1;i++) 281 { 282 if (m_pEdges[axis][i].IsMax()) 283 { 284 rayCallback.process(getHandle(m_pEdges[axis][i].m_handle)); 285 } 286 } 287 } 288 } 289 290 template <typename BP_FP_INT_TYPE> 291 void btAxisSweep3Internal<BP_FP_INT_TYPE>::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback) 292 { 293 if (m_raycastAccelerator) 294 { 295 m_raycastAccelerator->aabbTest(aabbMin,aabbMax,callback); 296 } else 297 { 298 //choose axis? 299 BP_FP_INT_TYPE axis = 0; 300 //for each proxy 301 for (BP_FP_INT_TYPE i=1;i<m_numHandles*2+1;i++) 302 { 303 if (m_pEdges[axis][i].IsMax()) 304 { 305 Handle* handle = getHandle(m_pEdges[axis][i].m_handle); 306 if (TestAabbAgainstAabb2(aabbMin,aabbMax,handle->m_aabbMin,handle->m_aabbMax)) 307 { 308 callback.process(handle); 309 } 310 } 311 } 312 } 313 } 314 315 316 317 template <typename BP_FP_INT_TYPE> 318 void btAxisSweep3Internal<BP_FP_INT_TYPE>::getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const 319 { 320 Handle* pHandle = static_cast<Handle*>(proxy); 321 aabbMin = pHandle->m_aabbMin; 322 aabbMax = pHandle->m_aabbMax; 323 } 324 325 326 template <typename BP_FP_INT_TYPE> 327 void btAxisSweep3Internal<BP_FP_INT_TYPE>::unQuantize(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const 328 { 329 Handle* pHandle = static_cast<Handle*>(proxy); 330 331 unsigned short vecInMin[3]; 332 unsigned short vecInMax[3]; 333 334 vecInMin[0] = m_pEdges[0][pHandle->m_minEdges[0]].m_pos ; 335 vecInMax[0] = m_pEdges[0][pHandle->m_maxEdges[0]].m_pos +1 ; 336 vecInMin[1] = m_pEdges[1][pHandle->m_minEdges[1]].m_pos ; 337 vecInMax[1] = m_pEdges[1][pHandle->m_maxEdges[1]].m_pos +1 ; 338 vecInMin[2] = m_pEdges[2][pHandle->m_minEdges[2]].m_pos ; 339 vecInMax[2] = m_pEdges[2][pHandle->m_maxEdges[2]].m_pos +1 ; 340 341 aabbMin.setValue((btScalar)(vecInMin[0]) / (m_quantize.getX()),(btScalar)(vecInMin[1]) / (m_quantize.getY()),(btScalar)(vecInMin[2]) / (m_quantize.getZ())); 342 aabbMin += m_worldAabbMin; 343 344 aabbMax.setValue((btScalar)(vecInMax[0]) / (m_quantize.getX()),(btScalar)(vecInMax[1]) / (m_quantize.getY()),(btScalar)(vecInMax[2]) / (m_quantize.getZ())); 345 aabbMax += m_worldAabbMin; 346 } 347 348 349 350 351 template <typename BP_FP_INT_TYPE> 352 btAxisSweep3Internal<BP_FP_INT_TYPE>::btAxisSweep3Internal(const btVector3& worldAabbMin,const btVector3& worldAabbMax, BP_FP_INT_TYPE handleMask, BP_FP_INT_TYPE handleSentinel,BP_FP_INT_TYPE userMaxHandles, btOverlappingPairCache* pairCache , bool disableRaycastAccelerator) 353 :m_bpHandleMask(handleMask), 354 m_handleSentinel(handleSentinel), 355 m_pairCache(pairCache), 356 m_userPairCallback(0), 357 m_ownsPairCache(false), 358 m_invalidPair(0), 359 m_raycastAccelerator(0) 360 { 361 BP_FP_INT_TYPE maxHandles = static_cast<BP_FP_INT_TYPE>(userMaxHandles+1);//need to add one sentinel handle 362 363 if (!m_pairCache) 364 { 365 void* ptr = btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16); 366 m_pairCache = new(ptr) btHashedOverlappingPairCache(); 367 m_ownsPairCache = true; 368 } 369 370 if (!disableRaycastAccelerator) 371 { 372 m_nullPairCache = new (btAlignedAlloc(sizeof(btNullPairCache),16)) btNullPairCache(); 373 m_raycastAccelerator = new (btAlignedAlloc(sizeof(btDbvtBroadphase),16)) btDbvtBroadphase(m_nullPairCache);//m_pairCache); 374 m_raycastAccelerator->m_deferedcollide = true;//don't add/remove pairs 375 } 376 377 //btAssert(bounds.HasVolume()); 378 379 // init bounds 380 m_worldAabbMin = worldAabbMin; 381 m_worldAabbMax = worldAabbMax; 382 383 btVector3 aabbSize = m_worldAabbMax - m_worldAabbMin; 384 385 BP_FP_INT_TYPE maxInt = m_handleSentinel; 386 387 m_quantize = btVector3(btScalar(maxInt),btScalar(maxInt),btScalar(maxInt)) / aabbSize; 388 389 // allocate handles buffer, using btAlignedAlloc, and put all handles on free list 390 m_pHandles = new Handle[maxHandles]; 391 392 m_maxHandles = maxHandles; 393 m_numHandles = 0; 394 395 // handle 0 is reserved as the null index, and is also used as the sentinel 396 m_firstFreeHandle = 1; 397 { 398 for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < maxHandles; i++) 399 m_pHandles[i].SetNextFree(static_cast<BP_FP_INT_TYPE>(i + 1)); 400 m_pHandles[maxHandles - 1].SetNextFree(0); 401 } 402 403 { 404 // allocate edge buffers 405 for (int i = 0; i < 3; i++) 406 { 407 m_pEdgesRawPtr[i] = btAlignedAlloc(sizeof(Edge)*maxHandles*2,16); 408 m_pEdges[i] = new(m_pEdgesRawPtr[i]) Edge[maxHandles * 2]; 409 } 410 } 411 //removed overlap management 412 413 // make boundary sentinels 414 415 m_pHandles[0].m_clientObject = 0; 416 417 for (int axis = 0; axis < 3; axis++) 418 { 419 m_pHandles[0].m_minEdges[axis] = 0; 420 m_pHandles[0].m_maxEdges[axis] = 1; 421 422 m_pEdges[axis][0].m_pos = 0; 423 m_pEdges[axis][0].m_handle = 0; 424 m_pEdges[axis][1].m_pos = m_handleSentinel; 425 m_pEdges[axis][1].m_handle = 0; 426 #ifdef DEBUG_BROADPHASE 427 debugPrintAxis(axis); 428 #endif //DEBUG_BROADPHASE 429 430 } 431 432 } 433 434 template <typename BP_FP_INT_TYPE> 435 btAxisSweep3Internal<BP_FP_INT_TYPE>::~btAxisSweep3Internal() 436 { 437 if (m_raycastAccelerator) 438 { 439 m_nullPairCache->~btOverlappingPairCache(); 440 btAlignedFree(m_nullPairCache); 441 m_raycastAccelerator->~btDbvtBroadphase(); 442 btAlignedFree (m_raycastAccelerator); 443 } 444 445 for (int i = 2; i >= 0; i--) 446 { 447 btAlignedFree(m_pEdgesRawPtr[i]); 448 } 449 delete [] m_pHandles; 450 451 if (m_ownsPairCache) 452 { 453 m_pairCache->~btOverlappingPairCache(); 454 btAlignedFree(m_pairCache); 455 } 456 } 457 458 template <typename BP_FP_INT_TYPE> 459 void btAxisSweep3Internal<BP_FP_INT_TYPE>::quantize(BP_FP_INT_TYPE* out, const btVector3& point, int isMax) const 460 { 461 #ifdef OLD_CLAMPING_METHOD 462 ///problem with this clamping method is that the floating point during quantization might still go outside the range [(0|isMax) .. (m_handleSentinel&m_bpHandleMask]|isMax] 463 ///see http://code.google.com/p/bullet/issues/detail?id=87 464 btVector3 clampedPoint(point); 465 clampedPoint.setMax(m_worldAabbMin); 466 clampedPoint.setMin(m_worldAabbMax); 467 btVector3 v = (clampedPoint - m_worldAabbMin) * m_quantize; 468 out[0] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getX() & m_bpHandleMask) | isMax); 469 out[1] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getY() & m_bpHandleMask) | isMax); 470 out[2] = (BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v.getZ() & m_bpHandleMask) | isMax); 471 #else 472 btVector3 v = (point - m_worldAabbMin) * m_quantize; 473 out[0]=(v[0]<=0)?(BP_FP_INT_TYPE)isMax:(v[0]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[0]&m_bpHandleMask)|isMax); 474 out[1]=(v[1]<=0)?(BP_FP_INT_TYPE)isMax:(v[1]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[1]&m_bpHandleMask)|isMax); 475 out[2]=(v[2]<=0)?(BP_FP_INT_TYPE)isMax:(v[2]>=m_handleSentinel)?(BP_FP_INT_TYPE)((m_handleSentinel&m_bpHandleMask)|isMax):(BP_FP_INT_TYPE)(((BP_FP_INT_TYPE)v[2]&m_bpHandleMask)|isMax); 476 #endif //OLD_CLAMPING_METHOD 477 } 478 479 480 template <typename BP_FP_INT_TYPE> 481 BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::allocHandle() 482 { 483 btAssert(m_firstFreeHandle); 484 485 BP_FP_INT_TYPE handle = m_firstFreeHandle; 486 m_firstFreeHandle = getHandle(handle)->GetNextFree(); 487 m_numHandles++; 488 489 return handle; 490 } 491 492 template <typename BP_FP_INT_TYPE> 493 void btAxisSweep3Internal<BP_FP_INT_TYPE>::freeHandle(BP_FP_INT_TYPE handle) 494 { 495 btAssert(handle > 0 && handle < m_maxHandles); 496 497 getHandle(handle)->SetNextFree(m_firstFreeHandle); 498 m_firstFreeHandle = handle; 499 500 m_numHandles--; 501 } 502 503 504 template <typename BP_FP_INT_TYPE> 505 BP_FP_INT_TYPE btAxisSweep3Internal<BP_FP_INT_TYPE>::addHandle(const btVector3& aabbMin,const btVector3& aabbMax, void* pOwner,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy) 506 { 507 // quantize the bounds 508 BP_FP_INT_TYPE min[3], max[3]; 509 quantize(min, aabbMin, 0); 510 quantize(max, aabbMax, 1); 511 512 // allocate a handle 513 BP_FP_INT_TYPE handle = allocHandle(); 514 515 516 Handle* pHandle = getHandle(handle); 517 518 pHandle->m_uniqueId = static_cast<int>(handle); 519 //pHandle->m_pOverlaps = 0; 520 pHandle->m_clientObject = pOwner; 521 pHandle->m_collisionFilterGroup = collisionFilterGroup; 522 pHandle->m_collisionFilterMask = collisionFilterMask; 523 pHandle->m_multiSapParentProxy = multiSapProxy; 524 525 // compute current limit of edge arrays 526 BP_FP_INT_TYPE limit = static_cast<BP_FP_INT_TYPE>(m_numHandles * 2); 527 528 529 // insert new edges just inside the max boundary edge 530 for (BP_FP_INT_TYPE axis = 0; axis < 3; axis++) 531 { 532 533 m_pHandles[0].m_maxEdges[axis] += 2; 534 535 m_pEdges[axis][limit + 1] = m_pEdges[axis][limit - 1]; 536 537 m_pEdges[axis][limit - 1].m_pos = min[axis]; 538 m_pEdges[axis][limit - 1].m_handle = handle; 539 540 m_pEdges[axis][limit].m_pos = max[axis]; 541 m_pEdges[axis][limit].m_handle = handle; 542 543 pHandle->m_minEdges[axis] = static_cast<BP_FP_INT_TYPE>(limit - 1); 544 pHandle->m_maxEdges[axis] = limit; 545 } 546 547 // now sort the new edges to their correct position 548 sortMinDown(0, pHandle->m_minEdges[0], dispatcher,false); 549 sortMaxDown(0, pHandle->m_maxEdges[0], dispatcher,false); 550 sortMinDown(1, pHandle->m_minEdges[1], dispatcher,false); 551 sortMaxDown(1, pHandle->m_maxEdges[1], dispatcher,false); 552 sortMinDown(2, pHandle->m_minEdges[2], dispatcher,true); 553 sortMaxDown(2, pHandle->m_maxEdges[2], dispatcher,true); 554 555 556 return handle; 557 } 558 559 560 template <typename BP_FP_INT_TYPE> 561 void btAxisSweep3Internal<BP_FP_INT_TYPE>::removeHandle(BP_FP_INT_TYPE handle,btDispatcher* dispatcher) 562 { 563 564 Handle* pHandle = getHandle(handle); 565 566 //explicitly remove the pairs containing the proxy 567 //we could do it also in the sortMinUp (passing true) 568 ///@todo: compare performance 569 if (!m_pairCache->hasDeferredRemoval()) 570 { 571 m_pairCache->removeOverlappingPairsContainingProxy(pHandle,dispatcher); 572 } 573 574 // compute current limit of edge arrays 575 int limit = static_cast<int>(m_numHandles * 2); 576 577 int axis; 578 579 for (axis = 0;axis<3;axis++) 580 { 581 m_pHandles[0].m_maxEdges[axis] -= 2; 582 } 583 584 // remove the edges by sorting them up to the end of the list 585 for ( axis = 0; axis < 3; axis++) 586 { 587 Edge* pEdges = m_pEdges[axis]; 588 BP_FP_INT_TYPE max = pHandle->m_maxEdges[axis]; 589 pEdges[max].m_pos = m_handleSentinel; 590 591 sortMaxUp(axis,max,dispatcher,false); 592 593 594 BP_FP_INT_TYPE i = pHandle->m_minEdges[axis]; 595 pEdges[i].m_pos = m_handleSentinel; 596 597 598 sortMinUp(axis,i,dispatcher,false); 599 600 pEdges[limit-1].m_handle = 0; 601 pEdges[limit-1].m_pos = m_handleSentinel; 602 603 #ifdef DEBUG_BROADPHASE 604 debugPrintAxis(axis,false); 605 #endif //DEBUG_BROADPHASE 606 607 608 } 609 610 611 // free the handle 612 freeHandle(handle); 613 614 615 } 616 617 template <typename BP_FP_INT_TYPE> 618 void btAxisSweep3Internal<BP_FP_INT_TYPE>::resetPool(btDispatcher* /*dispatcher*/) 619 { 620 if (m_numHandles == 0) 621 { 622 m_firstFreeHandle = 1; 623 { 624 for (BP_FP_INT_TYPE i = m_firstFreeHandle; i < m_maxHandles; i++) 625 m_pHandles[i].SetNextFree(static_cast<BP_FP_INT_TYPE>(i + 1)); 626 m_pHandles[m_maxHandles - 1].SetNextFree(0); 627 } 628 } 629 } 630 631 632 extern int gOverlappingPairs; 633 //#include <stdio.h> 634 635 template <typename BP_FP_INT_TYPE> 636 void btAxisSweep3Internal<BP_FP_INT_TYPE>::calculateOverlappingPairs(btDispatcher* dispatcher) 637 { 638 639 if (m_pairCache->hasDeferredRemoval()) 640 { 641 642 btBroadphasePairArray& overlappingPairArray = m_pairCache->getOverlappingPairArray(); 643 644 //perform a sort, to find duplicates and to sort 'invalid' pairs to the end 645 overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); 646 647 overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); 648 m_invalidPair = 0; 649 650 651 int i; 652 653 btBroadphasePair previousPair; 654 previousPair.m_pProxy0 = 0; 655 previousPair.m_pProxy1 = 0; 656 previousPair.m_algorithm = 0; 657 658 659 for (i=0;i<overlappingPairArray.size();i++) 660 { 661 662 btBroadphasePair& pair = overlappingPairArray[i]; 663 664 bool isDuplicate = (pair == previousPair); 665 666 previousPair = pair; 667 668 bool needsRemoval = false; 669 670 if (!isDuplicate) 671 { 672 ///important to use an AABB test that is consistent with the broadphase 673 bool hasOverlap = testAabbOverlap(pair.m_pProxy0,pair.m_pProxy1); 674 675 if (hasOverlap) 676 { 677 needsRemoval = false;//callback->processOverlap(pair); 678 } else 679 { 680 needsRemoval = true; 681 } 682 } else 683 { 684 //remove duplicate 685 needsRemoval = true; 686 //should have no algorithm 687 btAssert(!pair.m_algorithm); 688 } 689 690 if (needsRemoval) 691 { 692 m_pairCache->cleanOverlappingPair(pair,dispatcher); 693 694 // m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1); 695 // m_overlappingPairArray.pop_back(); 696 pair.m_pProxy0 = 0; 697 pair.m_pProxy1 = 0; 698 m_invalidPair++; 699 gOverlappingPairs--; 700 } 701 702 } 703 704 ///if you don't like to skip the invalid pairs in the array, execute following code: 705 #define CLEAN_INVALID_PAIRS 1 706 #ifdef CLEAN_INVALID_PAIRS 707 708 //perform a sort, to sort 'invalid' pairs to the end 709 overlappingPairArray.quickSort(btBroadphasePairSortPredicate()); 710 711 overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair); 712 m_invalidPair = 0; 713 #endif//CLEAN_INVALID_PAIRS 714 715 //printf("overlappingPairArray.size()=%d\n",overlappingPairArray.size()); 716 } 717 718 } 719 720 721 template <typename BP_FP_INT_TYPE> 722 bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testAabbOverlap(btBroadphaseProxy* proxy0,btBroadphaseProxy* proxy1) 723 { 724 const Handle* pHandleA = static_cast<Handle*>(proxy0); 725 const Handle* pHandleB = static_cast<Handle*>(proxy1); 726 727 //optimization 1: check the array index (memory address), instead of the m_pos 728 729 for (int axis = 0; axis < 3; axis++) 730 { 731 if (pHandleA->m_maxEdges[axis] < pHandleB->m_minEdges[axis] || 732 pHandleB->m_maxEdges[axis] < pHandleA->m_minEdges[axis]) 733 { 734 return false; 735 } 736 } 737 return true; 738 } 739 740 template <typename BP_FP_INT_TYPE> 741 bool btAxisSweep3Internal<BP_FP_INT_TYPE>::testOverlap2D(const Handle* pHandleA, const Handle* pHandleB,int axis0,int axis1) 742 { 743 //optimization 1: check the array index (memory address), instead of the m_pos 744 745 if (pHandleA->m_maxEdges[axis0] < pHandleB->m_minEdges[axis0] || 746 pHandleB->m_maxEdges[axis0] < pHandleA->m_minEdges[axis0] || 747 pHandleA->m_maxEdges[axis1] < pHandleB->m_minEdges[axis1] || 748 pHandleB->m_maxEdges[axis1] < pHandleA->m_minEdges[axis1]) 749 { 750 return false; 751 } 752 return true; 753 } 754 755 template <typename BP_FP_INT_TYPE> 756 void btAxisSweep3Internal<BP_FP_INT_TYPE>::updateHandle(BP_FP_INT_TYPE handle, const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher) 757 { 758 // btAssert(bounds.IsFinite()); 759 //btAssert(bounds.HasVolume()); 760 761 Handle* pHandle = getHandle(handle); 762 763 // quantize the new bounds 764 BP_FP_INT_TYPE min[3], max[3]; 765 quantize(min, aabbMin, 0); 766 quantize(max, aabbMax, 1); 767 768 // update changed edges 769 for (int axis = 0; axis < 3; axis++) 770 { 771 BP_FP_INT_TYPE emin = pHandle->m_minEdges[axis]; 772 BP_FP_INT_TYPE emax = pHandle->m_maxEdges[axis]; 773 774 int dmin = (int)min[axis] - (int)m_pEdges[axis][emin].m_pos; 775 int dmax = (int)max[axis] - (int)m_pEdges[axis][emax].m_pos; 776 777 m_pEdges[axis][emin].m_pos = min[axis]; 778 m_pEdges[axis][emax].m_pos = max[axis]; 779 780 // expand (only adds overlaps) 781 if (dmin < 0) 782 sortMinDown(axis, emin,dispatcher,true); 783 784 if (dmax > 0) 785 sortMaxUp(axis, emax,dispatcher,true); 786 787 // shrink (only removes overlaps) 788 if (dmin > 0) 789 sortMinUp(axis, emin,dispatcher,true); 790 791 if (dmax < 0) 792 sortMaxDown(axis, emax,dispatcher,true); 793 794 #ifdef DEBUG_BROADPHASE 795 debugPrintAxis(axis); 796 #endif //DEBUG_BROADPHASE 797 } 798 799 800 } 801 802 803 804 805 // sorting a min edge downwards can only ever *add* overlaps 806 template <typename BP_FP_INT_TYPE> 807 void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* /* dispatcher */, bool updateOverlaps) 808 { 809 810 Edge* pEdge = m_pEdges[axis] + edge; 811 Edge* pPrev = pEdge - 1; 812 Handle* pHandleEdge = getHandle(pEdge->m_handle); 813 814 while (pEdge->m_pos < pPrev->m_pos) 815 { 816 Handle* pHandlePrev = getHandle(pPrev->m_handle); 817 818 if (pPrev->IsMax()) 819 { 820 // if previous edge is a maximum check the bounds and add an overlap if necessary 821 const int axis1 = (1 << axis) & 3; 822 const int axis2 = (1 << axis1) & 3; 823 if (updateOverlaps && testOverlap2D(pHandleEdge, pHandlePrev,axis1,axis2)) 824 { 825 m_pairCache->addOverlappingPair(pHandleEdge,pHandlePrev); 826 if (m_userPairCallback) 827 m_userPairCallback->addOverlappingPair(pHandleEdge,pHandlePrev); 828 829 //AddOverlap(pEdge->m_handle, pPrev->m_handle); 830 831 } 832 833 // update edge reference in other handle 834 pHandlePrev->m_maxEdges[axis]++; 835 } 836 else 837 pHandlePrev->m_minEdges[axis]++; 838 839 pHandleEdge->m_minEdges[axis]--; 840 841 // swap the edges 842 Edge swap = *pEdge; 843 *pEdge = *pPrev; 844 *pPrev = swap; 845 846 // decrement 847 pEdge--; 848 pPrev--; 849 } 850 851 #ifdef DEBUG_BROADPHASE 852 debugPrintAxis(axis); 853 #endif //DEBUG_BROADPHASE 854 855 } 856 857 // sorting a min edge upwards can only ever *remove* overlaps 858 template <typename BP_FP_INT_TYPE> 859 void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMinUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps) 860 { 861 Edge* pEdge = m_pEdges[axis] + edge; 862 Edge* pNext = pEdge + 1; 863 Handle* pHandleEdge = getHandle(pEdge->m_handle); 864 865 while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos)) 866 { 867 Handle* pHandleNext = getHandle(pNext->m_handle); 868 869 if (pNext->IsMax()) 870 { 871 Handle* handle0 = getHandle(pEdge->m_handle); 872 Handle* handle1 = getHandle(pNext->m_handle); 873 const int axis1 = (1 << axis) & 3; 874 const int axis2 = (1 << axis1) & 3; 875 876 // if next edge is maximum remove any overlap between the two handles 877 if (updateOverlaps 878 #ifdef USE_OVERLAP_TEST_ON_REMOVES 879 && testOverlap2D(handle0,handle1,axis1,axis2) 880 #endif //USE_OVERLAP_TEST_ON_REMOVES 881 ) 882 { 883 884 885 m_pairCache->removeOverlappingPair(handle0,handle1,dispatcher); 886 if (m_userPairCallback) 887 m_userPairCallback->removeOverlappingPair(handle0,handle1,dispatcher); 888 889 } 890 891 892 // update edge reference in other handle 893 pHandleNext->m_maxEdges[axis]--; 894 } 895 else 896 pHandleNext->m_minEdges[axis]--; 897 898 pHandleEdge->m_minEdges[axis]++; 899 900 // swap the edges 901 Edge swap = *pEdge; 902 *pEdge = *pNext; 903 *pNext = swap; 904 905 // increment 906 pEdge++; 907 pNext++; 908 } 909 910 911 } 912 913 // sorting a max edge downwards can only ever *remove* overlaps 914 template <typename BP_FP_INT_TYPE> 915 void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxDown(int axis, BP_FP_INT_TYPE edge, btDispatcher* dispatcher, bool updateOverlaps) 916 { 917 918 Edge* pEdge = m_pEdges[axis] + edge; 919 Edge* pPrev = pEdge - 1; 920 Handle* pHandleEdge = getHandle(pEdge->m_handle); 921 922 while (pEdge->m_pos < pPrev->m_pos) 923 { 924 Handle* pHandlePrev = getHandle(pPrev->m_handle); 925 926 if (!pPrev->IsMax()) 927 { 928 // if previous edge was a minimum remove any overlap between the two handles 929 Handle* handle0 = getHandle(pEdge->m_handle); 930 Handle* handle1 = getHandle(pPrev->m_handle); 931 const int axis1 = (1 << axis) & 3; 932 const int axis2 = (1 << axis1) & 3; 933 934 if (updateOverlaps 935 #ifdef USE_OVERLAP_TEST_ON_REMOVES 936 && testOverlap2D(handle0,handle1,axis1,axis2) 937 #endif //USE_OVERLAP_TEST_ON_REMOVES 938 ) 939 { 940 //this is done during the overlappingpairarray iteration/narrowphase collision 941 942 943 m_pairCache->removeOverlappingPair(handle0,handle1,dispatcher); 944 if (m_userPairCallback) 945 m_userPairCallback->removeOverlappingPair(handle0,handle1,dispatcher); 946 947 948 949 } 950 951 // update edge reference in other handle 952 pHandlePrev->m_minEdges[axis]++;; 953 } 954 else 955 pHandlePrev->m_maxEdges[axis]++; 956 957 pHandleEdge->m_maxEdges[axis]--; 958 959 // swap the edges 960 Edge swap = *pEdge; 961 *pEdge = *pPrev; 962 *pPrev = swap; 963 964 // decrement 965 pEdge--; 966 pPrev--; 967 } 968 969 970 #ifdef DEBUG_BROADPHASE 971 debugPrintAxis(axis); 972 #endif //DEBUG_BROADPHASE 973 974 } 975 976 // sorting a max edge upwards can only ever *add* overlaps 977 template <typename BP_FP_INT_TYPE> 978 void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxUp(int axis, BP_FP_INT_TYPE edge, btDispatcher* /* dispatcher */, bool updateOverlaps) 979 { 980 Edge* pEdge = m_pEdges[axis] + edge; 981 Edge* pNext = pEdge + 1; 982 Handle* pHandleEdge = getHandle(pEdge->m_handle); 983 984 while (pNext->m_handle && (pEdge->m_pos >= pNext->m_pos)) 985 { 986 Handle* pHandleNext = getHandle(pNext->m_handle); 987 988 const int axis1 = (1 << axis) & 3; 989 const int axis2 = (1 << axis1) & 3; 990 991 if (!pNext->IsMax()) 992 { 993 // if next edge is a minimum check the bounds and add an overlap if necessary 994 if (updateOverlaps && testOverlap2D(pHandleEdge, pHandleNext,axis1,axis2)) 995 { 996 Handle* handle0 = getHandle(pEdge->m_handle); 997 Handle* handle1 = getHandle(pNext->m_handle); 998 m_pairCache->addOverlappingPair(handle0,handle1); 999 if (m_userPairCallback) 1000 m_userPairCallback->addOverlappingPair(handle0,handle1); 1001 } 1002 1003 // update edge reference in other handle 1004 pHandleNext->m_minEdges[axis]--; 1005 } 1006 else 1007 pHandleNext->m_maxEdges[axis]--; 1008 1009 pHandleEdge->m_maxEdges[axis]++; 1010 1011 // swap the edges 1012 Edge swap = *pEdge; 1013 *pEdge = *pNext; 1014 *pNext = swap; 1015 1016 // increment 1017 pEdge++; 1018 pNext++; 1019 } 1020 1021 } 1022 1023 1024 1025 //////////////////////////////////////////////////////////////////// 1026 1027 1028 /// The btAxisSweep3 is an efficient implementation of the 3d axis sweep and prune broadphase. 1029 /// It uses arrays rather then lists for storage of the 3 axis. Also it operates using 16 bit integer coordinates instead of floats. 1030 /// For large worlds and many objects, use bt32BitAxisSweep3 or btDbvtBroadphase instead. bt32BitAxisSweep3 has higher precision and allows more then 16384 objects at the cost of more memory and bit of performance. 1031 class btAxisSweep3 : public btAxisSweep3Internal<unsigned short int> 1032 { 1033 public: 1034 1035 btAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned short int maxHandles = 16384, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false); 1036 1037 }; 1038 1039 /// The bt32BitAxisSweep3 allows higher precision quantization and more objects compared to the btAxisSweep3 sweep and prune. 1040 /// This comes at the cost of more memory per handle, and a bit slower performance. 1041 /// It uses arrays rather then lists for storage of the 3 axis. 1042 class bt32BitAxisSweep3 : public btAxisSweep3Internal<unsigned int> 1043 { 1044 public: 1045 1046 bt32BitAxisSweep3(const btVector3& worldAabbMin,const btVector3& worldAabbMax, unsigned int maxHandles = 1500000, btOverlappingPairCache* pairCache = 0, bool disableRaycastAccelerator = false); 1047 1048 }; 1049 1050 #endif 1051 1052
