From 148c0c739fa956977646e5c7b700a3723af1b9ec Mon Sep 17 00:00:00 2001
From: Ziver Koc <ziver@koc.se>
Date: Mon, 16 Apr 2007 15:52:24 +0000
Subject: [PATCH] Added Astar algo

---
 src/ei/game/algo/AStar.java       | 111 +++++++++++++++++++++++++++
 src/ei/game/algo/AStarNode.java   | 123 ++++++++++++++++++++++++++++++
 src/ei/game/scene/Map.java        |  11 +++
 src/ei/game/scene/units/Unit.java |  15 +++-
 4 files changed, 258 insertions(+), 2 deletions(-)
 create mode 100644 src/ei/game/algo/AStar.java
 create mode 100644 src/ei/game/algo/AStarNode.java

diff --git a/src/ei/game/algo/AStar.java b/src/ei/game/algo/AStar.java
new file mode 100644
index 0000000..6d13150
--- /dev/null
+++ b/src/ei/game/algo/AStar.java
@@ -0,0 +1,111 @@
+package ei.game.algo;
+import java.util.*;
+
+/**
+  The AStarSearch class, along with the AStarNode class,
+  implements a generic A* search algorithm. The AStarNode
+  class should be subclassed to provide searching capability.
+*/
+public class AStar {
+
+
+  /**
+    A simple priority list, also called a priority queue.
+    Objects in the list are ordered by their priority,
+    determined by the object's Comparable interface.
+    The highest priority item is first in the list.
+  */
+  public static class PriorityList extends LinkedList {
+	private static final long serialVersionUID = 1L;
+
+	public void add(Comparable object) {
+      for (int i=0; i<size(); i++) {
+        if (object.compareTo(get(i)) <= 0) {
+          add(i, object);
+          return;
+        }
+      }
+      addLast(object);
+    }
+  }
+
+
+  /**
+    Construct the path, not including the start node.
+  */
+  protected List constructPath(AStarNode node) {
+    LinkedList<AStarNode> path = new LinkedList<AStarNode>();
+    while (node.pathParent != null) {
+      path.addFirst(node);
+      node = node.pathParent;
+    }
+    return path;
+  }
+
+  private boolean contains(LinkedList<AStarNode> list,AStarNode find){
+	  for(int i=0; i<list.size() ;i++){
+		  if(list.get(i).getPos().getX() == find.getPos().getX() &&
+				  list.get(i).getPos().getY() == find.getPos().getY())
+			  return true;
+	  }
+	  return false;
+  }
+
+  /**
+    Find the path from the start node to the end node. A list
+    of AStarNodes is returned, or null if the path is not
+    found. 
+  */
+  public List findPath(AStarNode startNode, AStarNode goalNode) {
+
+    PriorityList openList = new PriorityList();
+    LinkedList<AStarNode> closedList = new LinkedList<AStarNode>();
+
+    startNode.costFromStart = 0;
+    startNode.estimatedCostToGoal =
+      startNode.getEstimatedCost(goalNode);
+    startNode.pathParent = null;
+    openList.add(startNode);
+
+    while (!openList.isEmpty()) {
+      AStarNode node = (AStarNode)openList.removeFirst();
+      if (node == goalNode) {
+        // construct the path from start to goal
+        return constructPath(goalNode);
+      }
+
+      List neighbors = node.getNeighbors();
+      for (int i=0; i<neighbors.size(); i++) {
+        AStarNode neighborNode =
+          (AStarNode)neighbors.get(i);
+        boolean isOpen = contains(openList,neighborNode);
+        boolean isClosed = contains(closedList,neighborNode);
+        float costFromStart = node.costFromStart +
+          node.getCost(neighborNode);
+
+        // check if the neighbor node has not been
+        // traversed or if a shorter path to this
+        // neighbor node is found.
+        if ((!isOpen && !isClosed) ||
+          costFromStart < neighborNode.costFromStart)
+        {
+          neighborNode.pathParent = node;
+          neighborNode.costFromStart = costFromStart;
+          neighborNode.estimatedCostToGoal =
+            neighborNode.getEstimatedCost(goalNode);
+          if (isClosed) {
+            closedList.remove(neighborNode);
+          }
+          if (!isOpen) {
+            openList.add(neighborNode);
+          }
+        }
+      }
+      closedList.add(node);
+    }
+
+    // no path found
+    return null;
+  }
+
+}
\ No newline at end of file
diff --git a/src/ei/game/algo/AStarNode.java b/src/ei/game/algo/AStarNode.java
new file mode 100644
index 0000000..3480bf8
--- /dev/null
+++ b/src/ei/game/algo/AStarNode.java
@@ -0,0 +1,123 @@
+package ei.game.algo;
+import java.util.LinkedList;
+import java.util.List;
+
+import ei.engine.math.Vector2i;
+import ei.game.gamestate.InGameState;
+
+/**
+  The AStarNode class, along with the AStarSearch class,
+  implements a generic A* search algorithm. The AStarNode
+  class should be subclassed to provide searching capability.
+ */
+public class AStarNode implements Comparable {
+	private Vector2i pos;
+	AStarNode pathParent;
+	float costFromStart;
+	float estimatedCostToGoal;
+
+	public AStarNode(Vector2i pos,AStarNode parent){
+		this.pos = pos;
+		this.pathParent = parent;
+	}
+	
+	public float getCost() {
+		return costFromStart + estimatedCostToGoal;
+	}
+
+
+	public int compareTo(Object other) {
+		float thisValue = this.getCost();
+		float otherValue = ((AStarNode)other).getCost();
+
+		float v = thisValue - otherValue;
+		return (v>0)?1:(v<0)?-1:0; // sign function
+	}
+
+	public Vector2i getPos(){
+		return pos;
+	}
+
+	/**
+    Gets the cost between this node and the specified
+    adjacent (AKA "neighbor" or "child") node.
+	 */
+	public float getCost(AStarNode node){
+		if(pos.getX() != node.getPos().getX() && 
+				pos.getY() != node.getPos().getY()){
+			return 14;
+		}
+		return 10;
+	}
+
+
+	/**
+    Gets the estimated cost between this node and the
+    specified node. The estimated cost should never exceed
+    the true cost. The better the estimate, the more
+    effecient the search.
+	 */
+	public float getEstimatedCost(AStarNode node) {
+		int sum = 0;
+		if(pos.getX() > node.getPos().getX())
+			sum += pos.getX()-node.getPos().getX();
+		else
+			sum += node.getPos().getX()-pos.getX();
+
+		if(pos.getY() > node.getPos().getY())
+			sum += pos.getY()-node.getPos().getY();
+		else
+			sum += node.getPos().getY()-pos.getY();
+		return sum;
+	}
+
+
+	/**
+    Gets the children (AKA "neighbors" or "adjacent nodes")
+    of this node.
+	 */
+	public List getNeighbors() {
+		LinkedList<AStarNode> list = new LinkedList<AStarNode>();
+		Vector2i tempV;
+
+		//********Vertical and Horisontle
+		tempV = new Vector2i(pos.getX()+1,pos.getY());
+		if(tempV.getX() < InGameState.getMap().getSize().getX() && InGameState.getMap().isPosEmpty(tempV.getX(), tempV.getY()))
+			list.add(new AStarNode(tempV,this));
+
+		tempV = new Vector2i(pos.getX()-1,pos.getY());
+		if(tempV.getX() >= 0 && InGameState.getMap().isPosEmpty(tempV.getX(), tempV.getY()))
+			list.add(new AStarNode(tempV,this));
+
+		tempV = new Vector2i(pos.getX(),pos.getY()+1);
+		if(tempV.getY() < InGameState.getMap().getSize().getY() && InGameState.getMap().isPosEmpty(tempV.getX(), tempV.getY()))
+			list.add(new AStarNode(tempV,this));
+
+		tempV = new Vector2i(pos.getX(),pos.getY()-1);
+		if(tempV.getX() >= 0 && InGameState.getMap().isPosEmpty(tempV.getX(), tempV.getY()))
+			list.add(new AStarNode(tempV,this));
+
+		//*********Diagonal
+
+		tempV = new Vector2i(pos.getX()+1,pos.getY()+1);
+		if(tempV.getY() < InGameState.getMap().getSize().getY() &&
+				tempV.getX() < InGameState.getMap().getSize().getX() && InGameState.getMap().isPosEmpty(tempV.getX(), tempV.getY()))
+			list.add(new AStarNode(tempV,this));
+
+		tempV = new Vector2i(pos.getX()-1,pos.getY()-1);
+		if(tempV.getY() >= 0 &&	tempV.getX() >= 0 && InGameState.getMap().isPosEmpty(tempV.getX(), tempV.getY()))
+			list.add(new AStarNode(tempV,this));
+
+		tempV = new Vector2i(pos.getX()-1,pos.getY()+1);
+		if(tempV.getY() < InGameState.getMap().getSize().getY() &&
+				tempV.getX() >= 0 && InGameState.getMap().isPosEmpty(tempV.getX(), tempV.getY()))
+			list.add(new AStarNode(tempV,this));
+
+		tempV = new Vector2i(pos.getX()+1,pos.getY()-1);
+		if(tempV.getY() >= 0 &&
+				tempV.getX() < InGameState.getMap().getSize().getX() && InGameState.getMap().isPosEmpty(tempV.getX(), tempV.getY()))
+			list.add(new AStarNode(tempV,this));
+
+		return list;
+	}
+}  
\ No newline at end of file
diff --git a/src/ei/game/scene/Map.java b/src/ei/game/scene/Map.java
index f7f5b59..3ad6f92 100644
--- a/src/ei/game/scene/Map.java
+++ b/src/ei/game/scene/Map.java
@@ -34,6 +34,15 @@ public class Map {
 		}
 	}
 	
