preparation for a*search

src/environment/AStar.java

@@ -4,17 +4,30 @@ import java.util.ArrayList;
 import java.util.HashSet;
 import java.util.List;
 import java.util.Set;
+import environment.Node;
+import environment.World;
 
 public class AStar {
 
-	private Set<Node> open = new HashSet<Node>();
-	private Set<Node> closed = new HashSet<Node>();
-	private List<Node> nodes = new ArrayList<Node>();
-	
-	public AStar(Node start, Node goal) {
+	private World world;
+
+	public AStar(World world) {
+
+		this.world = world;
+
+	}
+
+	public List<Node> getNodes() {
+		List<Node> nodes = new ArrayList<Node>();
+
+		Set<Node> open = new HashSet<Node>();
+		Set<Node> closed = new HashSet<Node>();
+
+		Node start = new Node(world.getStart().getX(), world.getStart().getY());
+		Node stop = new Node(world.getStop().getX(), world.getStop().getY());
 
 		start.setG(0);
-		start.setH(manhattanDistance(start, goal));
+		start.setH(manhattanDistance(start, stop));
 		start.setF(start.getH());
 
 		open.add(start);
@@ -32,15 +45,22 @@ public class AStar {
 				}
 			}
 
-			if (current == goal) {
+			if (current == stop) {
 				break;
 			}
 
 			open.remove(current);
 			closed.add(current);
 
+			List<Node> neighbors = new ArrayList<Node>();
+			neighbors.add(new Node(current.getX() + world.getSquareEdgeLenght(), current.getY()));
+			neighbors.add(new Node(current.getX() - world.getSquareEdgeLenght(), current.getY()));
+			neighbors.add(new Node(current.getX(), current.getY() + world.getSquareEdgeLenght()));
+			neighbors.add(new Node(current.getX(), current.getY() - world.getSquareEdgeLenght()));
+			current.setNeighbors(neighbors);
+
 			for (Node neighbor : current.getNeighbors()) {
-				if (neighbor == null) {
+				if (neighbor == null || isOccupied(neighbor) || isOutsideMap(neighbor)) {
 					continue;
 				}
 
@@ -53,7 +73,7 @@ public class AStar {
 
 				if (!open.contains(neighbor) && !closed.contains(neighbor)) {
 					neighbor.setG(nextG);
-					neighbor.setH(manhattanDistance(neighbor, goal));
+					neighbor.setH(manhattanDistance(neighbor, stop));
 					neighbor.setF(neighbor.getG() + neighbor.getH());
 					neighbor.setParent(current);
 					open.add(neighbor);
@@ -61,22 +81,29 @@ public class AStar {
 			}
 		}
 
-		
-		Node current = goal;
+		Node current = stop;
 		while (current.getParent() != null) {
 			nodes.add(current);
 			current = current.getParent();
 		}
 		nodes.add(start);
 
-
-	}
-	
-	public List<Node> getNodes() {
 		return nodes;
 	}
 
 	private int manhattanDistance(Node node1, Node node2) {
 		return Math.abs(node1.getX() - node2.getX()) + Math.abs(node1.getY() - node2.getY());
-	}	
+	}
+
+	private boolean isOccupied(Node node) {
+		// We need to convert since obstacles in in points not nodes
+		Point point = new Point(world, node);
+		return world.getObstacles().contains(point);
+	}
+
+	private boolean isOutsideMap(Node node) {
+		return !(node.getX() >= 0 && node.getX() < world.getWorldWidth() && node.getY() >= 0 && node.getY() < world
+				.getWorldHeight());
+	}
+
 }

src/environment/Node.java

@@ -2,6 +2,7 @@ package environment;
 
 import java.util.List;
 import java.util.ArrayList;
+import environment.World;
 
 public class Node {
 	
@@ -10,10 +11,16 @@ public class Node {
 	private int f;
 	private int g;
 	private int h;
-	private int x;
+	private int x;	
 	private int y;
-	private int cost;
+	private final int cost = 1;
 
+	public Node(int x, int y) {
+		this.x = x;
+		this.y = y;
+	}
+	
+	
 	/**
 	 * @return the neighbors
 	 */
@@ -126,11 +133,12 @@ public class Node {
 		return cost;
 	}
 
-	/**
-	 * @param cost
-	 *            the cost to set
-	 */
-	public void setCost(int cost) {
-		this.cost = cost;
-	}
+
+//	/**
+//	 * @param cost
+//	 *            the cost to set
+//	 */
+//	public void setCost(int cost) {
+//		this.cost = cost;
+//	}
 }

src/environment/Point.java

@@ -1,6 +1,7 @@
 package environment;
 
 import environment.World;
+import environment.Node;
 
 public class Point{
 
@@ -19,6 +20,13 @@ public class Point{
         this.row = row;
     }
     
+    // Backward conversion
+    public Point(World world, Node node) {
+    	this.world = world;
+    	this.col = (node.getX() - world.getSquareCornerOffset()) / world.getSquareEdgeLenght();
+    	this.row = (node.getY() - world.getSquareCornerOffset()) / world.getSquareEdgeLenght();
+    }
+    
     /**
      * @return
      */

src/myrobot/RouteFindingRobot.java

@@ -1,10 +1,11 @@
 package myrobot;
 
-import java.util.TreeSet;
 
+import java.util.List;
+
+import environment.AStar;
 import environment.World;
 import environment.Node;
-import environment.Point;
 import robocode.Robot;
 
 
@@ -23,6 +24,10 @@ public class RouteFindingRobot extends Robot {
 	
 	public void run() {
 		
+		AStar aStar = new AStar(world);
+		
+		List<Node> nodes = aStar.getNodes();
+		
 		doNothing();
 		
 //		Node n = new Node(gen.start, new Point());