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