dali-physics/third-party/bullet3/src/BulletCollision/BroadphaseCollision/btAxisSweep3Internal.h

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