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