// Boths bodies must be !static
private void MergeIslands(RigidBody body0, RigidBody body1)
{
if (body0.island != body1.island) // <- both bodies are in different islands
{
if (body0.island == null) // <- one island is null
{
body0.island = body1.island;
body0.island.bodies.Add(body0);
}
else if (body1.island == null) // <- one island is null
{
body1.island = body0.island;
body1.island.bodies.Add(body1);
}
else // <- both islands are different,
{
// merge smaller into larger
RigidBody smallIslandOwner, largeIslandOwner;
if (body0.island.bodies.Count > body1.island.bodies.Count)
{
smallIslandOwner = body1;
largeIslandOwner = body0;
}
else
{
smallIslandOwner = body0;
largeIslandOwner = body1;
}
CollisionIsland giveBackIsland = smallIslandOwner.island;
Pool.GiveBack(giveBackIsland);
islands.Remove(giveBackIsland);
foreach (RigidBody b in giveBackIsland.bodies)
{
b.island = largeIslandOwner.island;
largeIslandOwner.island.bodies.Add(b);
}
foreach (Arbiter a in giveBackIsland.arbiter)
{
largeIslandOwner.island.arbiter.Add(a);
}
foreach (Constraint c in giveBackIsland.constraints)
{
largeIslandOwner.island.constraints.Add(c);
}
giveBackIsland.ClearLists();
}
}
else if (body0.island == null) // <- both are null
{
CollisionIsland island = Pool.GetNew();
island.islandManager = this;
body0.island = body1.island = island;
body0.island.bodies.Add(body0);
body0.island.bodies.Add(body1);
islands.Add(island);
}
}
private void SplitIslands(RigidBody body0, RigidBody body1)
{
System.Diagnostics.Debug.Assert(body0.island != null && (body0.island == body1.island),
"Islands not the same or null.");
leftSearchQueue.Enqueue(body0);
rightSearchQueue.Enqueue(body1);
visitedBodiesLeft.Add(body0);
visitedBodiesRight.Add(body1);
body0.marker = 1;
body1.marker = 2;
while (leftSearchQueue.Count > 0 && rightSearchQueue.Count > 0)
{
RigidBody currentNode = leftSearchQueue.Dequeue();
if (!currentNode.IsStaticNonKinematic)
{
for (int i = 0; i < currentNode.connections.Count; i++)
{
RigidBody connectedNode = currentNode.connections[i];
if (connectedNode.marker == 0)
{
leftSearchQueue.Enqueue(connectedNode);
visitedBodiesLeft.Add(connectedNode);
connectedNode.marker = 1;
}
else if (connectedNode.marker == 2)
{
leftSearchQueue.Clear();
rightSearchQueue.Clear();
goto ResetSearchStates;
}
}
}
currentNode = rightSearchQueue.Dequeue();
if (!currentNode.IsStaticNonKinematic)
{
for (int i = 0; i < currentNode.connections.Count; i++)
{
RigidBody connectedNode = currentNode.connections[i];
if (connectedNode.marker == 0)
{
rightSearchQueue.Enqueue(connectedNode);
visitedBodiesRight.Add(connectedNode);
connectedNode.marker = 2;
}
else if (connectedNode.marker == 1)
{
leftSearchQueue.Clear();
rightSearchQueue.Clear();
goto ResetSearchStates;
}
}
}
}
CollisionIsland island = Pool.GetNew();
island.islandManager = this;
islands.Add(island);
if (leftSearchQueue.Count == 0)
{
for (int i = 0; i < visitedBodiesLeft.Count; i++)
{
RigidBody body = visitedBodiesLeft[i];
body1.island.bodies.Remove(body);
island.bodies.Add(body);
body.island = island;
foreach (Arbiter a in body.arbiters)
{
body1.island.arbiter.Remove(a);
island.arbiter.Add(a);
}
foreach (Constraint c in body.constraints)
{
body1.island.constraints.Remove(c);
island.constraints.Add(c);
}
}
rightSearchQueue.Clear();
}
else if (rightSearchQueue.Count == 0)
{
for (int i = 0; i < visitedBodiesRight.Count; i++)
{
RigidBody body = visitedBodiesRight[i];
body0.island.bodies.Remove(body);
island.bodies.Add(body);
body.island = island;
foreach (Arbiter a in body.arbiters)
{
body0.island.arbiter.Remove(a);
island.arbiter.Add(a);
}
foreach (Constraint c in body.constraints)
{
body0.island.constraints.Remove(c);
island.constraints.Add(c);
}
}
leftSearchQueue.Clear();
}
ResetSearchStates:
for (int i = 0; i < visitedBodiesLeft.Count; i++)
{
visitedBodiesLeft[i].marker = 0;
}
for (int i = 0; i < visitedBodiesRight.Count; i++)
{
visitedBodiesRight[i].marker = 0;
}
visitedBodiesLeft.Clear();
visitedBodiesRight.Clear();
}
