dali-physics/third-party/bullet3/src/BulletCollision/CollisionDispatch/btCollisionDispatcherMt.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 "btCollisionDispatcherMt.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 btCollisionDispatcherMt::btCollisionDispatcherMt(btCollisionConfiguration* config, int grainSize)
  29         : btCollisionDispatcher(config)
  30 {
  31         m_batchManifoldsPtr.resize(btGetTaskScheduler()->getNumThreads());
  32         m_batchReleasePtr.resize(btGetTaskScheduler()->getNumThreads());
  33
  34         m_batchUpdating = false;
  35         m_grainSize = grainSize;  // iterations per task
  36 }
  37
  38 btPersistentManifold* btCollisionDispatcherMt::getNewManifold(const btCollisionObject* body0, const btCollisionObject* body1)
  39 {
  40         //optional relative contact breaking threshold, turned on by default (use setDispatcherFlags to switch off feature for improved performance)
  41
  42         btScalar contactBreakingThreshold = (m_dispatcherFlags & btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD) ? btMin(body0->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold), body1->getCollisionShape()->getContactBreakingThreshold(gContactBreakingThreshold))
  43                                                                                                                                                                                                                                                                 : gContactBreakingThreshold;
  44
  45         btScalar contactProcessingThreshold = btMin(body0->getContactProcessingThreshold(), body1->getContactProcessingThreshold());
  46
  47         void* mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold));
  48         if (NULL == mem)
  49         {
  50                 //we got a pool memory overflow, by default we fallback to dynamically allocate memory. If we require a contiguous contact pool then assert.
  51                 if ((m_dispatcherFlags & CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION) == 0)
  52                 {
  53                         mem = btAlignedAlloc(sizeof(btPersistentManifold), 16);
  54                 }
  55                 else
  56                 {
  57                         btAssert(0);
  58                         //make sure to increase the m_defaultMaxPersistentManifoldPoolSize in the btDefaultCollisionConstructionInfo/btDefaultCollisionConfiguration
  59                         return 0;
  60                 }
  61         }
  62         btPersistentManifold* manifold = new (mem) btPersistentManifold(body0, body1, 0, contactBreakingThreshold, contactProcessingThreshold);
  63         if (!m_batchUpdating)
  64         {
  65                 // batch updater will update manifold pointers array after finishing, so
  66                 // only need to update array when not batch-updating
  67                 //btAssert( !btThreadsAreRunning() );
  68                 manifold->m_index1a = m_manifoldsPtr.size();
  69                 m_manifoldsPtr.push_back(manifold);
  70         }
  71         else
  72         {
  73                 m_batchManifoldsPtr[btGetCurrentThreadIndex()].push_back(manifold);
  74         }
  75
  76         return manifold;
  77 }
  78
  79 void btCollisionDispatcherMt::releaseManifold(btPersistentManifold* manifold)
  80 {
  81         //btAssert( !btThreadsAreRunning() );
  82
  83         if (!m_batchUpdating)
  84         {
  85                 clearManifold(manifold);
  86                 // batch updater will update manifold pointers array after finishing, so
  87                 // only need to update array when not batch-updating
  88                 int findIndex = manifold->m_index1a;
  89                 btAssert(findIndex < m_manifoldsPtr.size());
  90                 m_manifoldsPtr.swap(findIndex, m_manifoldsPtr.size() - 1);
  91                 m_manifoldsPtr[findIndex]->m_index1a = findIndex;
  92                 m_manifoldsPtr.pop_back();
  93         } else {
  94                 m_batchReleasePtr[btGetCurrentThreadIndex()].push_back(manifold);
  95                 return;
  96         }
  97
  98         manifold->~btPersistentManifold();
  99         if (m_persistentManifoldPoolAllocator->validPtr(manifold))
 100         {
 101                 m_persistentManifoldPoolAllocator->freeMemory(manifold);
 102         }
 103         else
 104         {
 105                 btAlignedFree(manifold);
 106         }
 107 }
 108
 109 struct CollisionDispatcherUpdater : public btIParallelForBody
 110 {
 111         btBroadphasePair* mPairArray;
 112         btNearCallback mCallback;
 113         btCollisionDispatcher* mDispatcher;
 114         const btDispatcherInfo* mInfo;
 115
 116         CollisionDispatcherUpdater()
 117         {
 118                 mPairArray = NULL;
 119                 mCallback = NULL;
 120                 mDispatcher = NULL;
 121                 mInfo = NULL;
 122         }
 123         void forLoop(int iBegin, int iEnd) const
 124         {
 125                 for (int i = iBegin; i < iEnd; ++i)
 126                 {
 127                         btBroadphasePair* pair = &mPairArray[i];
 128                         mCallback(*pair, *mDispatcher, *mInfo);
 129                 }
 130         }
 131 };
 132
 133 void btCollisionDispatcherMt::dispatchAllCollisionPairs(btOverlappingPairCache* pairCache, const btDispatcherInfo& info, btDispatcher* dispatcher)
 134 {
 135         const int pairCount = pairCache->getNumOverlappingPairs();
 136         if (pairCount == 0)
 137         {
 138                 return;
 139         }
 140         CollisionDispatcherUpdater updater;
 141         updater.mCallback = getNearCallback();
 142         updater.mPairArray = pairCache->getOverlappingPairArrayPtr();
 143         updater.mDispatcher = this;
 144         updater.mInfo = &info;
 145
 146         m_batchUpdating = true;
 147         btParallelFor(0, pairCount, m_grainSize, updater);
 148         m_batchUpdating = false;
 149
 150         // merge new manifolds, if any
 151         for (int i = 0; i < m_batchManifoldsPtr.size(); ++i)
 152         {
 153                 btAlignedObjectArray<btPersistentManifold*>& batchManifoldsPtr = m_batchManifoldsPtr[i];
 154
 155                 for (int j = 0; j < batchManifoldsPtr.size(); ++j)
 156                 {
 157                         m_manifoldsPtr.push_back(batchManifoldsPtr[j]);
 158                 }
 159
 160                 batchManifoldsPtr.resizeNoInitialize(0);
 161         }
 162
 163         // remove batched remove manifolds.
 164         for (int i = 0; i < m_batchReleasePtr.size(); ++i)
 165         {
 166                 btAlignedObjectArray<btPersistentManifold*>& batchManifoldsPtr = m_batchReleasePtr[i];
 167                 for (int j = 0; j < batchManifoldsPtr.size(); ++j)
 168                 {
 169                         releaseManifold(batchManifoldsPtr[j]);
 170                 }
 171                 batchManifoldsPtr.resizeNoInitialize(0);
 172         }
 173
 174         // update the indices (used when releasing manifolds)
 175         for (int i = 0; i < m_manifoldsPtr.size(); ++i)
 176         {
 177                 m_manifoldsPtr[i]->m_index1a = i;
 178         }
 179 }