/// Finds a path in the NodeMap(map) from start to destination
/// Uses optimized Astar algorithem
/// returns a empty list if start is blocked, end is blocked or start == end.
/// Retruns a full list with each nodegrid to target on success
public static List <Coordinate> GetNodePath(NodeMap map, Coordinate start, Coordinate destination)
{
var checkedCoordinates = new Dictionary <Coordinate, Node>();
var promiseList = new NodePromiseList();
if (start == destination || map.IsBlocked(start) || map.IsBlocked(destination))
{
Debug.Log("No path");
return(new List <Coordinate>());
}
promiseList.AddSorted(new Node(null, start, 0, Coordinate.Distance(start, destination)));
Node current = null;
float cost = 1;
while (promiseList.Count > 0)
{
current = promiseList[0];
promiseList.Remove(current);
if (checkedCoordinates.ContainsKey(current.position))
{
continue;
}
checkedCoordinates[current.position] = current;
if (current.position == destination)
{
List <Coordinate> path = new List <Coordinate>();
while (current.parent != null)
{
path.Add(current.position);
current = current.parent;
}
path.Add(start);
path.Reverse();
return(path);
}
/* To speed this up, diagonal neighbours could be in their own list.
* This way we dont have to calculate the movement vector, check if they are diagonals and change the cost on every loop
*/
foreach (var neighbour in map.GetNode(current.position).Neighbours)
{
if (checkedCoordinates.ContainsKey(neighbour.Position))
{
continue;
}
if (IsDiagonalDirection(neighbour.Position - current.position))
{
cost = 1.4f;
}
else
{
cost = 1f;
}
Node newNode = new Node(current, neighbour.Position, current.cost + cost, current.cost + cost + Coordinate.Distance(neighbour.Position, destination));
promiseList.AddSorted(newNode);
}
}
Debug.Log("No path");
return(new List <Coordinate>());
}
//Find shortest path on a Map
//Use GetNodePath this is 80% Obsolete
public static List <Coordinate> GetPath(Map map, Coordinate start, Coordinate destination)
{
var checkedCoordinates = new Dictionary <Coordinate, Node>();
var promiseList = new NodePromiseList();
if (start == destination || map.IsBlocked(start) || map.IsBlocked(destination))
{
Debug.Log("No path");
return(null);
}
promiseList.Add(new Node(null, start, 0, Coordinate.Distance(start, destination)));
Coordinate next;
Node current = null;
while (promiseList.Count > 0)
{
current = promiseList.PopLowestPromise();
if (checkedCoordinates.ContainsKey(current.position))
{
continue;
}
checkedCoordinates[current.position] = current;
if (current.position == destination)
{
List <Coordinate> path = new List <Coordinate>();
while (current.parent != null)
{
path.Add(current.position);
current = current.parent;
}
path.Add(start);
path.Reverse();
return(path);
}
foreach (Coordinate direction in Coordinate.directions)
{
next = current.position + direction;
if (!map.WithinBounds(next) || checkedCoordinates.ContainsKey(next) || map.IsBlocked(next))
{
continue;
}
//Dont cross non-traversable corners when going diagonally
if (IsDiagonalDirection(direction) && (map.IsBlocked(current.position + new Coordinate(direction.X, 0)) || map.IsBlocked(current.position + new Coordinate(0, direction.Y))))
{
continue;
}
Node newNode = new Node(current, next, current.cost + 1, current.cost + 1 + Coordinate.Distance(next, destination));
promiseList.Add(newNode);
}
}
Debug.Log("No path");
return(null);
}