001 import java.io.*;
002 import java.lang.*;
003 import java.util.*;
004
005 /**
006 * An implementation of the A* algoritm. It is goal directed and only returns
007 * full paths.
008 */
009 public class AStar {
010
011 /**
012 * The main method.
013 * @param world Nodelist containing a world
014 * @param nGoal The goal node
015 * @param pStartInit the start node
016 * @return A NodeList with Nodes forming the path
017 */
018 public static NodeList run(NodeList world, Node nGoal, Node nStartInit) {
019 System.out.println("***In AStar");
020 Node nStart = new Node();
021 nStart = world.get(world.nodeExist(nStartInit.getX(), nStartInit.getY()));
022 nStart.setH(getManhattanDist(nStart, nGoal));
023 Node n = new Node();
024
025 //System.out.println("Start: " + nStart);
026 //System.out.println("Goal: " + nGoal);
027
028 NodeList openList = new NodeList();
029 NodeList closedList = new NodeList();
030 NodeList neighbourList = new NodeList();
031 NodeList path = new NodeList();
032
033 openList.add(nStart);
034
035 while(openList.isNotEmpty()){
036 n = openList.popNodeWithLowestF();
037 closedList.add(n);
038 if(nGoal.isPointEqual(n)){
039 path.add(n);
040 fetchFamily(n, nStart, path);
041 break;
042 }
043 neighbourList = getNeighbours(world, n, nGoal);
044 for(int i = 0; i < neighbourList.size();i++){
045 if(openList.beenThereDoneThat(neighbourList.get(i))){
046 continue;
047 }
048 if(closedList.beenThereDoneThat(neighbourList.get(i))){
049 continue;
050 }
051 openList.removeEqualPoints(neighbourList.get(i));
052 closedList.removeEqualPoints(neighbourList.get(i));
053 openList.add(neighbourList.get(i));
054 }
055 }
056 //System.out.println("PATH: " + path);
057 return path;
058 }
059
060 /**
061 * Recursive method that fetches all the nodes, starting
062 * with the goal and following the parents.
063 */
064 public static void fetchFamily(Node n, Node nStart, NodeList path){
065 if(!n.isPointEqual(nStart)){
066 path.add(n.getParent());
067 fetchFamily(n.getParent(),nStart,path);
068 }
069 }
070
071 /**
072 * Calculates the manhattan-distance between two nodes
073 */
074 public static int getManhattanDist(Node nA, Node nB) {
075 return (int)( Math.abs(nA.getX() - nB.getX()) + Math.abs(nA.getY() - nB.getY()) );
076 }
077
078 /**
079 * Makes a NodeList holding all the neighbour Nodes using 4-connectivity.
080 * @param world Nodelist containing a world
081 * @param nCurrent The currentnode
082 * @param nGoal the goal node
083 */
084 public static NodeList getNeighbours(NodeList world, Node nCurrent, Node nGoal){
085 NodeList neighbourList = new NodeList();
086 Node[] anTemp = new Node[4];
087
088 int iX = nCurrent.getX();
089 int iY = nCurrent.getY();
090 int iG = nCurrent.getG();
091
092 anTemp[0] = new Node(iX,iY+1);
093 anTemp[1] = new Node(iX,iY-1);
094 anTemp[2] = new Node(iX+1,iY);
095 anTemp[3] = new Node(iX-1,iY);
096
097 int location;
098 for(int i = 0; i < 4; i++) {
099 location = world.nodeExist(anTemp[i].getX(), anTemp[i].getY());
100 if(location >= 0){
101 if ( ((anTemp[i].getY() > iY) && (nCurrent.getTop() == 0)) ||
102 ((anTemp[i].getX() > iX) && (nCurrent.getRight() == 0)) ||
103 ((anTemp[i].getY() < iY) && (nCurrent.getBottom() == 0)) ||
104 ((anTemp[i].getX() < iX) && (nCurrent.getLeft() == 0))
105 ) {
106 Node temp = new Node();
107 temp = (Node)world.get(location);
108 anTemp[i].setG(iG+1);
109 anTemp[i].setH(getManhattanDist(anTemp[i], nGoal));
110 anTemp[i].setParent(nCurrent);
111 anTemp[i].setTop(temp.getTop());
112 anTemp[i].setRight(temp.getRight());
113 anTemp[i].setBottom(temp.getBottom());
114 anTemp[i].setLeft(temp.getLeft());
115 neighbourList.add(anTemp[i]);
116 }
117 }
118 }
119 //System.out.println("Neighbour of " + nCurrent + ": " + neighbourList);
120 return neighbourList;
121 }
122 }