Esempio n. 1
0
        // 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);
            }
        }
Esempio n. 2
0
        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();
        }