src/BulletCollision/BroadphaseCollision/btAxisSweep3.h

   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