+	/**
+	 * Returns the size of the map
+	 * 
+	 * @return The size of the map
+	 */
+	public Vector2i getSize(){
+		return new Vector2i(width,hight);
+	}
+	
 	/**
 	 * Returns the pos of the map by the pixel
 	 * @param x x pixels
@@ -66,6 +75,8 @@ public class Map {
 	 * @return True if empty else false
 	 */
 	public boolean isPosEmpty(int x, int y){
+		if(x < 0 || y < 0 || x >= width || y >= hight)
+			return false;
 		if(map[x][y] == null){
 			return true;
 		}
diff --git a/src/ei/game/scene/units/Unit.java b/src/ei/game/scene/units/Unit.java
index 3d7ac4f..33b37eb 100644
--- a/src/ei/game/scene/units/Unit.java
+++ b/src/ei/game/scene/units/Unit.java
@@ -1,11 +1,15 @@
 package ei.game.scene.units;
 
 
+import java.util.LinkedList;
+
 import ei.engine.math.Vector2f;
 import ei.engine.math.Vector2i;
 import ei.engine.math.Vector3f;
 import ei.engine.scene.Node;
 import ei.engine.scene.Sprite;
+import ei.game.algo.AStar;
+import ei.game.algo.AStarNode;
 import ei.game.gamestate.InGameState;
 import ei.game.scene.GameEntity;
 import ei.game.scene.Map;
@@ -24,6 +28,7 @@ public abstract class Unit extends GameEntity{
 	// Som temp pos for moving the unit
 	private Vector2i oldPos;
 	private Vector2f moveTo;
+	private LinkedList<AStarNode> path;
 
 	/**
 	 * Creates a empty unit
@@ -76,6 +81,7 @@ public abstract class Unit extends GameEntity{
 			InGameState.getMap().removePos(oldPos.getX(), oldPos.getY());
 		}
 		setPos(x, y);
+		path = (LinkedList) new AStar().findPath(new AStarNode(oldPos,null), new AStarNode(new Vector2i(x,y),null));
 		moveTo = Map.getPixelByPos((int)x, (int)y);
 		oldPos = new Vector2i(x, y);
 	}
@@ -120,9 +126,14 @@ public abstract class Unit extends GameEntity{
 			
 			if(moveTo.getX() == unitNode.getLocation().getX() 
 					&& moveTo.getY() == unitNode.getLocation().getY()) {
-				moveTo = null;
+				if(!path.isEmpty()){
+					Vector2i temp = path.poll().getPos();
+					moveTo = Map.getPixelByPos(temp.getX(), temp.getY());
+				}
+				else{
+					moveTo = null;
+				}
 			}
 		}
 	}
-
 }