OK, here I have the code for the maze:
import java.util.*;
interface State extends Comparable<State> {
double goalDistanceEstimate();
boolean isGoal();
ArrayList<State> getChildren();
}
class Node implements Comparable<Node> {
State state;
/* the cost associated with reaching this node from
the start node, i.e. cost of parent node + cost
of moving from parent to this node */
double g;
double h; // the estimated distance to goal
double f; // costs + estimated_distance
Node parent;
Node(State state, Node parent) {
this.state = state;
this.parent = parent;
if (parent != null)
g = parent.g + 1;
else
g = 0;
h = state.goalDistanceEstimate();
f = g + h;
}
// replace current value of g with given value
void setG(double value) {
g = value;
// new g affects f as well
f = g + h;
}
// replace current parent if shorter path found for this node
void setParent(Node parent) {
this.parent = parent;
}
public int compareTo(Node other) {
if (f < other.f)
return -1;
else if (f == other.f)
return 0;
else
return 1;
}
}
class AStarAlgorithm {
TreeMap<State, Node> open_list = new TreeMap<State, Node>();
TreeMap<State, Node> closed_list = new TreeMap<State, Node>();
// the list of nodes in open list in sorted order
PriorityQueue<Node> open_list_nodes = new PriorityQueue<Node>();
int iterations = 0;
/* Returns the goal node when solution is found, or
null when no solution was found */
public Node search() {
Node best;
while (!open_list_nodes.isEmpty()) {
++iterations;
// get the bestnode, as defined by its states's cost f
best = open_list_nodes.poll();
// if selected node is the goal node search is finished
if (best.state.isGoal())
return best;
// Debug
System.out.println("\nIteration : " + iterations);
System.out.println("--");
System.out.println("\n\n-->Best state: " + best.state);
System.out.println("Open list contains:");
Set<Map.Entry><State, Node>> set = open_list.entrySet();
for (Map.Entry<State, Node> e : set) {
State state = e.getKey();
Node node = e.getValue();
System.out.print(" State: " + state + ", Node: g=" + node.g +
", h=" + node.h + ", f=" + node.f);
}
System.out.println("\n\nClosed list contains:");
set = closed_list.entrySet();
for (Map.Entry<State, Node> e : set) {
State state = e.getKey();
Node node = e.getValue();
System.out.print(" State: " + state + ", Node: g=" + node.g +
", h=" + node.h + ", f=" + node.f);
}
System.out.println("\n\nNodes list contains:");
for (Node e : open_list_nodes)
System.out.println(e.state + " (f=" + e.f + ")");
double childCost = 1 + best.g;
ArrayList<State> children = best.state.getChildren();
// Debug
/*System.out.println("\nChildren states: ");
for (State e : children)
System.out.println(e); */
for (int i=0; i < children.size(); i++) {
State childState = children.get(i);
if (open_list.containsKey(childState)) {
Node childNode = open_list.get(childState);
/* update cost of child node if the calculated
cost from this path is smaller - new parent
must be noted as well */
if (childNode.g > childCost) {
childNode.setG(childCost);
childNode.setParent(best);
}
}
else if (closed_list.containsKey(childState)) {
Node childNode = closed_list.get(childState);
/* update cost of child node if the calculated
cost from this path is smaller - new parent
must be noted as well*/
if (childNode.g > childCost) {
childNode.setG(childCost);
childNode.setParent(best);
/* moved updated node from closed list
to open list */
closed_list.remove(childNode);
open_list.put(childState, childNode);
open_list_nodes.add(childNode);
}
}
else { // child node neither in open nor in closed lists
Node newNode = new Node(childState, best);
open_list.put(childState, newNode);
open_list_nodes.add(newNode);
}
}
/* Remove the expanded node from the open list
and put it in the closed list */
open_list.remove(best.state);
//open_list_nodes.remove(best);
closed_list.put(best.state, best);
}
return null; // no solution was found
}
/* returns an ArrayList containing the successive states which
consist of the minimum path, in reverse order, i.e. goal to
start */
public ArrayList<State> solve(State initial_state) {
/* The goal state containing the solution. The full solution
is obtained by following the parent path from this node to
the start position */
Node solution;
Node firstNode = new Node(initial_state, null);
open_list.put(initial_state, firstNode);
open_list_nodes.add(firstNode);
solution = search();
System.out.println("Solution found-> best past length: " + solution.g);
// clear all data structures for subsequent invocations of this method
open_list_nodes.clear();
open_list.clear();
closed_list.clear();
ArrayList<State> minimum_path = new ArrayList<State>();
while (solution.parent != null) {
minimum_path.add(solution.state);
solution = solution.parent;
}
minimum_path.add(solution.state);
return minimum_path;
}
}
class Maze implements State {
/* describes the obstacles and the empty spaces in
a 2-dimensional space */
static byte maze[][];
/* the following describe the start and goal positions
Since the starting position and goal position do not
change in this problem, we could declare them static */
static int start_row;
static int start_col;
static int goal_row;
static int goal_col;
/* as each maze is itself a "State", the following describe
the current "state" (position) of the maze */
int current_row;
int current_col;
public Maze(byte maze[][], int start_row, int start_col,
int goal_row, int goal_col) {
this.maze = maze;
this.start_row = start_row;
this.start_col = start_col;
this.goal_row = goal_row;
this.goal_col = goal_col;
current_row = start_row;
current_col = start_col;
}
public Maze(int current_row, int current_col) {
this.current_row = current_row;
this.current_col = current_col;
}
public boolean equals(Object o) {
return ((o instanceof Maze) &&
((Maze) o).current_row == current_row &&
((Maze) o).current_col == current_col);
}
/* compare this state with another state based on
current position. We only care about equality as
this is used by the containsKey() method of TreeMap
utilised in the AStarAlgorithm. */
public int compareTo(State other) {
if ((current_row == ((Maze) other).current_row) &&
(current_col == ((Maze) other).current_col))
return 0;
else
return 1;
}
public int hashCode() {
return current_row*current_col;
}
public double goalDistanceEstimate() {
double distance = 0;
int row = current_row;
int col = current_col;
/* goal is a north-east cell keep moving towards north east
until reaching the row or the column containing the goal */
if (goal_row <= current_row && goal_col >= current_col) {
while ((row != goal_row) && (col != goal_col)) {
--row;
++col;
++distance;
}
distance += Math.abs(row-goal_row) + Math.abs(col-goal_col);
}
else if (goal_row < current_row && goal_col < current_col) {
// goal is a north-west cell
while ((row != goal_row) && (col != goal_col)) {
--row;
--col;
++distance;
}
distance += Math.abs(row-goal_row) + Math.abs(col-goal_col);
}
else if (goal_row >= current_row && goal_col >= current_col) {
// goal is a south-east cell
while ((row != goal_row) && (col != goal_col)) {
++row;
++col;
++distance;
}
distance += Math.abs(row-goal_row) + Math.abs(col-goal_col);
}
else if (goal_row > current_row && goal_col < current_col) {
// goal is a south-west cell
while ((row != goal_row) && (col != goal_col)) {
++row;
--col;
++distance;
}
distance += Math.abs(row-goal_row) + Math.abs(col-goal_col);
}
return distance;
}
public boolean isGoal() {
if ((goal_row == current_row) && (goal_col == current_col))
return true;
else
return false;
}
public ArrayList<State> getChildren() {
ArrayList<State> list = new ArrayList<State>();
// get north neighbour of current cell if this is not an obstacle
if ((current_row - 1) >= 0 && maze[current_row-1][current_col] != 1
&& !((start_row == current_row -1) && (start_col == current_col)))
list.add(new Maze((current_row - 1), current_col));
// get south neighbour of current cell if this is not an obstacle
if ((current_row + 1) < maze.length &&
maze[current_row+1][current_col] != 1 &&
!((start_row == current_row+1) && (start_col == current_col)))
list.add(new Maze((current_row + 1), current_col));
// get east neighbour of current cell if this is not an obstacle
if ((current_col + 1) < maze[0].length &&
maze[current_row][current_col+1] != 1 &&
!((start_row == current_row) && (start_col == current_col+1)))
list.add(new Maze(current_row, (current_col + 1)));
// get west neighbour of current cell if this is not an obstacle
if ((current_col - 1) >= 0 &&
maze[current_row][current_col-1] != 1 &&
!((start_row == current_row) && (start_col == current_col-1)))
list.add(new Maze(current_row, (current_col - 1)));
// get north-east neighbour of current cell if this is not an obstacle
if ((current_row - 1) >= 0 && (current_col + 1) < maze[0].length &&
maze[current_row-1][current_col+1] != 1 &&
!((start_row == current_row-1) && (start_col == current_col+1)))
list.add(new Maze((current_row - 1), (current_col + 1)));
// get south-east neighbour of current cell if this is not an obstacle
if ((current_row + 1) < maze.length &&
(current_col + 1) < maze[0].length &&
maze[current_row+1][current_col+1] != 1 &&
!((start_row == current_row+1) && (start_col == current_col+1)))
list.add(new Maze((current_row + 1), (current_col + 1)));
// get north-west neighbour of current cell if this is not an obstacle
if ((current_row - 1) >= 0 && (current_col - 1) >= 0 &&
maze[current_row-1][current_col-1] != 1 &&
!((start_row == current_row-1) && (start_col == current_col-1)))
list.add(new Maze((current_row - 1), (current_col - 1)));
// get south-west neighbour of current cell if this is not an obstacle
if ((current_row + 1) < maze.length &&
(current_col - 1) >= 0 &&
maze[current_row+1][current_col-1] != 1 &&
!((start_row == current_row+1) && (start_col == current_col-1)))
list.add(new Maze((current_row + 1), (current_col - 1)));
return list;
}
public String toString() {
return "x: " + current_row + ", y: " + current_col;
}
}
public class MazeProblem {
public static void main(String[] args) {
/* following array describes a maze consisting of tiles.
1 stands for obstacle in the corresponding tile */
byte maze_description[][] = new byte[][]{
{1, 1, 0, 0, 0, 0, 0, 0, 0, 0},
{1, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 1, 1, 1, 1, 1, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 1, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 1, 0, 0},
{0, 0, 0, 0, 0, 0, 0, 1, 0, 0},
{1, 1, 1, 0, 0, 0, 0, 0, 0, 0},
{1, 0, 0, 0, 0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0, 1, 1, 0, 0}};
int start_row = 5;
int start_col = 4;
int goal_row = 1;
int goal_col = 6;
Maze maze = new Maze(maze_description, start_row, start_col,
goal_row, goal_col);
AStarAlgorithm algo = new AStarAlgorithm();
ArrayList<State> minimum_path = algo.solve(maze);
// print the full solution for the shortest path
System.out.println("\n\n\n*** Shortest Path is: ***");
for (int i=minimum_path.size()-1; i >=0 ; i--)
System.out.println(minimum_path.get(i));
}
}
The result of this code is in text based form which only displays the shortest path from the initial position to the goal position.
My task is to modify by adding a robot travelling from the start position to the goal position [the maze is splitted into 10 x 10 cells]. This should be displayed on a frame showing the robot moving from one cell to another at each stage.
I hope this provides adequate information,
Kind regards
asciia at 2007-7-13 13:38:46 >
