public virtual void ReleaseManifold(PersistentManifold manifold)
{
_manifoldCount--;
ClearManifold(manifold);
_manifolds.Remove(manifold);
}
public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (_manifold == null)
{
//swapped?
_manifold = Dispatcher.GetNewManifold(bodyA, bodyB);
_ownManifold = true;
}
resultOut.SetPersistentManifold(_manifold);
ConvexShape min0 = bodyA.CollisionShape as ConvexShape;
ConvexShape min1 = bodyB.CollisionShape as ConvexShape;
GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
//TODO: if (dispatchInfo.m_useContinuous)
_gjkPairDetector.setMinkowskiA(min0);
_gjkPairDetector.setMinkowskiB(min1);
input.MaximumDistanceSquared = min0.Margin + min1.Margin + PersistentManifold.ContactBreakingThreshold;
input.MaximumDistanceSquared *= input.MaximumDistanceSquared;
// input.m_maximumDistanceSquared = 1e30f;
input.TransformA = bodyA.WorldTransform;
input.TransformB = bodyB.WorldTransform;
_gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.DebugDraw);
}
public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (_manifold == null)
{
//swapped?
_manifold = Dispatcher.GetNewManifold(bodyA, bodyB);
_ownManifold = true;
}
resultOut.SetPersistentManifold(_manifold);
ConvexShape min0 = bodyA.CollisionShape as ConvexShape;
ConvexShape min1 = bodyB.CollisionShape as ConvexShape;
GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
//TODO: if (dispatchInfo.m_useContinuous)
_gjkPairDetector.setMinkowskiA(min0);
_gjkPairDetector.setMinkowskiB(min1);
input.MaximumDistanceSquared = min0.Margin + min1.Margin + PersistentManifold.ContactBreakingThreshold;
input.MaximumDistanceSquared *= input.MaximumDistanceSquared;
// input.m_maximumDistanceSquared = 1e30f;
input.TransformA = bodyA.WorldTransform;
input.TransformB = bodyB.WorldTransform;
_gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.DebugDraw);
}
public ConvexConvexAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
: base(collisionAlgorithmConstructionInfo)
{
_gjkPairDetector = new GjkPairDetector(null, null, simplexSolver, penetrationDepthSolver);
_ownManifold = false;
_manifold = manifold;
_lowLevelOfDetail = false;
}
public ConvexConvexAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
: base(collisionAlgorithmConstructionInfo)
{
_gjkPairDetector = new GjkPairDetector(null, null, simplexSolver, penetrationDepthSolver);
_ownManifold = false;
_manifold = manifold;
_lowLevelOfDetail = false;
}
private static int GetIslandId(PersistentManifold lhs)
{
int islandId;
CollisionObject rcolObjA = lhs.BodyA as CollisionObject;
CollisionObject rcolObjB = lhs.BodyB as CollisionObject;
islandId = rcolObjA.IslandTag >= 0 ? rcolObjA.IslandTag : rcolObjB.IslandTag;
return(islandId);
}
public ConvexTriangleCallback(IDispatcher dispatcher, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped)
{
_dispatcher = dispatcher;
_dispatchInfo = null;
_convexBody = isSwapped ? bodyB : bodyA;
_triBody = isSwapped ? bodyA : bodyB;
// create the manifold from the dispatcher 'manifold pool'
_manifold = _dispatcher.GetNewManifold(_convexBody, _triBody);
ClearCache();
}
public ConvexTriangleCallback(IDispatcher dispatcher, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped)
{
_dispatcher = dispatcher;
_dispatchInfo = null;
_convexBody = isSwapped ? bodyB : bodyA;
_triBody = isSwapped ? bodyA : bodyB;
// create the manifold from the dispatcher 'manifold pool'
_manifold = _dispatcher.GetNewManifold(_convexBody, _triBody);
ClearCache();
}
public SphereSphereCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
: base(collisionAlgorithmConstructionInfo)
{
_ownManifold = false;
_manifold = manifold;
if (_manifold == null)
{
_manifold = Dispatcher.GetNewManifold(bodyA, bodyB);
_ownManifold = true;
}
}
public SphereSphereCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
: base(collisionAlgorithmConstructionInfo)
{
_ownManifold = false;
_manifold = manifold;
if (_manifold == null)
{
_manifold = Dispatcher.GetNewManifold(bodyA, bodyB);
_ownManifold = true;
}
}
public virtual PersistentManifold GetNewManifold(object bodyA, object bodyB)
{
_manifoldCount++;
CollisionObject body0 = bodyA as CollisionObject;
CollisionObject body1 = bodyB as CollisionObject;
PersistentManifold manifold = new PersistentManifold(body0, body1);
_manifolds.Add(manifold);
return(manifold);
}
private static int PersistentManifoldSortPredicate(PersistentManifold lhs, PersistentManifold rhs)
{
int rIslandIdA, lIslandIdB;
rIslandIdA = GetIslandId(rhs);
lIslandIdB = GetIslandId(lhs);
//return lIslandId0 < rIslandId0;
if (lIslandIdB < rIslandIdA)
{
return(-1);
}
//else if (lIslandIdB > rIslandIdA)
// return 1;
return(1);
}
public SphereBoxCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject collisionObjectA, CollisionObject collisionObjectB, bool isSwapped)
: base(collisionAlgorithmConstructionInfo)
{
_ownManifold = false;
_manifold = manifold;
_isSwapped = isSwapped;
CollisionObject sphereObject = _isSwapped ? collisionObjectB : collisionObjectA;
CollisionObject boxObject = _isSwapped ? collisionObjectA : collisionObjectB;
if (_manifold == null && Dispatcher.NeedsCollision(sphereObject, boxObject))
{
_manifold = Dispatcher.GetNewManifold(sphereObject, boxObject);
_ownManifold = true;
}
}
public SphereBoxCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject collisionObjectA, CollisionObject collisionObjectB, bool isSwapped)
: base(collisionAlgorithmConstructionInfo)
{
_ownManifold = false;
_manifold = manifold;
_isSwapped = isSwapped;
CollisionObject sphereObject = _isSwapped ? collisionObjectB : collisionObjectA;
CollisionObject boxObject = _isSwapped ? collisionObjectA : collisionObjectB;
if (_manifold == null && Dispatcher.NeedsCollision(sphereObject, boxObject))
{
_manifold = Dispatcher.GetNewManifold(sphereObject, boxObject);
_ownManifold = true;
}
}
public void FindUnions(IDispatcher dispatcher)
{
for (int i = 0; i < dispatcher.ManifoldCount; i++)
{
PersistentManifold manifold = dispatcher.GetManifoldByIndex(i);
//static objects (invmass 0.f) don't merge !
CollisionObject colObjA = manifold.BodyA as CollisionObject;
CollisionObject colObjB = manifold.BodyB as CollisionObject;
if (((colObjA != null) && (colObjA.MergesSimulationIslands)) &&
((colObjB != null) && (colObjB.MergesSimulationIslands)))
{
_unionFind.Unite(colObjA.IslandTag, colObjB.IslandTag);
}
}
}
public void BuildAndProcessIslands(IDispatcher dispatcher, List <CollisionObject> collisionObjects, IIslandCallback callback)
{
//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
UnionFind.SortIslands();
int numElem = UnionFind.ElementCount;
int endIslandIndex = 1;
int startIslandIndex;
//update the sleeping state for bodies, if all are sleeping
for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
{
int islandId = UnionFind[startIslandIndex].ID;
for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (UnionFind[endIslandIndex].ID == islandId); endIslandIndex++)
{
}
//int numSleeping = 0;
bool allSleeping = true;
int idx;
for (idx = startIslandIndex; idx < endIslandIndex; idx++)
{
int i = UnionFind[idx].Size;
CollisionObject colObjA = collisionObjects[i];
if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1))
{
Console.WriteLine("error in island management");
}
BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1));
if (colObjA.IslandTag == islandId)
{
if (colObjA.ActivationState == ActivationState.Active)
{
allSleeping = false;
}
if (colObjA.ActivationState == ActivationState.DisableDeactivation)
{
allSleeping = false;
}
}
}
if (allSleeping)
{
for (idx = startIslandIndex; idx < endIslandIndex; idx++)
{
int i = UnionFind[idx].Size;
CollisionObject colObjA = collisionObjects[i];
if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1))
{
Console.WriteLine("error in island management");
}
BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1));
if (colObjA.IslandTag == islandId)
{
colObjA.ActivationState = ActivationState.IslandSleeping;
}
}
}
else
{
for (idx = startIslandIndex; idx < endIslandIndex; idx++)
{
int i = UnionFind[idx].Size;
CollisionObject colObjA = collisionObjects[i];
if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1))
{
Console.WriteLine("error in island management");
}
BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1));
if (colObjA.IslandTag == islandId)
{
if (colObjA.ActivationState == ActivationState.IslandSleeping)
{
colObjA.ActivationState = ActivationState.WantsDeactivation;
}
}
}
}
}
//int maxNumManifolds = dispatcher.ManifoldCount;
List <PersistentManifold> islandmanifold = new List <PersistentManifold>(dispatcher.ManifoldCount);
for (int i = 0; i < dispatcher.ManifoldCount; i++)
{
PersistentManifold manifold = dispatcher.GetManifoldByIndex(i);
CollisionObject colObjA = manifold.BodyA as CollisionObject;
CollisionObject colObjB = manifold.BodyB as CollisionObject;
//todo: check sleeping conditions!
if (((colObjA != null) && colObjA.ActivationState != ActivationState.IslandSleeping) ||
((colObjB != null) && colObjB.ActivationState != ActivationState.IslandSleeping))
{
//kinematic objects don't merge islands, but wake up all connected objects
if (colObjA.IsStaticOrKinematicObject && colObjA.ActivationState != ActivationState.IslandSleeping)
{
colObjB.Activate();
}
if (colObjB.IsStaticOrKinematicObject && colObjB.ActivationState != ActivationState.IslandSleeping)
{
colObjA.Activate();
}
//filtering for response
if (dispatcher.NeedsResponse(colObjA, colObjB))
{
islandmanifold.Add(manifold);
}
}
}
int numManifolds = islandmanifold.Count;
// Sort manifolds, based on islands
// Sort the vector using predicate and std::sort
islandmanifold.Sort(new Comparison <PersistentManifold>(PersistentManifoldSortPredicate));
//now process all active islands (sets of manifolds for now)
int startManifoldIndex = 0;
int endManifoldIndex = 1;
List <CollisionObject> islandBodies = new List <CollisionObject>();
for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
{
int islandId = UnionFind[startIslandIndex].ID;
bool islandSleeping = false;
for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (UnionFind[endIslandIndex].ID == islandId); endIslandIndex++)
{
int i = UnionFind[endIslandIndex].Size;
CollisionObject colObjA = collisionObjects[i];
islandBodies.Add(colObjA);
if (!colObjA.IsActive)
{
islandSleeping = true;
}
}
//find the accompanying contact manifold for this islandId
int numIslandManifolds = 0;
List <PersistentManifold> startManifold = new List <PersistentManifold>(numIslandManifolds);
if (startManifoldIndex < numManifolds)
{
int curIslandID = GetIslandId(islandmanifold[startManifoldIndex]);
if (curIslandID == islandId)
{
for (int k = startManifoldIndex; k < islandmanifold.Count; k++)
{
startManifold.Add(islandmanifold[k]);
}
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, startManifold, numIslandManifolds, islandId);
}
if (numIslandManifolds != 0)
{
startManifoldIndex = endManifoldIndex;
}
islandBodies.Clear();
}
}
public CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB, PersistentManifold sharedManifold)
{
CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo = new CollisionAlgorithmConstructionInfo();
collisionAlgorithmConstructionInfo.Dispatcher = this;
collisionAlgorithmConstructionInfo.Manifold = sharedManifold;
CollisionAlgorithm collisionAlgorithm = _doubleDispatch[(int)bodyA.CollisionShape.ShapeType, (int)bodyB.CollisionShape.ShapeType].CreateCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB);
return(collisionAlgorithm);
}
public CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB, PersistentManifold sharedManifold)
{
CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo = new CollisionAlgorithmConstructionInfo();
collisionAlgorithmConstructionInfo.Dispatcher = this;
collisionAlgorithmConstructionInfo.Manifold = sharedManifold;
CollisionAlgorithm collisionAlgorithm = _doubleDispatch[(int)bodyA.CollisionShape.ShapeType, (int)bodyB.CollisionShape.ShapeType].CreateCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB);
return collisionAlgorithm;
}
private static int PersistentManifoldSortPredicate(PersistentManifold lhs, PersistentManifold rhs)
{
int rIslandIdA, lIslandIdB;
rIslandIdA = GetIslandId(rhs);
lIslandIdB = GetIslandId(lhs);
//return lIslandId0 < rIslandId0;
if (lIslandIdB < rIslandIdA)
return -1;
//else if (lIslandIdB > rIslandIdA)
// return 1;
return 1;
}
private static int GetIslandId(PersistentManifold lhs)
{
int islandId;
CollisionObject rcolObjA = lhs.BodyA as CollisionObject;
CollisionObject rcolObjB = lhs.BodyB as CollisionObject;
islandId = rcolObjA.IslandTag >= 0 ? rcolObjA.IslandTag : rcolObjB.IslandTag;
return islandId;
}
public CollisionAlgorithmConstructionInfo(IDispatcher dispatcher)
{
_dispatcher = dispatcher;
_manifold = null;
}
public void SetPersistentManifold(PersistentManifold manifold)
{
_manifold = manifold;
}
public CollisionAlgorithmConstructionInfo(IDispatcher dispatcher)
{
_dispatcher = dispatcher;
_manifold = null;
}
public virtual void ReleaseManifold(PersistentManifold manifold)
{
_manifoldCount--;
ClearManifold(manifold);
_manifolds.Remove(manifold);
}
public virtual void ClearManifold(PersistentManifold manifold)
{
manifold.ClearManifold();
}
public void SetPersistentManifold(PersistentManifold manifold)
{
_manifold = manifold;
}
public virtual PersistentManifold GetNewManifold(object bodyA, object bodyB)
{
_manifoldCount++;
CollisionObject body0 = bodyA as CollisionObject;
CollisionObject body1 = bodyB as CollisionObject;
PersistentManifold manifold = new PersistentManifold(body0, body1);
_manifolds.Add(manifold);
return manifold;
}
public virtual void ClearManifold(PersistentManifold manifold)
{
manifold.ClearManifold();
}
