/*
|
* 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;
|
|
/**
|
* SimulationIslandManager creates and handles simulation islands, using {@link UnionFind}.
|
*
|
* @author jezek2
|
*/
|
public class SimulationIslandManager implements java.io.Serializable {
|
|
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");
|
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();
|
}
|
}
|
|
////////////////////////////////////////////////////////////////////////////
|
|
public static abstract class IslandCallback implements java.io.Serializable {
|
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;
|
}
|
};
|
|
}
|
