/// <summary>
/// Returns path between two nodes. On the path first node is start and last node is end.
/// TODO: limit range for optimization?
/// TODO: use this for all pathfinding
/// TODO: diagonals?
/// </summary>
/// <param name="all_nodes"></param>
/// <param name="start"></param>
/// <param name="end"></param>
/// <returns></returns>
public static List <PathfindingNode> Path(List <PathfindingNode> all_nodes, PathfindingNode start, PathfindingNode end, bool diagonal_movement)
{
List <PathfindingNode> path = new List <PathfindingNode>();
List <PathfindingNode> Q = new List <PathfindingNode>();
Dictionary <PathfindingNode, float> dist = new Dictionary <PathfindingNode, float>();
Dictionary <PathfindingNode, PathfindingNode> prev = new Dictionary <PathfindingNode, PathfindingNode>();
if (!all_nodes.Contains(end))
{
all_nodes.Add(end);
}
if (!all_nodes.Contains(start))
{
all_nodes.Add(start);
}
for (int i = 0; i < all_nodes.Count; i++)
{
dist.Add(all_nodes[i], float.MaxValue);
prev.Add(all_nodes[i], null);
Q.Add(all_nodes[i]);
}
dist[start] = 0.0f;
///////TODO: Check for impassable tiles and give them dist = float.MaxValue
foreach (KeyValuePair <Coordinates.Direction, PathfindingNode> v in start.Get_Adjanced_Nodes(all_nodes.ToList(), diagonal_movement))
{
dist[v.Value] = Helper.Is_Diagonal(v.Key) ? Mathf.Sqrt(2.0f) * v.Value.Cost : v.Value.Cost;
prev[v.Value] = start;
}
while (Q.Count > 0)
{
int min_dist_index = 0;
float min_dist = -1.0f;
for (int i = 0; i < Q.Count; i++)
{
if (dist[Q[i]] < min_dist || min_dist == -1.0f)
{
min_dist_index = i;
min_dist = dist[Q[i]];
}
}
PathfindingNode u = Q[min_dist_index];
Q.RemoveAt(min_dist_index);
if (u.Equals(end))
{
while (prev[u] != null)
{
path.Insert(0, prev[u]);
u = prev[u];
}
path.Add(end);
break;
}
else
{
foreach (KeyValuePair <Coordinates.Direction, PathfindingNode> v in u.Get_Adjanced_Nodes(all_nodes, diagonal_movement))
{
float alt = Helper.Is_Diagonal(v.Key) ? dist[u] + (Mathf.Sqrt(2.0f) * v.Value.Cost) : dist[u] + v.Value.Cost;
///////TODO: Check for impassable tiles and give them dist = float.MaxValue
if (alt < dist[v.Value])
{
dist[v.Value] = alt;
prev[v.Value] = u;
}
}
}
}
if (path.Count == 1 && path[0] == end)
{
return(new List <PathfindingNode>());
}
return(path);
}
