public WaypointNode(Waypoint wp) { waypoint = wp; cameFrom = null; g = 0; h = 0; }
//Retrace from goal once finished AStar private List <Waypoint> RetracePath(WaypointNode goalNode) { List <Waypoint> path = new List <Waypoint>(); WaypointNode current = goalNode; while (current != null) { path.Insert(0, current.Waypoint); WaypointNode next = current.cameFrom; current.cameFrom = null; current = next; } path.RemoveAt(0); //Removing First One -- test return(path); }
//Maybe List<Waypoint> ? public List <Waypoint> AStar(Waypoint start, Waypoint goal) { WaypointNode goalNode = FindNode(goal); List <WaypointNode> open = new List <WaypointNode> { FindNode(start) }; List <WaypointNode> visited = new List <WaypointNode>(); int o = 0; while (open.Count > 0 && o++ < 100) { //Get Current Node By Smallest F. Possible heap optimization WaypointNode current = open[0]; float smallestF = current.f; for (int i = 1; i < open.Count; ++i) { if (open[i].f < smallestF) { current = open[i]; smallestF = open[i].f; } } open.Remove(current); visited.Add(current); //Termination Condition Check if (current == goalNode) { return(RetracePath(goalNode)); } //Add Neighbors foreach (Waypoint neighbor in current.Waypoint.connected) { WaypointNode neighborNode = FindNode(neighbor); float newGCost = current.g + WaypointNode.Distance(current, neighborNode); neighborNode.g = newGCost; if (visited.Contains(neighborNode)) { continue; } //Update Best Path and Add Neighbors to Open List if (!open.Contains(neighborNode) || neighborNode.g < newGCost) { neighborNode.g = newGCost; neighborNode.h = WaypointNode.Distance(neighborNode, goalNode); neighborNode.cameFrom = current; if (!open.Contains(neighborNode)) { open.Add(neighborNode); } } } } Debug.LogWarning("Failed to use AStar to find path"); return(new List <Waypoint>()); }
public static float Distance(WaypointNode wp1, WaypointNode wp2) { return(Vector2Int.Distance(new Vector2Int(wp1.waypoint.X, wp1.waypoint.Y), new Vector2Int(wp2.waypoint.X, wp2.waypoint.Y))); //return Mathf.Abs(wp1.waypoint.X - wp2.waypoint.X) + Mathf.Abs(wp1.waypoint.Y - wp2.waypoint.Y); }