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);
}