public static double GetBestPathLength(IntPoint2D from, IntPoint2D to, bool[,] pathingGrid, out int visitedNodesCount)
{
var visitedNodes = new HashSet <Node>();
var unvisitedNodes = new HashSet <Node>();
unvisitedNodes.Add(new Node()
{
Point = from, PathLength = 0
});
while (unvisitedNodes.Count > 0)
{
Node bestNode = GetBestNode(unvisitedNodes, to);
visitedNodes.Add(bestNode);
if (bestNode.Point.Equals(to))
{
visitedNodesCount = visitedNodes.Count;
return(bestNode.PathLength);
}
unvisitedNodes.Remove(bestNode);
AddNewNodes(bestNode, pathingGrid, visitedNodes, unvisitedNodes);
}
//for debugging
var result = VisualizeNodes(visitedNodes, pathingGrid.GetLength(0), pathingGrid.GetLength(1));
//no path found
visitedNodesCount = visitedNodes.Count;
return(-1);
}
public override bool Equals(object obj)
{
if (obj is IntPoint2D)
{
IntPoint2D secondPoint = (IntPoint2D)obj;
return(X == secondPoint.X && Y == secondPoint.Y);
}
return(false);
}
private static Node GetBestNode(HashSet <Node> unvisitedNodes, IntPoint2D to)
{
Node bestNode = null;
double bestDistance = 0;
foreach (var node in unvisitedNodes)
{
double newDistance = Math.Sqrt(SquaredDistance(to, node));
if (bestNode == null || node.PathLength + newDistance < bestNode.PathLength + bestDistance)
{
bestNode = node;
bestDistance = newDistance;
}
}
return(bestNode);
}
private static double SquaredDistance(IntPoint2D to, Node bestNode)
{
return(Math.Pow(bestNode.Point.X - to.X, 2) + Math.Pow(bestNode.Point.Y - to.Y, 2));
}
private static bool IsPathable(IntPoint2D node, bool[,] map)
{
return(map[(int)node.X, (int)node.Y]);
}
public static double GetBestPathLength(IntPoint2D from, IntPoint2D to, bool[,] pathingGrid)
{
return(GetBestPathLength(from, to, pathingGrid, out _));
}