BEPUphysics.DeactivationManagement.DeactivationManager.TryToSplit C# (CSharp) Method

TryToSplit() private method

Tries to split connections between the two island members.
private TryToSplit ( SimulationIslandMember member1, SimulationIslandMember member2 ) : bool
member1 SimulationIslandMember First island member.
member2 SimulationIslandMember Second island member.
return bool
        private bool TryToSplit(SimulationIslandMember member1, SimulationIslandMember member2)
        {
            //Can't split if they aren't even in the same island.
            //This also covers the case where the connection involves a kinematic entity that has no 
            //simulation island at all.
            if (member1.SimulationIsland != member2.SimulationIsland ||
                member1.SimulationIsland == null ||
                member2.SimulationIsland == null)
                return false;


            //By now, we know the members belong to the same island and are not null.
            //Start a BFS starting from each member.
            //Two-way can complete the search quicker.

            member1Friends.Enqueue(member1);
            member2Friends.Enqueue(member2);
            searchedMembers1.Add(member1);
            searchedMembers2.Add(member2);
            member1.searchState = SimulationIslandSearchState.OwnedByFirst;
            member2.searchState = SimulationIslandSearchState.OwnedBySecond;

            while (member1Friends.Count > 0 && member2Friends.Count > 0)
            {


                SimulationIslandMember currentNode = member1Friends.Dequeue();
                for (int i = 0; i < currentNode.connections.Count; i++)
                {
                    for (int j = 0; j < currentNode.connections.Elements[i].entries.Count; j++)
                    {
                        SimulationIslandMember connectedNode;
                        if ((connectedNode = currentNode.connections.Elements[i].entries.Elements[j].Member) != currentNode &&
                            connectedNode.SimulationIsland != null) //The connection could be connected to something that isn't in the Space and has no island, or it's not dynamic.
                        {
                            switch (connectedNode.searchState)
                            {
                                case SimulationIslandSearchState.Unclaimed:
                                    //Found a new friend :)
                                    member1Friends.Enqueue(connectedNode);
                                    connectedNode.searchState = SimulationIslandSearchState.OwnedByFirst;
                                    searchedMembers1.Add(connectedNode);
                                    break;
                                case SimulationIslandSearchState.OwnedBySecond:
                                    //Found our way to member2Friends set; cannot split!
                                    member1Friends.Clear();
                                    member2Friends.Clear();
                                    goto ResetSearchStates;
                            }

                        }
                    }
                }

                currentNode = member2Friends.Dequeue();
                for (int i = 0; i < currentNode.connections.Count; i++)
                {
                    for (int j = 0; j < currentNode.connections.Elements[i].entries.Count; j++)
                    {
                        SimulationIslandMember connectedNode;
                        if ((connectedNode = currentNode.connections.Elements[i].entries.Elements[j].Member) != currentNode &&
                            connectedNode.SimulationIsland != null) //The connection could be connected to something that isn't in the Space and has no island, or it's not dynamic.
                        {
                            switch (connectedNode.searchState)
                            {
                                case SimulationIslandSearchState.Unclaimed:
                                    //Found a new friend :)
                                    member2Friends.Enqueue(connectedNode);
                                    connectedNode.searchState = SimulationIslandSearchState.OwnedBySecond;
                                    searchedMembers2.Add(connectedNode);
                                    break;
                                case SimulationIslandSearchState.OwnedByFirst:
                                    //Found our way to member1Friends set; cannot split!
                                    member1Friends.Clear();
                                    member2Friends.Clear();
                                    goto ResetSearchStates;
                            }

                        }
                    }
                }
            }
            //If one of the queues empties out without finding anything, it means it's isolated.  The other one will never find it.
            //Now we can do a split.  Grab a new Island, fill it with the isolated search stuff.  Remove the isolated search stuff from the old Island.


            SimulationIsland newIsland = islandPool.Take();
            simulationIslands.Add(newIsland);
            if (member1Friends.Count == 0)
            {

                //Member 1 is isolated, give it its own simulation island!
                for (int i = 0; i < searchedMembers1.Count; i++)
                {
                    searchedMembers1[i].simulationIsland.Remove(searchedMembers1[i]);
                    newIsland.Add(searchedMembers1[i]);
                }
                member2Friends.Clear();
            }
            else if (member2Friends.Count == 0)
            {

                //Member 2 is isolated, give it its own simulation island!
                for (int i = 0; i < searchedMembers2.Count; i++)
                {
                    searchedMembers2[i].simulationIsland.Remove(searchedMembers2[i]);
                    newIsland.Add(searchedMembers2[i]);
                }
                member1Friends.Clear();
            }

            //Force the system awake.
            //Technically, the members should already be awake.
            //However, calling Activate on them resets the members'
            //deactivation candidacy timers.  This prevents the island
            //from instantly going back to sleep, which could leave
            //objects hanging in mid-air.
            member1.Activate();
            member2.Activate();


        ResetSearchStates:
            for (int i = 0; i < searchedMembers1.Count; i++)
            {
                searchedMembers1[i].searchState = SimulationIslandSearchState.Unclaimed;
            }
            for (int i = 0; i < searchedMembers2.Count; i++)
            {
                searchedMembers2[i].searchState = SimulationIslandSearchState.Unclaimed;
            }
            searchedMembers1.Clear();
            searchedMembers2.Clear();
            return true;

        }