private static NodeGraph MergeNodeGraph(NodeGraph nodeGraph, VoxelGrid voxelGrid) { bool proximity = true; while (proximity) { proximity = false; Edge mergableEdge = null; foreach (Edge edge in nodeGraph.Edges) { if ((nodeGraph.GetCoordinateOfNode(edge.nodeIndex1) - nodeGraph.GetCoordinateOfNode(edge.nodeIndex2)).Length() < mergeThreshold * voxelGrid.resolution) { mergableEdge = edge; proximity = true; break; } } if (mergableEdge != null) { Vect3 centerPoint = (nodeGraph.GetCoordinateOfNode(mergableEdge.nodeIndex1) + nodeGraph.GetCoordinateOfNode(mergableEdge.nodeIndex2)) / 2d; nodeGraph.MoveNode(nodeGraph.GetNode(mergableEdge.nodeIndex1), centerPoint); nodeGraph.MoveNode(nodeGraph.GetNode(mergableEdge.nodeIndex2), centerPoint); } } nodeGraph.ReIndexNodeGraph(); return(nodeGraph); }
/* * Takes in a list of nodeGraphs and unify them into one nodeGraph. */ public static NodeGraph UnifyGraphs(List <NodeGraph> unifyingGraphs) { NodeGraph baseGraph = unifyingGraphs[0]; unifyingGraphs.Remove(baseGraph); // Define Variables var joiningEdges = new Stack <Edge>(); int newNodeIndex1; int newNodeIndex2; Edge joiningEdge; Edge baseEdge; var edgesAdded = new List <Edge>(); Vect3 collisionPoint; int baseNodeIndex; int extraJoiningNodeIndex; Edge extraJoiningEdge1; Edge extraJoiningEdge2; // Unify each graph with the base graph foreach (NodeGraph joiningGraph in unifyingGraphs) { // Fill edge stack of joining edges joiningEdges.Clear(); for (int i = joiningGraph.Edges.Count - 1; i >= 0; i--) { joiningEdges.Push(joiningGraph.GetEdge(i)); } newNodeIndex1 = -1; newNodeIndex2 = -1; edgesAdded.Clear(); // Join edges LOOP // Find which node indexes that the edge should have. // If -1 a new node needs to be created. while (joiningEdges.Count > 0) { joiningEdge = joiningEdges.Pop(); newNodeIndex2 = newNodeIndex1; newNodeIndex1 = -1; for (int i = 0; i < baseGraph.Edges.Count; i++) { baseEdge = baseGraph.GetEdge(i); if (baseEdge == null || edgesAdded.Contains(baseEdge)) { continue; } // If base edge and joining edge has a collision, find where on the base graph the collision is. // To find out if a new node needs to be created or of the graph should join an exisiting node. if (EdgesCollide(baseEdge, joiningEdge, baseGraph, joiningGraph)) { collisionPoint = GetEdgesCollidingPoint(baseEdge, joiningEdge, baseGraph, joiningGraph); if (collisionPoint != null) { if (joiningGraph.PointCloseToNode(collisionPoint, joiningGraph.GetNode(joiningEdge.nodeIndex1))) { baseNodeIndex = baseGraph.GetCollisionNodeIndex(collisionPoint, baseEdge); if (newNodeIndex1 == -1) { newNodeIndex1 = baseNodeIndex; } else if (newNodeIndex1 != baseNodeIndex) { newNodeIndex1 = baseGraph.UnifyNodes(baseGraph.GetNode(newNodeIndex1), baseGraph.GetNode(baseNodeIndex)).index; baseGraph.MoveNode(baseGraph.GetNode(newNodeIndex1), baseGraph.GetNode(baseNodeIndex).coordinate); } } else if (joiningGraph.PointCloseToNode(collisionPoint, joiningGraph.GetNode(joiningEdge.nodeIndex2))) { baseNodeIndex = baseGraph.GetCollisionNodeIndex(collisionPoint, baseEdge); if (newNodeIndex2 == -1) { newNodeIndex2 = baseNodeIndex; } else if (newNodeIndex2 != baseNodeIndex) { newNodeIndex2 = baseGraph.UnifyNodes(baseGraph.GetNode(newNodeIndex2), baseGraph.GetNode(baseNodeIndex)).index; } } else if (joiningGraph.PointOnEdge(collisionPoint, joiningEdge)) { joiningGraph.AddNode(collisionPoint); extraJoiningNodeIndex = joiningGraph.GetNode(collisionPoint).index; extraJoiningEdge1 = joiningGraph.AddEdge(joiningEdge.nodeIndex1, extraJoiningNodeIndex, joiningEdge.width, joiningEdge.height); extraJoiningEdge2 = joiningGraph.AddEdge(extraJoiningNodeIndex, joiningEdge.nodeIndex2, joiningEdge.width, joiningEdge.height); joiningEdge = extraJoiningEdge2; joiningEdges.Push(extraJoiningEdge1); baseNodeIndex = baseGraph.GetCollisionNodeIndex(collisionPoint, baseEdge); newNodeIndex1 = baseNodeIndex; } } } } // After base graph edge loop. Add Edge to base graph correctly. if (newNodeIndex1 == -1) { baseGraph.AddNode(joiningGraph.GetNode(joiningEdge.nodeIndex1)); newNodeIndex1 = baseGraph.GetNode(joiningGraph.GetNode(joiningEdge.nodeIndex1).coordinate).index; } if (newNodeIndex2 == -1) { baseGraph.AddNode(joiningGraph.GetNode(joiningEdge.nodeIndex2)); newNodeIndex2 = baseGraph.GetNode(joiningGraph.GetNode(joiningEdge.nodeIndex2).coordinate).index; } if (newNodeIndex1 == newNodeIndex2) { throw new Exception("Same new node index, " + newNodeIndex1); } edgesAdded.Add(baseGraph.AddEdge(newNodeIndex1, newNodeIndex2, joiningEdge.width, joiningEdge.height)); } baseGraph.ReIndexNodeGraph(); } return(baseGraph); }