private Path Find(List<Path> pathes, MatrixPosition end)
{
// find the path with cheaper frontier
var orderedPathes = pathes.OrderBy(p => p.Cost(end.x, end.z));
var current = orderedPathes.First();
if (current.frontier.position.x == end.x && current.frontier.position.z == end.z)
{
return current;
}
// explore current frontier
foreach (var v in current.frontier.Output)
{
if (!CheckVertexExplored(v.To, pathes))
{
var newPath = new Path { actions = new List<Edge>(current.actions) { v }, frontier = v.To };
pathes.Add(newPath);
}
}
// remove current, it is not needed more
pathes.Remove(current);
return Find(pathes, end);
}
// find the best path with the AStar
public Path GeneratePath(MatrixPosition start, MatrixPosition end)
{
var pathes = new List<Path>();
var startVertex = vertices[start.x, start.z, start.k];
var first = new Path {frontier = startVertex};
pathes.Add(first);
return Find(pathes, end);
}