/*
* Java port of Bullet (c) 2008 Martin Dvorak <jezek2@advel.cz>
*
* Bullet Continuous Collision Detection and Physics Library
* Copyright (c) 2003-2008 Erwin Coumans http://www.bulletphysics.com/
*
* 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.
*/
package com.bulletphysics.collision.dispatch;
import com.bulletphysics.BulletStats;
import java.util.Comparator;
import com.bulletphysics.collision.broadphase.BroadphasePair;
import com.bulletphysics.collision.broadphase.Dispatcher;
import com.bulletphysics.collision.narrowphase.PersistentManifold;
import com.bulletphysics.linearmath.MiscUtil;
import com.bulletphysics.util.ObjectArrayList;
import com.bulletphysics.util.Stack;
/**
* SimulationIslandManager creates and handles simulation islands, using {@link UnionFind}.
*
* @author jezek2
*/
public class SimulationIslandManager {
private final UnionFind unionFind = new UnionFind();
private final ObjectArrayList<PersistentManifold> islandmanifold = new ObjectArrayList<PersistentManifold>();
private final ObjectArrayList<CollisionObject> islandBodies = new ObjectArrayList<CollisionObject>();
public void initUnionFind(int n) {
unionFind.reset(n);
}
public UnionFind getUnionFind() {
return unionFind;
}
public void findUnions(Dispatcher dispatcher, CollisionWorld colWorld) {
ObjectArrayList<BroadphasePair> pairPtr = colWorld.getPairCache().getOverlappingPairArray();
for (int i=0; i<pairPtr.size(); i++) {
BroadphasePair collisionPair = pairPtr.getQuick(i);
CollisionObject colObj0 = (CollisionObject) collisionPair.pProxy0.clientObject;
CollisionObject colObj1 = (CollisionObject) collisionPair.pProxy1.clientObject;
if (((colObj0 != null) && ((colObj0).mergesSimulationIslands())) &&
((colObj1 != null) && ((colObj1).mergesSimulationIslands()))) {
unionFind.unite((colObj0).getIslandTag(), (colObj1).getIslandTag());
}
}
}
public void updateActivationState(CollisionWorld colWorld, Dispatcher dispatcher) {
initUnionFind(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++) {
CollisionObject collisionObject = colWorld.getCollisionObjectArray().getQuick(i);
collisionObject.setIslandTag(index);
collisionObject.setCompanionId(-1);
collisionObject.setHitFraction(1f);
index++;
}
}
// do the union find
findUnions(dispatcher, colWorld);
}
public void storeIslandActivationState(CollisionWorld colWorld) {
// put the islandId ('find' value) into m_tag
{
int index = 0;
int i;
for (i = 0; i < colWorld.getCollisionObjectArray().size(); i++) {
CollisionObject collisionObject = colWorld.getCollisionObjectArray().getQuick(i);
if (!collisionObject.isStaticOrKinematicObject()) {
collisionObject.setIslandTag(unionFind.find(index));
collisionObject.setCompanionId(-1);
}
else {
collisionObject.setIslandTag(-1);
collisionObject.setCompanionId(-2);
}
index++;
}
}
}
private static int getIslandId(PersistentManifold lhs) {
int islandId;
CollisionObject rcolObj0 = (CollisionObject) lhs.getBody0();
CollisionObject rcolObj1 = (CollisionObject) lhs.getBody1();
islandId = rcolObj0.getIslandTag() >= 0? rcolObj0.getIslandTag() : rcolObj1.getIslandTag();
return islandId;
}
public void buildIslands(Dispatcher dispatcher, ObjectArrayList<CollisionObject> collisionObjects) {
BulletStats.pushProfile("islandUnionFindAndQuickSort");
try {
islandmanifold.clear();
// 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).id;
for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).id == islandId); endIslandIndex++) {
}
//int numSleeping = 0;
boolean allSleeping = true;
int idx;
for (idx = startIslandIndex; idx < endIslandIndex; idx++) {
int i = getUnionFind().getElement(idx).sz;
CollisionObject colObj0 = collisionObjects.getQuick(i);
if ((colObj0.getIslandTag() != islandId) && (colObj0.getIslandTag() != -1)) {
//System.err.println("error in island management\n");
}
assert ((colObj0.getIslandTag() == islandId) || (colObj0.getIslandTag() == -1));
if (colObj0.getIslandTag() == islandId) {
if (colObj0.getActivationState() == CollisionObject.ACTIVE_TAG) {
allSleeping = false;
}
if (colObj0.getActivationState() == CollisionObject.DISABLE_DEACTIVATION) {
allSleeping = false;
}
}
}
if (allSleeping) {
//int idx;
for (idx = startIslandIndex; idx < endIslandIndex; idx++) {
int i = getUnionFind().getElement(idx).sz;
CollisionObject colObj0 = collisionObjects.getQuick(i);
if ((colObj0.getIslandTag() != islandId) && (colObj0.getIslandTag() != -1)) {
//System.err.println("error in island management\n");
}
assert ((colObj0.getIslandTag() == islandId) || (colObj0.getIslandTag() == -1));
if (colObj0.getIslandTag() == islandId) {
colObj0.setActivationState(CollisionObject.ISLAND_SLEEPING);
}
}
}
else {
//int idx;
for (idx = startIslandIndex; idx < endIslandIndex; idx++) {
int i = getUnionFind().getElement(idx).sz;
CollisionObject colObj0 = collisionObjects.getQuick(i);
if ((colObj0.getIslandTag() != islandId) && (colObj0.getIslandTag() != -1)) {
//System.err.println("error in island management\n");
}
assert ((colObj0.getIslandTag() == islandId) || (colObj0.getIslandTag() == -1));
if (colObj0.getIslandTag() == islandId) {
if (colObj0.getActivationState() == CollisionObject.ISLAND_SLEEPING) {
colObj0.setActivationState(CollisionObject.WANTS_DEACTIVATION);
}
}
}
}
}
int i;
int maxNumManifolds = dispatcher.getNumManifolds();
//#define SPLIT_ISLANDS 1
//#ifdef SPLIT_ISLANDS
//#endif //SPLIT_ISLANDS
for (i = 0; i < maxNumManifolds; i++) {
PersistentManifold manifold = dispatcher.getManifoldByIndexInternal(i);
CollisionObject colObj0 = (CollisionObject) manifold.getBody0();
CollisionObject colObj1 = (CollisionObject) manifold.getBody1();
// todo: check sleeping conditions!
if (((colObj0 != null) && colObj0.getActivationState() != CollisionObject.ISLAND_SLEEPING) ||
((colObj1 != null) && colObj1.getActivationState() != CollisionObject.ISLAND_SLEEPING)) {
// kinematic objects don't merge islands, but wake up all connected objects
if (colObj0.isKinematicObject() && colObj0.getActivationState() != CollisionObject.ISLAND_SLEEPING) {
colObj1.activate();
}
if (colObj1.isKinematicObject() && colObj1.getActivationState() != CollisionObject.ISLAND_SLEEPING) {
colObj0.activate();
}
//#ifdef SPLIT_ISLANDS
//filtering for response
if (dispatcher.needsResponse(colObj0, colObj1)) {
islandmanifold.add(manifold);
}
//#endif //SPLIT_ISLANDS
}
}
}
finally {
BulletStats.popProfile();
}
}
public void buildAndProcessIslands(Dispatcher dispatcher, ObjectArrayList<CollisionObject> collisionObjects, IslandCallback callback) {
buildIslands(dispatcher, collisionObjects);
int endIslandIndex = 1;
int startIslandIndex;
int numElem = getUnionFind().getNumElements();
BulletStats.pushProfile("processIslands");
Stack stack = Stack.enter();
int sp = stack.getSp();
try {
//#ifndef SPLIT_ISLANDS
//btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
//
//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 = islandmanifold.size();
// we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
//islandmanifold.heapSort(btPersistentManifoldSortPredicate());
// JAVA NOTE: memory optimized sorting with caching of temporary array
//Collections.sort(islandmanifold, persistentManifoldComparator);
MiscUtil.quickSort(islandmanifold, persistentManifoldComparator);
// 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).id;
boolean islandSleeping = false;
for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).id == islandId); endIslandIndex++) {
int i = getUnionFind().getElement(endIslandIndex).sz;
CollisionObject colObj0 = collisionObjects.getQuick(i);
islandBodies.add(colObj0);
if (!colObj0.isActive()) {
islandSleeping = true;
}
}
// find the accompanying contact manifold for this islandId
int numIslandManifolds = 0;
//ObjectArrayList<PersistentManifold> startManifold = null;
int startManifold_idx = -1;
if (startManifoldIndex < numManifolds) {
int curIslandId = getIslandId(islandmanifold.getQuick(startManifoldIndex));
if (curIslandId == islandId) {
//startManifold = &m_islandmanifold[startManifoldIndex];
//startManifold = islandmanifold.subList(startManifoldIndex, islandmanifold.size());
startManifold_idx = startManifoldIndex;
for (endManifoldIndex = startManifoldIndex + 1; (endManifoldIndex < numManifolds) && (islandId == getIslandId(islandmanifold.getQuick(endManifoldIndex))); endManifoldIndex++) {
}
// Process the actual simulation, only if not sleeping/deactivated
numIslandManifolds = endManifoldIndex - startManifoldIndex;
}
}
if (!islandSleeping) {
callback.processIsland(islandBodies, islandBodies.size(), islandmanifold, startManifold_idx, numIslandManifolds, islandId);
//printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
}
if (numIslandManifolds != 0) {
startManifoldIndex = endManifoldIndex;
}
islandBodies.clear();
}
//#endif //SPLIT_ISLANDS
}
finally {
BulletStats.popProfile();
stack.leave(sp);
}
}
////////////////////////////////////////////////////////////////////////////
public static abstract class IslandCallback {
public abstract void processIsland(ObjectArrayList<CollisionObject> bodies, int numBodies, ObjectArrayList<PersistentManifold> manifolds, int manifolds_offset, int numManifolds, int islandId);
}
private static final Comparator<PersistentManifold> persistentManifoldComparator = new Comparator<PersistentManifold>() {
public int compare(PersistentManifold lhs, PersistentManifold rhs) {
return getIslandId(lhs) < getIslandId(rhs)? -1 : +1;
}
};
}