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