private void AddEdges(GraphNode <Platform> gNode)
{
int top = (int)Math.Floor((float)gNode.Value.Y / TileHeight) - 4;
int bottom = (int)Math.Floor((float)gNode.Value.Y / TileHeight) + 14;
int left = (int)Math.Floor((float)gNode.Value.LeftEdgeX / TileWidth) - 4;
int right = (int)Math.Floor((float)gNode.Value.RightEdgeX / TileWidth) + 4;
if (top < 0)
{
top = 0;
}
if (bottom > Height)
{
bottom = Height;
}
if (left < 0)
{
left = 0;
}
if (right > Width)
{
right = Width;
}
GraphNode <Platform> fromNode = (GraphNode <Platform>)NavMesh.Nodes.FindByValue(gNode.Value);
GraphNode <Platform> toNode;
for (int y = top; y <= bottom; y++)
{
for (int x = left; x <= right; x++)
{
toNode = (GraphNode <Platform>)NavMesh.Nodes.FirstOrDefault(node => node.Value.Y == y * TileHeight && node.Value.LeftEdgeX <= x * TileWidth && node.Value.RightEdgeX >= x * TileWidth);
if (toNode != null && !toNode.Equals(fromNode) && !fromNode.Neighbors.Contains(toNode))
{
NavMesh.AddDirectedEdge(fromNode, toNode, Math.Abs(fromNode.Value.Y - toNode.Value.Y));
}
}
}
//GraphNode<Tile> tempGNode;
//foreach (var node in possibleNeighbors)
//{
// if (node.Value.X > left)
// {
// tempTile = new Tile(GetCollision(node.Value.X-1, node.Value.Y),node.Value.X-1,node.Value.Y,TileWidth,TileHeight);
// tempGNode = (GraphNode<Tile>)possibleNeighbors.Nodes.FindByValue(tempTile);
// if (tempGNode != null)
// possibleNeighbors.AddDirectedEdge(node,tempGNode,1);
// }
// if (node.Value.X < right)
// {
// tempTile = new Tile(GetCollision(node.Value.X + 1, node.Value.Y), node.Value.X + 1, node.Value.Y, TileWidth, TileHeight);
// tempGNode = (GraphNode<Tile>)possibleNeighbors.Nodes.FindByValue(tempTile);
// if (tempGNode != null)
// possibleNeighbors.AddDirectedEdge(node, tempGNode, 1);
// }
// if (node.Value.Y > top)
// {
// tempTile = new Tile(GetCollision(node.Value.X, node.Value.Y - 1), node.Value.X, node.Value.Y - 1, TileWidth, TileHeight);
// tempGNode = (GraphNode<Tile>)possibleNeighbors.Nodes.FindByValue(tempTile);
// if (tempGNode != null)
// possibleNeighbors.AddDirectedEdge(node, tempGNode, 1);
// }
// if (node.Value.Y < bottom)
// {
// tempTile = new Tile(GetCollision(node.Value.X, node.Value.Y+1), node.Value.X, node.Value.Y+1, TileWidth, TileHeight);
// tempGNode = (GraphNode<Tile>)possibleNeighbors.Nodes.FindByValue(tempTile);
// if (tempGNode != null)
// possibleNeighbors.AddDirectedEdge(node, tempGNode, 1);
// }
//}
//return possibleNeighbors;
}
static public Graph <Platform> FindPath(Platform start, Platform finish)
{
if (navmesh == null)
{
throw new NullReferenceException("No Navigation Mesh Loaded.");
}
GraphNode <Platform> startNode = (GraphNode <Platform>)navmesh.Nodes.FindByValue(start);
GraphNode <Platform> finishNode = (GraphNode <Platform>)navmesh.Nodes.FindByValue(finish);
if (startNode == null || finishNode == null)
{
throw new ArgumentNullException("Start or finish platform is not in nav mesh");
}
Graph <Platform> path = new Graph <Platform>();
Queue <GraphNode <Platform> > frontier = new Queue <GraphNode <Platform> >();
Dictionary <GraphNode <Platform>, GraphNode <Platform> > cameFrom = new Dictionary <GraphNode <Platform>, GraphNode <Platform> >();
GraphNode <Platform> current;
frontier.Enqueue(startNode);
cameFrom.Add(startNode, null);
bool foundIt = false;
while (frontier.Count > 0)
{
current = frontier.Dequeue();
if (current.Equals(finishNode))
{
foundIt = true;
break;
}
foreach (GraphNode <Platform> neighbor in current.Neighbors)
{
if (!cameFrom.ContainsKey(neighbor))
{
cameFrom.Add(neighbor, current);
frontier.Enqueue(neighbor);
}
}
}
if (foundIt == false)
{
return(null);
}
GraphNode <Platform> previous;
current = finishNode;
path.AddNode(current.Value);
while (current != startNode)
{
previous = current;
current = cameFrom[current];
path.AddNode(current.Value);
path.AddDirectedEdge((GraphNode <Platform>)path.Nodes.FindByValue(current.Value), (GraphNode <Platform>)path.Nodes.FindByValue(previous.Value), 1);
}
return(path);
}