void btSimulationIslandManager::addBodiesToIslands( btCollisionWorld* collisionWorld )
{
btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
int endIslandIndex = 1;
int startIslandIndex;
int numElem = getUnionFind().getNumElements();
// create explicit islands and add bodies to each
for ( startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex )
{
int islandId = getUnionFind().getElement( startIslandIndex ).m_id;
// find end index
for ( endIslandIndex = startIslandIndex; ( endIslandIndex < numElem ) && ( getUnionFind().getElement( endIslandIndex ).m_id == islandId ); endIslandIndex++ )
{
}
// check if island is sleeping
bool islandSleeping = true;
for ( int iElem = startIslandIndex; iElem < endIslandIndex; iElem++ )
{
int i = getUnionFind().getElement( iElem ).m_sz;
btCollisionObject* colObj = collisionObjects[ i ];
if ( colObj->isActive() )
{
islandSleeping = false;
}
}
if ( !islandSleeping )
{
// want to count the number of bodies before allocating the island to optimize memory usage of the Island structures
int numBodies = endIslandIndex - startIslandIndex;
Island* island = allocateIsland( islandId, numBodies );
island->isSleeping = false;
// add bodies to island
for ( int iElem = startIslandIndex; iElem < endIslandIndex; iElem++ )
{
int i = getUnionFind().getElement( iElem ).m_sz;
btCollisionObject* colObj = collisionObjects[ i ];
island->bodyArray.push_back( colObj );
}
}
}
}
void btSimulationIslandManager::initIslandPools()
{
// reset island pools
int numElem = getUnionFind().getNumElements();
m_lookupIslandFromId.resize( numElem );
for ( int i = 0; i < m_lookupIslandFromId.size(); ++i )
{
m_lookupIslandFromId[ i ] = NULL;
}
m_activeIslands.resize( 0 );
m_freeIslands.resize( 0 );
// check whether allocated islands are sorted by body capacity (largest to smallest)
int lastCapacity = 0;
bool isSorted = true;
for ( int i = 0; i < m_allocatedIslands.size(); ++i )
{
Island* island = m_allocatedIslands[ i ];
int cap = island->bodyArray.capacity();
if ( cap > lastCapacity )
{
isSorted = false;
break;
}
lastCapacity = cap;
}
if ( !isSorted )
{
m_allocatedIslands.quickSort( IslandBodyCapacitySortPredicate() );
}
m_batchIsland = NULL;
// mark all islands free (but avoid deallocation)
for ( int i = 0; i < m_allocatedIslands.size(); ++i )
{
Island* island = m_allocatedIslands[ i ];
island->bodyArray.resize( 0 );
island->manifoldArray.resize( 0 );
island->constraintArray.resize( 0 );
island->id = -1;
island->isSleeping = true;
m_freeIslands.push_back( island );
}
}
///@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());
//we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
m_islandmanifold.quickSort(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 = 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 = true;
}
//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)
}
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);
btCollisionObject* colObj0 = static_cast<btCollisionObject*>(manifold->getBody0());
btCollisionObject* colObj1 = static_cast<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)
{
colObj1->activate();
}
if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
{
colObj0->activate();
}
if(m_splitIslands)
{
//filtering for response
if (dispatcher->needsResponse(colObj0,colObj1))
m_islandmanifold.push_back(manifold);
}
}
}
}
void btSimulationIslandManager::buildIslands( btDispatcher* dispatcher, btCollisionWorld* collisionWorld )
{
BT_PROFILE("islandUnionFindAndQuickSort");
btCollisionObjectArray& collisionObjects = collisionWorld->getCollisionObjectArray();
//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);
}
}
}
}
}
}
//
// todo: this is random access, it can be walked 'cache friendly'!
//
void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher,btCollisionObjectArray& collisionObjects, IslandCallback* callback)
{
/*if (0)
{
int maxNumManifolds = dispatcher->getNumManifolds();
btCollisionDispatcher* colDis = (btCollisionDispatcher*)dispatcher;
btPersistentManifold** manifold = colDis->getInternalManifoldPointer();
callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, 0);
return;
}
*/
BEGIN_PROFILE("islandUnionFindAndHeapSort");
//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");
}
assert((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");
}
assert((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");
}
assert((colObj0->getIslandTag() == islandId) || (colObj0->getIslandTag() == -1));
if (colObj0->getIslandTag() == islandId)
{
if ( colObj0->getActivationState() == ISLAND_SLEEPING)
{
colObj0->setActivationState( WANTS_DEACTIVATION);
}
}
}
}
}
btAlignedObjectArray<btPersistentManifold*> islandmanifold;
int i;
int maxNumManifolds = dispatcher->getNumManifolds();
islandmanifold.reserve(maxNumManifolds);
for (i=0;i<maxNumManifolds ;i++)
{
btPersistentManifold* manifold = dispatcher->getManifoldByIndexInternal(i);
btCollisionObject* colObj0 = static_cast<btCollisionObject*>(manifold->getBody0());
btCollisionObject* colObj1 = static_cast<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->isStaticOrKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING)
{
colObj1->activate();
}
if (colObj1->isStaticOrKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING)
{
colObj0->activate();
}
//filtering for response
if (dispatcher->needsResponse(colObj0,colObj1))
islandmanifold.push_back(manifold);
}
}
int numManifolds = int (islandmanifold.size());
// Sort manifolds, based on islands
// Sort the vector using predicate and std::sort
//std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
//we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
islandmanifold.heapSort(btPersistentManifoldSortPredicate());
//now process all active islands (sets of manifolds for now)
int startManifoldIndex = 0;
int endManifoldIndex = 1;
//int islandId;
END_PROFILE("islandUnionFindAndHeapSort");
btAlignedObjectArray<btCollisionObject*> islandBodies;
//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 = false;
for (endIslandIndex = startIslandIndex;(endIslandIndex<numElem) && (getUnionFind().getElement(endIslandIndex).m_id == islandId);endIslandIndex++)
{
int i = getUnionFind().getElement(endIslandIndex).m_sz;
btCollisionObject* colObj0 = collisionObjects[i];
islandBodies.push_back(colObj0);
if (!colObj0->isActive())
islandSleeping = true;
}
//find the accompanying contact manifold for this islandId
int numIslandManifolds = 0;
btPersistentManifold** startManifold = 0;
if (startManifoldIndex<numManifolds)
{
int curIslandId = getIslandId(islandmanifold[startManifoldIndex]);
if (curIslandId == islandId)
{
startManifold = &islandmanifold[startManifoldIndex];
for (endManifoldIndex = startManifoldIndex+1;(endManifoldIndex<numManifolds) && (islandId == getIslandId(islandmanifold[endManifoldIndex]));endManifoldIndex++)
{
}
/// Process the actual simulation, only if not sleeping/deactivated
numIslandManifolds = endManifoldIndex-startManifoldIndex;
}
}
if (!islandSleeping)
{
callback->ProcessIsland(&islandBodies[0],islandBodies.size(),startManifold,numIslandManifolds, islandId);
}
if (numIslandManifolds)
{
startManifoldIndex = endManifoldIndex;
}
islandBodies.resize(0);
}
}
