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.cpp450
1 files changed, 450 insertions, 0 deletions
diff --git a/thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
new file mode 100644
index 0000000000..1344782257
--- /dev/null
+++ b/thirdparty/bullet/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp
@@ -0,0 +1,450 @@
+
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
+
+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.
+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:
+
+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.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+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"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
+#include "BulletCollision/CollisionDispatch/btCollisionWorld.h"
+
+//#include <stdio.h>
+#include "LinearMath/btQuickprof.h"
+
+btSimulationIslandManager::btSimulationIslandManager():
+m_splitIslands(true)
+{
+}
+
+btSimulationIslandManager::~btSimulationIslandManager()
+{
+}
+
+
+void btSimulationIslandManager::initUnionFind(int n)
+{
+ m_unionFind.reset(n);
+}
+
+
+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;
+
+ 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)
+{
+
+ // put the index into m_controllers into m_tag
+ int index = 0;
+ {
+
+ int i;
+ for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
+ {
+ btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
+ //Adding filtering here
+ if (!collisionObject->isStaticOrKinematicObject())
+ {
+ collisionObject->setIslandTag(index++);
+ }
+ collisionObject->setCompanionId(-1);
+ collisionObject->setHitFraction(btScalar(1.));
+ }
+ }
+ // do the union find
+
+ initUnionFind( index );
+
+ findUnions(dispatcher,colWorld);
+}
+
+void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
+{
+ // put the islandId ('find' value) into m_tag
+ {
+ int index = 0;
+ int i;
+ for (i=0;i<colWorld->getCollisionObjectArray().size();i++)
+ {
+ btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
+ if (!collisionObject->isStaticOrKinematicObject())
+ {
+ 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
+ {
+ collisionObject->setIslandTag(-1);
+ collisionObject->setCompanionId(-2);
+ }
+ }
+ }
+}
+
+
+#else //STATIC_SIMULATION_ISLAND_OPTIMIZATION
+void btSimulationIslandManager::updateActivationState(btCollisionWorld* colWorld,btDispatcher* dispatcher)
+{
+
+ initUnionFind( int (colWorld->getCollisionObjectArray().size()));
+
+ // put the index into m_controllers into m_tag
+ {
+
+ int index = 0;
+ int i;
+ for (i=0;i<colWorld->getCollisionObjectArray().size(); i++)
+ {
+ btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
+ collisionObject->setIslandTag(index);
+ collisionObject->setCompanionId(-1);
+ collisionObject->setHitFraction(btScalar(1.));
+ index++;
+
+ }
+ }
+ // do the union find
+
+ findUnions(dispatcher,colWorld);
+}
+
+void btSimulationIslandManager::storeIslandActivationState(btCollisionWorld* colWorld)
+{
+ // put the islandId ('find' value) into m_tag
+ {
+
+
+ int index = 0;
+ int i;
+ for (i=0;i<colWorld->getCollisionObjectArray().size();i++)
+ {
+ btCollisionObject* collisionObject= colWorld->getCollisionObjectArray()[i];
+ if (!collisionObject->isStaticOrKinematicObject())
+ {
+ collisionObject->setIslandTag( m_unionFind.find(index) );
+ collisionObject->setCompanionId(-1);
+ } else
+ {
+ collisionObject->setIslandTag(-1);
+ collisionObject->setCompanionId(-2);
+ }
+ index++;
+ }
+ }
+}
+
+#endif //STATIC_SIMULATION_ISLAND_OPTIMIZATION
+
+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();
+ 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);
+ }
+};
+
+
+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 startIslandIndex;
+
+
+ //update the sleeping state for bodies, if all are sleeping
+ 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++)
+ {
+ }
+
+ //int numSleeping = 0;
+
+ bool allSleeping = true;
+
+ int 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");
+ }
+
+ btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+ if (colObj0->getIslandTag() == islandId)
+ {
+ if (colObj0->getActivationState()== ACTIVE_TAG)
+ {
+ allSleeping = false;
+ }
+ if (colObj0->getActivationState()== DISABLE_DEACTIVATION)
+ {
+ allSleeping = false;
+ }
+ }
+ }
+
+
+ if (allSleeping)
+ {
+ int 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");
+ }
+
+ btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+
+ if (colObj0->getIslandTag() == islandId)
+ {
+ colObj0->setActivationState( ISLAND_SLEEPING );
+ }
+ }
+ } else
+ {
+
+ int 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");
+ }
+
+ btAssert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
+
+ if (colObj0->getIslandTag() == islandId)
+ {
+ if ( colObj0->getActivationState() == ISLAND_SLEEPING)
+ {
+ colObj0->setActivationState( WANTS_DEACTIVATION);
+ colObj0->setDeactivationTime(0.f);
+ }
+ }
+ }
+ }
+ }
+
+
+ int i;
+ int maxNumManifolds = dispatcher->getNumManifolds();
+
+//#define SPLIT_ISLANDS 1
+//#ifdef SPLIT_ISLANDS
+
+
+//#endif //SPLIT_ISLANDS
+
+
+ for (i=0;i<maxNumManifolds ;i++)
+ {
+ btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
+
+ 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)
+ {
+ if (colObj0->hasContactResponse())
+ colObj1->activate();
+ }
+ if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
+ {
+ if (colObj1->hasContactResponse())
+ colObj0->activate();
+ }
+ if(m_splitIslands)
+ {
+ //filtering for response
+ 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)
+{
+ btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
+
+ buildIslands(dispatcher,collisionWorld);
+
+ int endIslandIndex=1;
+ int startIslandIndex;
+ int numElem = getUnionFind().getNumElements();
+
+ BT_PROFILE("processIslands");
+
+ if(!m_splitIslands)
+ {
+ btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
+ int maxNumManifolds = dispatcher->getNumManifolds();
+ callback->processIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
+ }
+ else
+ {
+ // Sort manifolds, based on islands
+ // Sort the vector using predicate and std::sort
+ //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
+
+ int numManifolds = int (m_islandmanifold.size());
+
+ //tried a radix sort, but quicksort/heapsort seems still faster
+ //@todo rewrite island management
+ m_islandmanifold.quickSort(btPersistentManifoldSortPredicate());
+ //m_islandmanifold.heapSort(btPersistentManifoldSortPredicate());
+
+ //now process all active islands (sets of manifolds for now)
+
+ int startManifoldIndex = 0;
+ int endManifoldIndex = 1;
+
+ //int islandId;
+
+
+
+ // 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)
+ {
+ int islandId = getUnionFind().getElement(startIslandIndex).m_id;
+
+
+ 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;
+ }
+
+
+ //find the accompanying contact manifold for this islandId
+ int numIslandManifolds = 0;
+ btPersistentManifold** startManifold = 0;
+
+ 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++)
+ {
+
+ }
+ /// Process the actual simulation, only if not sleeping/deactivated
+ 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);
+ }
+
+ if (numIslandManifolds)
+ {
+ startManifoldIndex = endManifoldIndex;
+ }
+
+ m_islandBodies.resize(0);
+ }
+ } // else if(!splitIslands)
+
+}
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-18 21:15:40 +0000