dali-physics/third-party/bullet3/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 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.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 #include "btSimpleBroadphase.h"
  17 #include "BulletCollision/BroadphaseCollision/btDispatcher.h"
  18 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
  19
  20 #include "LinearMath/btVector3.h"
  21 #include "LinearMath/btTransform.h"
  22 #include "LinearMath/btMatrix3x3.h"
  23 #include "LinearMath/btAabbUtil2.h"
  24
  25 #include <new>
  26
  27 void btSimpleBroadphase::validate()
  28 {
  29         for (int i = 0; i < m_numHandles; i++)
  30         {
  31                 for (int j = i + 1; j < m_numHandles; j++)
  32                 {
  33                         btAssert(&m_pHandles[i] != &m_pHandles[j]);
  34                 }
  35         }
  36 }
  37
  38 btSimpleBroadphase::btSimpleBroadphase(int maxProxies, btOverlappingPairCache* overlappingPairCache)
  39         : m_pairCache(overlappingPairCache),
  40           m_ownsPairCache(false),
  41           m_invalidPair(0)
  42 {
  43         if (!overlappingPairCache)
  44         {
  45                 void* mem = btAlignedAlloc(sizeof(btHashedOverlappingPairCache), 16);
  46                 m_pairCache = new (mem) btHashedOverlappingPairCache();
  47                 m_ownsPairCache = true;
  48         }
  49
  50         // allocate handles buffer and put all handles on free list
  51         m_pHandlesRawPtr = btAlignedAlloc(sizeof(btSimpleBroadphaseProxy) * maxProxies, 16);
  52         m_pHandles = new (m_pHandlesRawPtr) btSimpleBroadphaseProxy[maxProxies];
  53         m_maxHandles = maxProxies;
  54         m_numHandles = 0;
  55         m_firstFreeHandle = 0;
  56         m_LastHandleIndex = -1;
  57
  58         {
  59                 for (int i = m_firstFreeHandle; i < maxProxies; i++)
  60                 {
  61                         m_pHandles[i].SetNextFree(i + 1);
  62                         m_pHandles[i].m_uniqueId = i + 2;  //any UID will do, we just avoid too trivial values (0,1) for debugging purposes
  63                 }
  64                 m_pHandles[maxProxies - 1].SetNextFree(0);
  65         }
  66 }
  67
  68 btSimpleBroadphase::~btSimpleBroadphase()
  69 {
  70         btAlignedFree(m_pHandlesRawPtr);
  71
  72         if (m_ownsPairCache)
  73         {
  74                 m_pairCache->~btOverlappingPairCache();
  75                 btAlignedFree(m_pairCache);
  76         }
  77 }
  78
  79 btBroadphaseProxy* btSimpleBroadphase::createProxy(const btVector3& aabbMin, const btVector3& aabbMax, int shapeType, void* userPtr, int collisionFilterGroup, int collisionFilterMask, btDispatcher* /*dispatcher*/)
  80 {
  81         if (m_numHandles >= m_maxHandles)
  82         {
  83                 btAssert(0);
  84                 return 0;  //should never happen, but don't let the game crash ;-)
  85         }
  86         btAssert(aabbMin[0] <= aabbMax[0] && aabbMin[1] <= aabbMax[1] && aabbMin[2] <= aabbMax[2]);
  87
  88         int newHandleIndex = allocHandle();
  89         btSimpleBroadphaseProxy* proxy = new (&m_pHandles[newHandleIndex]) btSimpleBroadphaseProxy(aabbMin, aabbMax, shapeType, userPtr, collisionFilterGroup, collisionFilterMask);
  90
  91         return proxy;
  92 }
  93
  94 class RemovingOverlapCallback : public btOverlapCallback
  95 {
  96 protected:
  97         virtual bool processOverlap(btBroadphasePair& pair)
  98         {
  99                 (void)pair;
 100                 btAssert(0);
 101                 return false;
 102         }
 103 };
 104
 105 class RemovePairContainingProxy
 106 {
 107         btBroadphaseProxy* m_targetProxy;
 108
 109 public:
 110         virtual ~RemovePairContainingProxy()
 111         {
 112         }
 113
 114 protected:
 115         virtual bool processOverlap(btBroadphasePair& pair)
 116         {
 117                 btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy0);
 118                 btSimpleBroadphaseProxy* proxy1 = static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy1);
 119
 120                 return ((m_targetProxy == proxy0 || m_targetProxy == proxy1));
 121         };
 122 };
 123
 124 void btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg, btDispatcher* dispatcher)
 125 {
 126         m_pairCache->removeOverlappingPairsContainingProxy(proxyOrg, dispatcher);
 127
 128         btSimpleBroadphaseProxy* proxy0 = static_cast<btSimpleBroadphaseProxy*>(proxyOrg);
 129         freeHandle(proxy0);
 130
 131         //validate();
 132 }
 133
 134 void btSimpleBroadphase::getAabb(btBroadphaseProxy* proxy, btVector3& aabbMin, btVector3& aabbMax) const
 135 {
 136         const btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy);
 137         aabbMin = sbp->m_aabbMin;
 138         aabbMax = sbp->m_aabbMax;
 139 }
 140
 141 void btSimpleBroadphase::setAabb(btBroadphaseProxy* proxy, const btVector3& aabbMin, const btVector3& aabbMax, btDispatcher* /*dispatcher*/)
 142 {
 143         btSimpleBroadphaseProxy* sbp = getSimpleProxyFromProxy(proxy);
 144         sbp->m_aabbMin = aabbMin;
 145         sbp->m_aabbMax = aabbMax;
 146 }
 147
 148 void btSimpleBroadphase::rayTest(const btVector3& rayFrom, const btVector3& rayTo, btBroadphaseRayCallback& rayCallback, const btVector3& aabbMin, const btVector3& aabbMax)
 149 {
 150         for (int i = 0; i <= m_LastHandleIndex; i++)
 151         {
 152                 btSimpleBroadphaseProxy* proxy = &m_pHandles[i];
 153                 if (!proxy->m_clientObject)
 154                 {
 155                         continue;
 156                 }
 157                 rayCallback.process(proxy);
 158         }
 159 }
 160
 161 void btSimpleBroadphase::aabbTest(const btVector3& aabbMin, const btVector3& aabbMax, btBroadphaseAabbCallback& callback)
 162 {
 163         for (int i = 0; i <= m_LastHandleIndex; i++)
 164         {
 165                 btSimpleBroadphaseProxy* proxy = &m_pHandles[i];
 166                 if (!proxy->m_clientObject)
 167                 {
 168                         continue;
 169                 }
 170                 if (TestAabbAgainstAabb2(aabbMin, aabbMax, proxy->m_aabbMin, proxy->m_aabbMax))
 171                 {
 172                         callback.process(proxy);
 173                 }
 174         }
 175 }
 176
 177 bool btSimpleBroadphase::aabbOverlap(btSimpleBroadphaseProxy* proxy0, btSimpleBroadphaseProxy* proxy1)
 178 {
 179         return proxy0->m_aabbMin[0] <= proxy1->m_aabbMax[0] && proxy1->m_aabbMin[0] <= proxy0->m_aabbMax[0] &&
 180                    proxy0->m_aabbMin[1] <= proxy1->m_aabbMax[1] && proxy1->m_aabbMin[1] <= proxy0->m_aabbMax[1] &&
 181                    proxy0->m_aabbMin[2] <= proxy1->m_aabbMax[2] && proxy1->m_aabbMin[2] <= proxy0->m_aabbMax[2];
 182 }
 183
 184 //then remove non-overlapping ones
 185 class CheckOverlapCallback : public btOverlapCallback
 186 {
 187 public:
 188         virtual bool processOverlap(btBroadphasePair& pair)
 189         {
 190                 return (!btSimpleBroadphase::aabbOverlap(static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy0), static_cast<btSimpleBroadphaseProxy*>(pair.m_pProxy1)));
 191         }
 192 };
 193
 194 void btSimpleBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher)
 195 {
 196         //first check for new overlapping pairs
 197         int i, j;
 198         if (m_numHandles >= 0)
 199         {
 200                 int new_largest_index = -1;
 201                 for (i = 0; i <= m_LastHandleIndex; i++)
 202                 {
 203                         btSimpleBroadphaseProxy* proxy0 = &m_pHandles[i];
 204                         if (!proxy0->m_clientObject)
 205                         {
 206                                 continue;
 207                         }
 208                         new_largest_index = i;
 209                         for (j = i + 1; j <= m_LastHandleIndex; j++)
 210                         {
 211                                 btSimpleBroadphaseProxy* proxy1 = &m_pHandles[j];
 212                                 btAssert(proxy0 != proxy1);
 213                                 if (!proxy1->m_clientObject)
 214                                 {
 215                                         continue;
 216                                 }
 217
 218                                 btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
 219                                 btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
 220
 221                                 if (aabbOverlap(p0, p1))
 222                                 {
 223                                         if (!m_pairCache->findPair(proxy0, proxy1))
 224                                         {
 225                                                 m_pairCache->addOverlappingPair(proxy0, proxy1);
 226                                         }
 227                                 }
 228                                 else
 229                                 {
 230                                         if (!m_pairCache->hasDeferredRemoval())
 231                                         {
 232                                                 if (m_pairCache->findPair(proxy0, proxy1))
 233                                                 {
 234                                                         m_pairCache->removeOverlappingPair(proxy0, proxy1, dispatcher);
 235                                                 }
 236                                         }
 237                                 }
 238                         }
 239                 }
 240
 241                 m_LastHandleIndex = new_largest_index;
 242
 243                 if (m_ownsPairCache && m_pairCache->hasDeferredRemoval())
 244                 {
 245                         btBroadphasePairArray& overlappingPairArray = m_pairCache->getOverlappingPairArray();
 246
 247                         //perform a sort, to find duplicates and to sort 'invalid' pairs to the end
 248                         overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
 249
 250                         overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
 251                         m_invalidPair = 0;
 252
 253                         btBroadphasePair previousPair;
 254                         previousPair.m_pProxy0 = 0;
 255                         previousPair.m_pProxy1 = 0;
 256                         previousPair.m_algorithm = 0;
 257
 258                         for (i = 0; i < overlappingPairArray.size(); i++)
 259                         {
 260                                 btBroadphasePair& pair = overlappingPairArray[i];
 261
 262                                 bool isDuplicate = (pair == previousPair);
 263
 264                                 previousPair = pair;
 265
 266                                 bool needsRemoval = false;
 267
 268                                 if (!isDuplicate)
 269                                 {
 270                                         bool hasOverlap = testAabbOverlap(pair.m_pProxy0, pair.m_pProxy1);
 271
 272                                         if (hasOverlap)
 273                                         {
 274                                                 needsRemoval = false;  //callback->processOverlap(pair);
 275                                         }
 276                                         else
 277                                         {
 278                                                 needsRemoval = true;
 279                                         }
 280                                 }
 281                                 else
 282                                 {
 283                                         //remove duplicate
 284                                         needsRemoval = true;
 285                                         //should have no algorithm
 286                                         btAssert(!pair.m_algorithm);
 287                                 }
 288
 289                                 if (needsRemoval)
 290                                 {
 291                                         m_pairCache->cleanOverlappingPair(pair, dispatcher);
 292
 293                                         //              m_overlappingPairArray.swap(i,m_overlappingPairArray.size()-1);
 294                                         //              m_overlappingPairArray.pop_back();
 295                                         pair.m_pProxy0 = 0;
 296                                         pair.m_pProxy1 = 0;
 297                                         m_invalidPair++;
 298                                 }
 299                         }
 300
 301                         ///if you don't like to skip the invalid pairs in the array, execute following code:
 302 #define CLEAN_INVALID_PAIRS 1
 303 #ifdef CLEAN_INVALID_PAIRS
 304
 305                         //perform a sort, to sort 'invalid' pairs to the end
 306                         overlappingPairArray.quickSort(btBroadphasePairSortPredicate());
 307
 308                         overlappingPairArray.resize(overlappingPairArray.size() - m_invalidPair);
 309                         m_invalidPair = 0;
 310 #endif  //CLEAN_INVALID_PAIRS
 311                 }
 312         }
 313 }
 314
 315 bool btSimpleBroadphase::testAabbOverlap(btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1)
 316 {
 317         btSimpleBroadphaseProxy* p0 = getSimpleProxyFromProxy(proxy0);
 318         btSimpleBroadphaseProxy* p1 = getSimpleProxyFromProxy(proxy1);
 319         return aabbOverlap(p0, p1);
 320 }
 321
 322 void btSimpleBroadphase::resetPool(btDispatcher* dispatcher)
 323 {
 324         //not yet
 325 }