public static List <PathingNode> RetracePath(PathingNode start, PathingNode end)
{
List <PathingNode> path = new List <PathingNode>();
PathingNode current = end;
while (current != start && current != null)
{
path.Add(current);
if (current.parent != null)
{
current = current.parent;
}
else
{
current = null;
}
}
path.Add(start);
path.Reverse();
return(path);
}
//Basic A* algorithm. Unable to provide reference as this has been passed through my projects for a while, and is a personal translation of a common algorithm
public List <Transform> GetPath(Transform start, Transform end, Dictionary <Transform, List <Edge> > branches)
{
List <PathingNode> allNodes = new List <PathingNode>();
foreach (Transform t in branches.Keys)
{
allNodes.Add(new PathingNode()
{
transform = t
});
}
//This is inefficient AF, but i think it should work.
//Going to use the edges to assign adjacent nodes.
foreach (PathingNode node in allNodes)
{
List <PathingNode> n = new List <PathingNode>();
foreach (Edge e in branches[node.transform])
{
n.Add(allNodes.Where(element => element.transform == e.end).FirstOrDefault());
}
node.neighbours = n;
}
PathingNode startNode = allNodes.Where(element => element.transform == start).FirstOrDefault();
PathingNode endNode = allNodes.Where(element => element.transform == end).FirstOrDefault();
List <PathingNode> openList = new List <PathingNode>();
List <PathingNode> closedList = new List <PathingNode>();
openList.Add(startNode);
while (openList.Count > 0)
{
PathingNode current = openList[0];
for (int i = 1; i < openList.Count; i++)
{
if (openList[i].F < current.F || (openList[i].F == current.F && openList[i].H < current.H))
{
current = openList[i];
}
}
openList.Remove(current);
closedList.Add(current);
if (current.transform.position == endNode.transform.position)
{
List <PathingNode> nodePath = PathingNode.RetracePath(startNode, endNode);
List <Transform> result = new List <Transform>();
Array.ForEach(nodePath.ToArray(), element => result.Add(element.transform));
//return path;
//Success
}
foreach (PathingNode neighbour in current.neighbours)
{
if (closedList.Contains(neighbour) /* || !neighbour.traversable*/)
{
continue;
}
float newMovementCostToNeighbour = current.G + PathingNode.GetNodeDist(current.transform, neighbour.transform);
if (newMovementCostToNeighbour < neighbour.G || !openList.Contains(neighbour))
{
neighbour.G = newMovementCostToNeighbour;
neighbour.H = PathingNode.GetNodeDist(neighbour.transform, endNode.transform);
neighbour.parent = current;
if (!openList.Contains(neighbour))
{
openList.Add(neighbour);
}
}
}
}
return(null);
}
