FIXED ASTAR NOW WORKING!!!!!!!

src/ei/game/algo/AStarNode2D.java

@@ -0,0 +1,117 @@
+package ei.game.algo;
+
+import ei.game.algo.AStarNeighbour.Location;
+
+/**
+ * 
+ * @author Árni Arent
+ *
+ */
+public class AStarNode2D extends AStarNode {
+	protected final static float adjacentCost = 1;
+	protected final static float diagonalCost = (float)Math.sqrt(2)*adjacentCost;
+	protected final static float tieBreaker = adjacentCost/(1024f/1024f); 
+	 
+	public enum Heuristic {
+		Manhattan,
+		Euclidean,
+		Diagonal,
+		DiagonalWithTieBreaking,
+		DiagonalWithTieBreakingCrossProduct,
+	}
+	
+	public static Heuristic heuristic = Heuristic.Manhattan;
+	
+	protected int x, y;
+	
+	public AStarNode2D(int x, int y, int nodeId) {
+		super(nodeId);
+		
+		this.x = x;
+		this.y = y;
+	}
+	
+	public int getX() {
+		return x;
+	}
+
+	public int getY() {
+		return y;
+	}
+
+
+	@Override
+	public float getEstimatedCostTo(AStarNode node, AStarNode startNode) {
+		AStarNode2D goal = (AStarNode2D)node;
+		AStarNode2D start = (AStarNode2D)startNode;
+		
+		float h = 0;
+		
+		/*
+		 * Manhattan distance heuristics. (no diagonal treatment) 
+		 */
+		if(heuristic == Heuristic.Manhattan) {
+			h = adjacentCost * (Math.abs(x-goal.x) + Math.abs(y-goal.y));
+		}
+		
+		/*
+		 * Euclidean distances (move at any angle, again no diagonal treatment).
+		 */
+		else if(heuristic == Heuristic.Euclidean) {
+			h = adjacentCost * (float)Math.sqrt(Math.pow(x-goal.x, 2) + Math.pow(y-goal.y,2));
+		}
+		
+		/*
+		 * Diagonal distance heuristic.
+		 */
+		else if(heuristic == Heuristic.Diagonal || 
+				heuristic == Heuristic.DiagonalWithTieBreaking || 
+				heuristic == Heuristic.DiagonalWithTieBreakingCrossProduct) {
+			float diagonal = Math.min(Math.abs(x-goal.x), Math.abs(y-goal.y));
+			float straight = (Math.abs(x-goal.x) + Math.abs(y-goal.y));
+			h = (diagonalCost * diagonal) + (adjacentCost * (straight - (2*diagonal)));
+			
+			
+			/*
+			 * Normal tie-breaking.
+			 */
+			if(heuristic == Heuristic.DiagonalWithTieBreaking) {
+				h *= (1.0 + tieBreaker);
+			}
+			
+			/*
+			 * Add tie-breaking cross-product to the heuristics.
+			 * (Produces nicer looking diagonal paths, but weird with obstacles)
+			 */
+			else if(heuristic == Heuristic.DiagonalWithTieBreakingCrossProduct) {
+				float dx1 = x - goal.x;
+				float dy1 = y - goal.y;
+				float dx2 = start.x - goal.x;
+				float dy2 = start.y - goal.y;
+				float cross = Math.abs(dx1*dy2 - dx2*dy1);
+				h += cross*0.001;
+			}
+		}
+
+		// Return the heuristic.
+		return h;
+	}
+
+	@Override
+	public float getCost(AStarNode node, Location location) {
+
+		if(heuristic == Heuristic.Manhattan || heuristic == Heuristic.Euclidean) {
+			return adjacentCost;
+		} else {
+			if(location == Location.Adjacent) {
+				return adjacentCost;
+			} else {
+				return diagonalCost;
+			}
+		}
+	}
+	
+
+	
+
+}