summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp')
-rw-r--r--thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp264
1 files changed, 113 insertions, 151 deletions
diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
index 91c76a8dac..e5097ccbbf 100644
--- a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
+++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
@@ -14,7 +14,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
-
#include "LinearMath/btScalar.h"
#include "btSimulationIslandManager.h"
#include "BulletCollision/BroadphaseCollision/btDispatcher.h"
@@ -25,8 +24,7 @@ subject to the following restrictions:
//#include <stdio.h>
#include "LinearMath/btQuickprof.h"
-btSimulationIslandManager::btSimulationIslandManager():
-m_splitIslands(true)
+btSimulationIslandManager::btSimulationIslandManager() : m_splitIslands(true)
{
}
@@ -34,53 +32,47 @@ btSimulationIslandManager::~btSimulationIslandManager()
{
}
-
void btSimulationIslandManager::initUnionFind(int n)
{
- m_unionFind.reset(n);
+ m_unionFind.reset(n);
}
-
-void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btCollisionWorld* colWorld)
+void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */, btCollisionWorld* colWorld)
{
-
{
btOverlappingPairCache* pairCachePtr = colWorld->getPairCache();
const int numOverlappingPairs = pairCachePtr->getNumOverlappingPairs();
if (numOverlappingPairs)
{
- btBroadphasePair* pairPtr = pairCachePtr->getOverlappingPairArrayPtr();
-
- for (int i=0;i<numOverlappingPairs;i++)
- {
- const btBroadphasePair& collisionPair = pairPtr[i];
- btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
- btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
+ btBroadphasePair* pairPtr = pairCachePtr->getOverlappingPairArrayPtr();
- if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
- ((colObj1) && ((colObj1)->mergesSimulationIslands())))
+ for (int i = 0; i < numOverlappingPairs; i++)
{
+ const btBroadphasePair& collisionPair = pairPtr[i];
+ btCollisionObject* colObj0 = (btCollisionObject*)collisionPair.m_pProxy0->m_clientObject;
+ btCollisionObject* colObj1 = (btCollisionObject*)collisionPair.m_pProxy1->m_clientObject;
- m_unionFind.unite((colObj0)->getIslandTag(),
- (colObj1)->getIslandTag());
+ if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
+ ((colObj1) && ((colObj1)->mergesSimulationIslands())))
+ {
+ m_unionFind.unite((colObj0)->getIslandTag(),
+ (colObj1)->getIslandTag());
+ }
}
}
- }
}
}
#ifdef STATIC_SIMULATION_ISLAND_OPTIMIZATION
-void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
+void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld, btDispatcher* dispatcher)
{
-
- // put the index into m_controllers into m_tag
+ // put the index into m_controllers into m_tag
int index = 0;
{
-
int i;
- for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
+ for (i = 0; i < colWorld->getCollisionObjectArray().size(); i++)
{
- btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
+ btCollisionObject* collisionObject = colWorld->getCollisionObjectArray()[i];
//Adding filtering here
if (!collisionObject->isStaticOrKinematicObject())
{
@@ -92,28 +84,29 @@ void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWor
}
// do the union find
- initUnionFind( index );
+ initUnionFind(index);
- findUnions(dispatcher,colWorld);
+ findUnions(dispatcher, colWorld);
}
-void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
+void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
{
- // put the islandId ('find' value) into m_tag
+ // put the islandId ('find' value) into m_tag
{
int index = 0;
int i;
- for (i=0;i<colWorld->getCollisionObjectArray().size();i++)
+ for (i = 0; i < colWorld->getCollisionObjectArray().size(); i++)
{
- btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
+ btCollisionObject* collisionObject = colWorld->getCollisionObjectArray()[i];
if (!collisionObject->isStaticOrKinematicObject())
{
- collisionObject->setIslandTag( m_unionFind.find(index) );
+ collisionObject->setIslandTag(m_unionFind.find(index));
//Set the correct object offset in Collision Object Array
m_unionFind.getElement(index).m_sz = i;
collisionObject->setCompanionId(-1);
index++;
- } else
+ }
+ else
{
collisionObject->setIslandTag(-1);
collisionObject->setCompanionId(-2);
@@ -122,49 +115,44 @@ void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* c
}
}
-
-#else //STATIC_SIMULATION_ISLAND_OPTIMIZATION
-void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
+#else //STATIC_SIMULATION_ISLAND_OPTIMIZATION
+void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld, btDispatcher* dispatcher)
{
+ initUnionFind(int(colWorld->getCollisionObjectArray().size()));
- initUnionFind( int (colWorld->getCollisionObjectArray().size()));
-
- // put the index into m_controllers into m_tag
+ // put the index into m_controllers into m_tag
{
-
int index = 0;
int i;
- for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
+ for (i = 0; i < colWorld->getCollisionObjectArray().size(); i++)
{
- btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
+ btCollisionObject* collisionObject = colWorld->getCollisionObjectArray()[i];
collisionObject->setIslandTag(index);
collisionObject->setCompanionId(-1);
collisionObject->setHitFraction(btScalar(1.));
index++;
-
}
}
// do the union find
- findUnions(dispatcher,colWorld);
+ findUnions(dispatcher, colWorld);
}
-void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
+void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
{
- // put the islandId ('find' value) into m_tag
+ // put the islandId ('find' value) into m_tag
{
-
-
int index = 0;
int i;
- for (i=0;i<colWorld->getCollisionObjectArray().size();i++)
+ for (i = 0; i < colWorld->getCollisionObjectArray().size(); i++)
{
- btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
+ btCollisionObject* collisionObject = colWorld->getCollisionObjectArray()[i];
if (!collisionObject->isStaticOrKinematicObject())
{
- collisionObject->setIslandTag( m_unionFind.find(index) );
+ collisionObject->setIslandTag(m_unionFind.find(index));
collisionObject->setCompanionId(-1);
- } else
+ }
+ else
{
collisionObject->setIslandTag(-1);
collisionObject->setCompanionId(-2);
@@ -174,72 +162,59 @@ void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* col
}
}
-#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION
+#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION
-inline int getIslandId(const btPersistentManifold* lhs)
+inline int getIslandId(const btPersistentManifold* lhs)
{
int islandId;
const btCollisionObject* rcolObj0 = static_cast<const btCollisionObject*>(lhs->getBody0());
const btCollisionObject* rcolObj1 = static_cast<const btCollisionObject*>(lhs->getBody1());
- islandId= rcolObj0->getIslandTag()>=0?rcolObj0->getIslandTag():rcolObj1->getIslandTag();
+ islandId = rcolObj0->getIslandTag() >= 0 ? rcolObj0->getIslandTag() : rcolObj1->getIslandTag();
return islandId;
-
}
-
-
/// function object that routes calls to operator<
class btPersistentManifoldSortPredicate
{
- public:
-
- SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs ) const
- {
- return getIslandId(lhs) < getIslandId(rhs);
- }
+public:
+ SIMD_FORCE_INLINE bool operator()(const btPersistentManifold* lhs, const btPersistentManifold* rhs) const
+ {
+ return getIslandId(lhs) < getIslandId(rhs);
+ }
};
class btPersistentManifoldSortPredicateDeterministic
{
public:
-
- SIMD_FORCE_INLINE bool operator() (const btPersistentManifold* lhs, const btPersistentManifold* rhs) const
+ SIMD_FORCE_INLINE bool operator()(const btPersistentManifold* lhs, const btPersistentManifold* rhs) const
{
return (
- (getIslandId(lhs) < getIslandId(rhs))
- || ((getIslandId(lhs) == getIslandId(rhs)) && lhs->getBody0()->getBroadphaseHandle()->m_uniqueId < rhs->getBody0()->getBroadphaseHandle()->m_uniqueId)
- ||((getIslandId(lhs) == getIslandId(rhs)) && (lhs->getBody0()->getBroadphaseHandle()->m_uniqueId == rhs->getBody0()->getBroadphaseHandle()->m_uniqueId) &&
- (lhs->getBody1()->getBroadphaseHandle()->m_uniqueId < rhs->getBody1()->getBroadphaseHandle()->m_uniqueId))
- );
-
+ (getIslandId(lhs) < getIslandId(rhs)) || ((getIslandId(lhs) == getIslandId(rhs)) && lhs->getBody0()->getBroadphaseHandle()->m_uniqueId < rhs->getBody0()->getBroadphaseHandle()->m_uniqueId) || ((getIslandId(lhs) == getIslandId(rhs)) && (lhs->getBody0()->getBroadphaseHandle()->m_uniqueId == rhs->getBody0()->getBroadphaseHandle()->m_uniqueId) && (lhs->getBody1()->getBroadphaseHandle()->m_uniqueId < rhs->getBody1()->getBroadphaseHandle()->m_uniqueId)));
}
};
-
-void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld)
+void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld)
{
-
BT_PROFILE("islandUnionFindAndQuickSort");
-
+
btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
m_islandmanifold.resize(0);
//we are going to sort the unionfind array, and store the element id in the size
//afterwards, we clean unionfind, to make sure no-one uses it anymore
-
+
getUnionFind().sortIslands();
int numElem = getUnionFind().getNumElements();
- int endIslandIndex=1;
+ int endIslandIndex = 1;
int startIslandIndex;
-
//update the sleeping state for bodies, if all are sleeping
- for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
+ for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
{
int islandId = getUnionFind().getElement(startIslandIndex).m_id;
- for (endIslandIndex = startIslandIndex+1;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
+ for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId); endIslandIndex++)
{
}
@@ -248,69 +223,68 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio
bool allSleeping = true;
int idx;
- for (idx=startIslandIndex;idx<endIslandIndex;idx++)
+ for (idx = startIslandIndex; idx < endIslandIndex; idx++)
{
int i = getUnionFind().getElement(idx).m_sz;
btCollisionObject* colObj0 = collisionObjects[i];
if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
{
-// printf("error in island management\n");
+ // printf("error in island management\n");
}
btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
if (colObj0->getIslandTag() == islandId)
{
- if (colObj0->getActivationState()== ACTIVE_TAG ||
- colObj0->getActivationState()== DISABLE_DEACTIVATION)
+ if (colObj0->getActivationState() == ACTIVE_TAG ||
+ colObj0->getActivationState() == DISABLE_DEACTIVATION)
{
allSleeping = false;
break;
}
}
}
-
if (allSleeping)
{
int idx;
- for (idx=startIslandIndex;idx<endIslandIndex;idx++)
+ for (idx = startIslandIndex; idx < endIslandIndex; idx++)
{
int i = getUnionFind().getElement(idx).m_sz;
btCollisionObject* colObj0 = collisionObjects[i];
if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
{
-// printf("error in island management\n");
+ // printf("error in island management\n");
}
btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
if (colObj0->getIslandTag() == islandId)
{
- colObj0->setActivationState( ISLAND_SLEEPING );
+ colObj0->setActivationState(ISLAND_SLEEPING);
}
}
- } else
+ }
+ else
{
-
int idx;
- for (idx=startIslandIndex;idx<endIslandIndex;idx++)
+ for (idx = startIslandIndex; idx < endIslandIndex; idx++)
{
int i = getUnionFind().getElement(idx).m_sz;
btCollisionObject* colObj0 = collisionObjects[i];
if ((colObj0->getIslandTag() != islandId) && (colObj0->getIslandTag() != -1))
{
-// printf("error in island management\n");
+ // printf("error in island management\n");
}
btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
if (colObj0->getIslandTag() == islandId)
{
- if ( colObj0->getActivationState() == ISLAND_SLEEPING)
+ if (colObj0->getActivationState() == ISLAND_SLEEPING)
{
- colObj0->setActivationState( WANTS_DEACTIVATION);
+ colObj0->setActivationState(WANTS_DEACTIVATION);
colObj0->setDeactivationTime(0.f);
}
}
@@ -318,34 +292,30 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio
}
}
-
int i;
int maxNumManifolds = dispatcher->getNumManifolds();
-//#define SPLIT_ISLANDS 1
-//#ifdef SPLIT_ISLANDS
+ //#define SPLIT_ISLANDS 1
+ //#ifdef SPLIT_ISLANDS
-
-//#endif //SPLIT_ISLANDS
+ //#endif //SPLIT_ISLANDS
-
- for (i=0;i<maxNumManifolds ;i++)
+ for (i = 0; i < maxNumManifolds; i++)
{
- btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
- if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs)
- {
+ btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
+ if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs)
+ {
if (manifold->getNumContacts() == 0)
continue;
- }
+ }
- const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0());
- const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1());
-
- ///@todo: check sleeping conditions!
- if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
+ const btCollisionObject* colObj0 = static_cast<const btCollisionObject*>(manifold->getBody0());
+ const btCollisionObject* colObj1 = static_cast<const btCollisionObject*>(manifold->getBody1());
+
+ ///@todo: check sleeping conditions!
+ if (((colObj0) && colObj0->getActivationState() != ISLAND_SLEEPING) ||
((colObj1) && colObj1->getActivationState() != ISLAND_SLEEPING))
{
-
//kinematic objects don't merge islands, but wake up all connected objects
if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
{
@@ -357,36 +327,34 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio
if (colObj1->hasContactResponse())
colObj0->activate();
}
- if(m_splitIslands)
- {
+ if (m_splitIslands)
+ {
//filtering for response
- if (dispatcher->needsResponse(colObj0,colObj1))
+ if (dispatcher->needsResponse(colObj0, colObj1))
m_islandmanifold.push_back(manifold);
}
}
}
}
-
-
///@todo: this is random access, it can be walked 'cache friendly'!
-void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback)
+void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, btCollisionWorld* collisionWorld, IslandCallback* callback)
{
btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
- buildIslands(dispatcher,collisionWorld);
+ buildIslands(dispatcher, collisionWorld);
- int endIslandIndex=1;
+ int endIslandIndex = 1;
int startIslandIndex;
int numElem = getUnionFind().getNumElements();
BT_PROFILE("processIslands");
- if(!m_splitIslands)
+ if (!m_splitIslands)
{
btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
int maxNumManifolds = dispatcher->getNumManifolds();
- callback->processIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
+ callback->processIsland(&collisionObjects[0], collisionObjects.size(), manifold, maxNumManifolds, -1);
}
else
{
@@ -394,7 +362,7 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
// Sort the vector using predicate and std::sort
//std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
- int numManifolds = int (m_islandmanifold.size());
+ int numManifolds = int(m_islandmanifold.size());
//tried a radix sort, but quicksort/heapsort seems still faster
//@todo rewrite island management
@@ -403,7 +371,8 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
if (collisionWorld->getDispatchInfo().m_deterministicOverlappingPairs)
{
m_islandmanifold.quickSort(btPersistentManifoldSortPredicateDeterministic());
- } else
+ }
+ else
{
m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
}
@@ -417,55 +386,49 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
//int islandId;
-
-
- // printf("Start Islands\n");
+ // printf("Start Islands\n");
//traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
- for ( startIslandIndex=0;startIslandIndex<numElem;startIslandIndex = endIslandIndex)
+ for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
{
int islandId = getUnionFind().getElement(startIslandIndex).m_id;
+ bool islandSleeping = true;
- bool islandSleeping = true;
-
- for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
- {
- int i = getUnionFind().getElement(endIslandIndex).m_sz;
- btCollisionObject* colObj0 = collisionObjects[i];
- m_islandBodies.push_back(colObj0);
- if (colObj0->isActive())
- islandSleeping = false;
- }
-
+ for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId); endIslandIndex++)
+ {
+ int i = getUnionFind().getElement(endIslandIndex).m_sz;
+ btCollisionObject* colObj0 = collisionObjects[i];
+ m_islandBodies.push_back(colObj0);
+ if (colObj0->isActive())
+ islandSleeping = false;
+ }
//find the accompanying contact manifold for this islandId
int numIslandManifolds = 0;
btPersistentManifold** startManifold = 0;
- if (startManifoldIndex<numManifolds)
+ if (startManifoldIndex < numManifolds)
{
int curIslandId = getIslandId(m_islandmanifold[startManifoldIndex]);
if (curIslandId == islandId)
{
startManifold = &m_islandmanifold[startManifoldIndex];
-
- for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex]));endManifoldIndex++)
- {
+ for (endManifoldIndex = startManifoldIndex + 1; (endManifoldIndex < numManifolds) && (islandId == getIslandId(m_islandmanifold[endManifoldIndex])); endManifoldIndex++)
+ {
}
/// Process the actual simulation, only if not sleeping/deactivated
- numIslandManifolds = endManifoldIndex-startManifoldIndex;
+ numIslandManifolds = endManifoldIndex - startManifoldIndex;
}
-
}
if (!islandSleeping)
{
- callback->processIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId);
- // printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
+ callback->processIsland(&m_islandBodies[0], m_islandBodies.size(), startManifold, numIslandManifolds, islandId);
+ // printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
}
-
+
if (numIslandManifolds)
{
startManifoldIndex = endManifoldIndex;
@@ -473,6 +436,5 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,
m_islandBodies.resize(0);
}
- } // else if(!splitIslands)
-
+ } // else if(!splitIslands)
}
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-17 01:03:43 +0000