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