dali-physics/third-party/bullet3/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.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 "btCollisionDispatcher.h"
  17 #include "LinearMath/btQuickprof.h"
  18
  19 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
  20
  21 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
  22 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
  23 #include "BulletCollision/BroadphaseCollision/btOverlappingPairCache.h"
  24 #include "LinearMath/btPoolAllocator.h"
  25 #include "BulletCollision/CollisionDispatch/btCollisionConfiguration.h"
  26 #include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
  27
  28 #ifdef BT_DEBUG
  29 #include <stdio.h>
  30 #endif
  31
  32 btCollisionDispatcher::btCollisionDispatcher(btCollisionConfiguration* collisionConfiguration) : m_dispatcherFlags(btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD),
  33                                                                                                                                                                                                  m_collisionConfiguration(collisionConfiguration)
  34 {
  35         int i;
  36
  37         setNearCallback(defaultNearCallback);
  38
  39         m_collisionAlgorithmPoolAllocator = collisionConfiguration->getCollisionAlgorithmPool();
  40
  41         m_persistentManifoldPoolAllocator = collisionConfiguration->getPersistentManifoldPool();
  42
  43         for (i = 0; i < MAX_BROADPHASE_COLLISION_TYPES; i++)
  44         {
  45                 for (int j = 0; j < MAX_BROADPHASE_COLLISION_TYPES; j++)
  46                 {
  47                         m_doubleDispatchContactPoints[i][j] = m_collisionConfiguration->getCollisionAlgorithmCreateFunc(i, j);
  48                         btAssert(m_doubleDispatchContactPoints[i][j]);
  49                         m_doubleDispatchClosestPoints[i][j] = m_collisionConfiguration->getClosestPointsAlgorithmCreateFunc(i, j);
  50                 }
  51         }
  52 }
  53
  54 void btCollisionDispatcher::registerCollisionCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc* createFunc)
  55 {
  56         m_doubleDispatchContactPoints[proxyType0][proxyType1] = createFunc;
  57 }
  58
  59 void btCollisionDispatcher::registerClosestPointsCreateFunc(int proxyType0, int proxyType1, btCollisionAlgorithmCreateFunc* createFunc)
  60 {
  61         m_doubleDispatchClosestPoints[proxyType0][proxyType1] = createFunc;
  62 }
  63
  64 btCollisionDispatcher::~btCollisionDispatcher()
  65 {
  66 }
  67
  68 btPersistentManifold* btCollisionDispatcher::getNewManifold(const btCollisionObject* body0, const btCollisionObject* body1)
  69 {
  70         //btAssert(gNumManifold < 65535);
  71
  72         //optional relative contact breaking threshold, turned on by default (use setDispatcherFlags to switch off feature for improved performance)
  73
  74         btScalar contactBreakingThreshold = (m_dispatcherFlags & btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD) ? btMin(body0->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold), body1->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold))
  75                                                                                                                                                                                                                                                                 : gContactBreakingThreshold;
  76
  77         btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(), body1->getContactProcessingThreshold());
  78
  79         void* mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold));
  80         if (NULL == mem)
  81         {
  82                 //we got a pool memory overflow, by default we fallback to dynamically allocate memory. If we require a contiguous contact pool then assert.
  83                 if ((m_dispatcherFlags & CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION) == 0)
  84                 {
  85                         mem = btAlignedAlloc(sizeof(btPersistentManifold), 16);
  86                 }
  87                 else
  88                 {
  89                         btAssert(0);
  90                         //make sure to increase the m_defaultMaxPersistentManifoldPoolSize in the btDefaultCollisionConstructionInfo/btDefaultCollisionConfiguration
  91                         return 0;
  92                 }
  93         }
  94         btPersistentManifold* manifold = new (mem) btPersistentManifold(body0, body1, 0, contactBreakingThreshold, contactProcessingThreshold);
  95         manifold->m_index1a = m_manifoldsPtr.size();
  96         m_manifoldsPtr.push_back(manifold);
  97
  98         return manifold;
  99 }
 100
 101 void btCollisionDispatcher::clearManifold(btPersistentManifold* manifold)
 102 {
 103         manifold->clearManifold();
 104 }
 105
 106 void btCollisionDispatcher::releaseManifold(btPersistentManifold* manifold)
 107 {
 108         //printf("releaseManifold: gNumManifold %d\n",gNumManifold);
 109         clearManifold(manifold);
 110
 111         int findIndex = manifold->m_index1a;
 112         btAssert(findIndex < m_manifoldsPtr.size());
 113         m_manifoldsPtr.swap(findIndex, m_manifoldsPtr.size() - 1);
 114         m_manifoldsPtr[findIndex]->m_index1a = findIndex;
 115         m_manifoldsPtr.pop_back();
 116
 117         manifold->~btPersistentManifold();
 118         if (m_persistentManifoldPoolAllocator->validPtr(manifold))
 119         {
 120                 m_persistentManifoldPoolAllocator->freeMemory(manifold);
 121         }
 122         else
 123         {
 124                 btAlignedFree(manifold);
 125         }
 126 }
 127
 128 btCollisionAlgorithm* btCollisionDispatcher::findAlgorithm(const btCollisionObjectWrapper* body0Wrap, const btCollisionObjectWrapper* body1Wrap, btPersistentManifold* sharedManifold, ebtDispatcherQueryType algoType)
 129 {
 130         btCollisionAlgorithmConstructionInfo ci;
 131
 132         ci.m_dispatcher1 = this;
 133         ci.m_manifold = sharedManifold;
 134         btCollisionAlgorithm* algo = 0;
 135         if (algoType == BT_CONTACT_POINT_ALGORITHMS)
 136         {
 137                 algo = m_doubleDispatchContactPoints[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci, body0Wrap, body1Wrap);
 138         }
 139         else
 140         {
 141                 algo = m_doubleDispatchClosestPoints[body0Wrap->getCollisionShape()->getShapeType()][body1Wrap->getCollisionShape()->getShapeType()]->CreateCollisionAlgorithm(ci, body0Wrap, body1Wrap);
 142         }
 143
 144         return algo;
 145 }
 146
 147 bool btCollisionDispatcher::needsResponse(const btCollisionObject* body0, const btCollisionObject* body1)
 148 {
 149         //here you can do filtering
 150         bool hasResponse =
 151                 (body0->hasContactResponse() && body1->hasContactResponse());
 152         //no response between two static/kinematic bodies:
 153         hasResponse = hasResponse &&
 154                                   ((!body0->isStaticOrKinematicObject()) || (!body1->isStaticOrKinematicObject()));
 155         return hasResponse;
 156 }
 157
 158 bool btCollisionDispatcher::needsCollision(const btCollisionObject* body0, const btCollisionObject* body1)
 159 {
 160         btAssert(body0);
 161         btAssert(body1);
 162
 163         bool needsCollision = true;
 164
 165 #ifdef BT_DEBUG
 166         if (!(m_dispatcherFlags & btCollisionDispatcher::CD_STATIC_STATIC_REPORTED))
 167         {
 168                 //broadphase filtering already deals with this
 169                 if (body0->isStaticOrKinematicObject() && body1->isStaticOrKinematicObject())
 170                 {
 171                         m_dispatcherFlags |= btCollisionDispatcher::CD_STATIC_STATIC_REPORTED;
 172                         printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n");
 173                 }
 174         }
 175 #endif  //BT_DEBUG
 176
 177         if ((!body0->isActive()) && (!body1->isActive()))
 178                 needsCollision = false;
 179         else if ((!body0->checkCollideWith(body1)) || (!body1->checkCollideWith(body0)))
 180                 needsCollision = false;
 181
 182         return needsCollision;
 183 }
 184
 185 ///interface for iterating all overlapping collision pairs, no matter how those pairs are stored (array, set, map etc)
 186 ///this is useful for the collision dispatcher.
 187 class btCollisionPairCallback : public btOverlapCallback
 188 {
 189         const btDispatcherInfo& m_dispatchInfo;
 190         btCollisionDispatcher* m_dispatcher;
 191
 192 public:
 193         btCollisionPairCallback(const btDispatcherInfo& dispatchInfo, btCollisionDispatcher* dispatcher)
 194                 : m_dispatchInfo(dispatchInfo),
 195                   m_dispatcher(dispatcher)
 196         {
 197         }
 198
 199         /*btCollisionPairCallback& operator=(btCollisionPairCallback& other)
 200         {
 201                 m_dispatchInfo = other.m_dispatchInfo;
 202                 m_dispatcher = other.m_dispatcher;
 203                 return *this;
 204         }
 205         */
 206
 207         virtual ~btCollisionPairCallback() {}
 208
 209         virtual bool processOverlap(btBroadphasePair& pair)
 210         {
 211                 (*m_dispatcher->getNearCallback())(pair, *m_dispatcher, m_dispatchInfo);
 212                 return false;
 213         }
 214 };
 215
 216 void btCollisionDispatcher::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& dispatchInfo, btDispatcher* dispatcher)
 217 {
 218         //m_blockedForChanges = true;
 219
 220         btCollisionPairCallback collisionCallback(dispatchInfo, this);
 221
 222         {
 223                 BT_PROFILE("processAllOverlappingPairs");
 224                 pairCache->processAllOverlappingPairs(&collisionCallback, dispatcher, dispatchInfo);
 225         }
 226
 227         //m_blockedForChanges = false;
 228 }
 229
 230 //by default, Bullet will use this near callback
 231 void btCollisionDispatcher::defaultNearCallback(btBroadphasePair& collisionPair, btCollisionDispatcher& dispatcher, const btDispatcherInfo& dispatchInfo)
 232 {
 233         btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
 234         btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
 235
 236         if (dispatcher.needsCollision(colObj0, colObj1))
 237         {
 238                 btCollisionObjectWrapper obj0Wrap(0, colObj0->getCollisionShape(), colObj0, colObj0->getWorldTransform(), -1, -1);
 239                 btCollisionObjectWrapper obj1Wrap(0, colObj1->getCollisionShape(), colObj1, colObj1->getWorldTransform(), -1, -1);
 240
 241                 //dispatcher will keep algorithms persistent in the collision pair
 242                 if (!collisionPair.m_algorithm)
 243                 {
 244                         collisionPair.m_algorithm = dispatcher.findAlgorithm(&obj0Wrap, &obj1Wrap, 0, BT_CONTACT_POINT_ALGORITHMS);
 245                 }
 246
 247                 if (collisionPair.m_algorithm)
 248                 {
 249                         btManifoldResult contactPointResult(&obj0Wrap, &obj1Wrap);
 250
 251                         if (dispatchInfo.m_dispatchFunc == btDispatcherInfo::DISPATCH_DISCRETE)
 252                         {
 253                                 //discrete collision detection query
 254
 255                                 collisionPair.m_algorithm->processCollision(&obj0Wrap, &obj1Wrap, dispatchInfo, &contactPointResult);
 256                         }
 257                         else
 258                         {
 259                                 //continuous collision detection query, time of impact (toi)
 260                                 btScalar toi = collisionPair.m_algorithm->calculateTimeOfImpact(colObj0, colObj1, dispatchInfo, &contactPointResult);
 261                                 if (dispatchInfo.m_timeOfImpact > toi)
 262                                         dispatchInfo.m_timeOfImpact = toi;
 263                         }
 264                 }
 265         }
 266 }
 267
 268 void* btCollisionDispatcher::allocateCollisionAlgorithm(int size)
 269 {
 270         void* mem = m_collisionAlgorithmPoolAllocator->allocate(size);
 271         if (NULL == mem)
 272         {
 273                 //warn user for overflow?
 274                 return btAlignedAlloc(static_cast<size_t>(size), 16);
 275         }
 276         return mem;
 277 }
 278
 279 void btCollisionDispatcher::freeCollisionAlgorithm(void* ptr)
 280 {
 281         if (m_collisionAlgorithmPoolAllocator->validPtr(ptr))
 282         {
 283                 m_collisionAlgorithmPoolAllocator->freeMemory(ptr);
 284         }
 285         else
 286         {
 287                 btAlignedFree(ptr);
 288         }
 289 }