private void Update_cell(DStatLiteNode paramLightCell, int Iteration, Maze maze) { DStatLiteNode localLightCell = null; System.Console.WriteLine("Update_cell: " + paramLightCell); Key localKey = new Key(paramLightCell); paramLightCell.iteration = Iteration; if (paramLightCell != this.goal) { paramLightCell.rhs = 2147483647; for (int i = 0; i < Maze.N_DIRECTIONS_WITHOUT_DIAGONALS; i++) { int j = paramLightCell.GetMazeLightCell().X + Maze.delta_x[i]; int k = paramLightCell.GetMazeLightCell().Y + Maze.delta_y[i]; if ((0 > j) || (j >= SimulationForm.MAZE_W) || (0 > k) || (k >= SimulationForm.MAZE_H) || (maze.cells[j, k].blocked)) { continue; } int m; if (this.graph[j, k].g == 2147483647) { m = 2147483647; } else { m = this.graph[j, k].g + 1; } if (paramLightCell.rhs > m) { paramLightCell.rhs = m; localLightCell = this.graph[j, k]; } } } //open_list.Delete(localLightCell); this.u.remove(localKey); paramLightCell.parent = localLightCell; System.Console.WriteLine("New Parente: " + localLightCell); if (paramLightCell.g != paramLightCell.rhs) { open_list.Insert(paramLightCell); this.u.add(new Key(paramLightCell)); } }
public void Calculate_path(bool paramBoolean, int Iteration, Maze maze) { //Clear_path(); //with heap : DStatLiteNode node; while (!Execution_end()) { //with heap : node = (DStatLiteNode)open_list.Pop(); //with heap : node.closed = true; Key localKey = (Key)this.u.first(); this.u.remove(localKey); DStatLiteNode node = localKey.cell; node.iteration = Iteration; int i; int j; int k; if (node.g > node.rhs) { node.g = node.rhs; if (node != this.start) { for (i = 0; i < Maze.N_DIRECTIONS_WITHOUT_DIAGONALS; i++) { j = node.GetMazeLightCell().X + Maze.delta_x[i]; k = node.GetMazeLightCell().Y + Maze.delta_y[i]; //if ((0 > j) || (j >= this.size_x) || (0 > k) || (k >= this.size_y) || (this.cells[j][k].type_robot_vision == 1) || (node.g + 1 >= this.cells[j][k].rhs)) if ((0 > j) || (j >= SimulationForm.MAZE_W) || (0 > k) || (k >= SimulationForm.MAZE_H) || (maze.cells[j, k].blocked) || (node.g + 1 >= this.graph[j, k].rhs)) { continue; } Update_cell(this.graph[j, k], Iteration, maze); } } } else { node.g = 2147483647; Update_cell(node, Iteration, maze); for (i = 0; i < Maze.N_DIRECTIONS_WITHOUT_DIAGONALS; i++) { j = node.GetMazeLightCell().X + Maze.delta_x[i]; k = node.GetMazeLightCell().Y + Maze.delta_y[i]; //if ((0 > j) || (j >= this.size_x) || (0 > k) || (k >= this.size_y) || (this.cells[j][k].type_robot_vision == 1)) if ((0 > j) || (j >= SimulationForm.MAZE_W) || (0 > k) || (k >= SimulationForm.MAZE_H) || (maze.cells[j, k].blocked)) { continue; } Update_cell(this.graph[j, k], Iteration, maze); } } if (paramBoolean) { break; } } if (Execution_end()) { Mark_path(maze); } }
public void Solve() { DStatLiteNode node; while (!HasExecutionFinished()) { node = (DStatLiteNode)open_list.Pop(); node.closed = true; if (node == goal) { DStatLiteNode node_child = null; path_cost = node.g; has_solution = true; if (mark_path) { node.GetMazeLightCell().SetPathFlag(); node_child = node; node = node.parent; do { node.GetMazeLightCell().SetNextMazeCell(node_child.GetMazeLightCell()); node.GetMazeLightCell().SetPathFlag(); node_child = node; node = node.parent; }while(node != null); } break; } for (int i = 0; i < neighborhood; i++) { int x, y; x = node.GetMazeLightCell().X + Maze.delta_x[i]; y = node.GetMazeLightCell().Y + Maze.delta_y[i]; if (0 <= x && x < w && 0 <= y && y < h) { DStatLiteNode child = graph[y, x]; if (child.GetMazeLightCell().IsBlocked() || child.closed) { continue; } int cost = child.GetMazeLightCell().GetCost(); if (open_list.Has(child)) { if (child.g > node.g + cost) { child.parent = node; child.g = node.g + cost; child.f = child.g + child.h; open_list.Insert(child); } } else { child.parent = node; child.g = node.g + cost; child.f = child.g + child.h; open_list.Insert(child); } } } if (step_by_step) { break; } } }