static string TracePath(Maze_element elem) { //Console.WriteLine("Tracing..."); string path = ""; var tmp = elem; while (tmp.fromNode != null) { var par = tmp.fromNode; if (par.x < tmp.x) { path = "S" + path; } else if (par.x > tmp.x) { path = "N" + path; } else if (par.y < tmp.y) { path = "E" + path; } else if (par.y > tmp.y) { path = "W" + path; } tmp = par; } return(path); }
public Maze_element(int i, int j) { x = i; y = j; f = Int32.MaxValue; g = Int32.MaxValue; h = 0; heapIndex = -1; fromNode = null; }
public void Add(Maze_element element) { if (_size == _elements.Length) { throw new IndexOutOfRangeException(); } _elements[_size] = element; element.heapIndex = _size; _size++; ReCalculateUp(_size - 1); }
static string A_star_pathFinder(List <List <string> > Maze, int Ax, int Ay, int Bx, int By) { //Console.WriteLine("Start A*"); string pathResult = ""; List <List <Maze_element> > MazeObj = new List <List <Maze_element> >(); int n = Maze.Count; int m = Maze[0].Count; MinHeapClass MinHeap = new MinHeapClass(n * m); for (int i = 0; i < n; i++) { List <Maze_element> tmpArr = new List <Maze_element>(); for (int j = 0; j < m; j++) { Maze_element elem = new Maze_element(i, j); elem.h = Math.Abs(i - Bx) + Math.Abs(j - By); if ((i == Ax) && (j == Ay)) { elem.g = 0; elem.f = elem.g + elem.h; } tmpArr.Add(elem); if (Maze[i][j] != "X") { MinHeap.Add(elem); } } MazeObj.Add(tmpArr); } Console.WriteLine(MinHeap.Peek().f); bool stop = false; do { Maze_element elem = MinHeap.Pop(); //Scan 4 adjacent block int[] compass = { -1, 0, 1, 0, -1 }; for (int i = 0; i < 4; i++) { int x = elem.x + compass[i]; int y = elem.y + compass[i + 1]; if ((x < 0) || (y < 0)) { continue; } if ((x >= n) || (y >= m)) { continue; } if (Maze[x][y] == "X") { continue; } if (MazeObj[x][y].f > elem.g + 1 + MazeObj[x][y].h) { MazeObj[x][y].g = elem.g + 1; MazeObj[x][y].f = MazeObj[x][y].g + MazeObj[x][y].h; MazeObj[x][y].fromNode = elem; var heapIndex = MazeObj[x][y].heapIndex; MinHeap.ReCalculateUp(heapIndex); MinHeap.ReCalculateDown(heapIndex); } if ((x == Bx) && (y == By)) { stop = true; //Console.WriteLine("Found B"); pathResult = TracePath(MazeObj[x][y]); break; } } if (stop) { break; } } while (!MinHeap.IsEmpty()); return(pathResult); }