Question

My problem is that right now my A* algorithm in java can find the path only if it goes from top down and from left to right. I want the code to be able to check top bottom left right before deciding where to move not only bottom and right. Can you guys help me? This is my code

public class PathFinder extends AStar<PathFinder.Node>{
            private int[][] map;

            public static class Node{
                            public int x;
                            public int y;
                            Node(int x, int y){
                                            this.x = x; 
                                            this.y = y;
                            }
                            public String toString(){
                                            return "(" + x + ", " + y + ") ";
                            } 
            }
            public PathFinder(int[][] map){
                            this.map = map;
            }

            protected boolean isGoal(Node node){
                            return (node.x == map[0].length - 1) && (node.y == map.length - 1);
            }

            protected Double g(Node from, Node to){

                            if(from.x == to.x && from.y == to.y)
                                            return 0.0;

                            if(map[to.y][to.x] == 1)
                                            return 1.0;

                            return Double.MAX_VALUE;
            }

            protected Double h(Node from, Node to){
                            /* Use the Manhattan distance heuristic.  */
                            return new Double(Math.abs(map[0].length - 1 - to.x) + Math.abs(map.length - 1 - to.y));
            }

            protected List<Node> generateSuccessors(Node node){
                            List<Node> ret = new LinkedList<Node>();
                            int x = node.x;
                            int y = node.y;
                            if(y < map.length - 1 && map[y+1][x] == 1)
                                            ret.add(new Node(x, y+1));

                            if(x < map[0].length - 1 && map[y][x+1] == 1)
                                            ret.add(new Node(x+1, y));


                            return ret;
            }

            public static void main(String [] args){
                            int [][] map = new int[][]{
                                            {1, 0, 1, 0, 1, 0 ,1 ,0, 1, 0 ,1, 0},
                                            {1, 1, 1, 1, 1, 1, 1 ,1 ,1 ,1, 1 ,1},
                                            {1, 1, 1, 0, 1, 1 ,1, 1, 1 ,0, 1 ,0},
                                            {0, 1, 1, 1, 1, 1 ,0, 1, 0, 1, 1 ,1},
                                            {0, 0, 1, 1, 1, 0 ,1, 1 ,1, 1, 0 ,0},
                                            {0, 1, 0, 1, 0, 0, 1, 1, 1, 1 ,1, 1},
                                            {1, 1, 1, 1, 1, 1, 1 ,0 ,1 ,0, 1, 0},
                                            {1, 0, 0, 0, 1, 0 ,1, 1 ,1 ,0 ,1 ,1},
                                            {1, 1, 0, 1, 1, 0 ,1 ,0 ,1, 1, 1, 0},
                                            {1, 1, 1, 1, 0, 1, 1 ,1 ,0 ,1, 1 ,1},
                                            {1, 0, 1, 1, 1, 1, 0, 1, 1 ,0 ,1, 0},
                                            {1, 0, 1, 0, 1, 1, 0 ,1, 0, 1, 1 ,1}
                            };
                            PathFinder pf = new PathFinder(map);

                            System.out.println("Find a path from the top left corner to the right bottom one.");

                            for(int i = 0; i < map.length; i++){
                                            for(int j = 0; j < map[0].length; j++)
                                                            System.out.print(map[i][j] + " ");
                                            System.out.println();
                            }

                            long begin = System.currentTimeMillis();

                            List<Node> nodes = pf.compute(new PathFinder.Node(0,0));

                            long end = System.currentTimeMillis();


                            System.out.println("Time = " + (end - begin) + " ms" );
                            System.out.println("Expanded = " + pf.getExpandedCounter());
                            System.out.println("Cost = " + pf.getCost());

                            if(nodes == null)
                                            System.out.println("No path");
                            else{
                                            System.out.print("Path = ");
                                            for(Node n : nodes)
                                                            System.out.print(n);
                                            System.out.println();
                            }
            }

  }

and this is the other class

public abstract class AStar<T>{
            private class Path implements Comparable{
                            public T point;
                            public Double f;
                            public Double g;
                            public Path parent;

                            /**
                             * Default c'tor.
                             */
                            public Path(){
                                            parent = null;
                                            point = null;
                                            g = f = 0.0;
                            }

                            /**
                             * C'tor by copy another object.
                             * 
                             * @param p The path object to clone.
                             */
                            public Path(Path p){
                                            this();
                                            parent = p;
                                            g = p.g;
                                            f = p.f;
                            }

                            /**
                             * Compare to another object using the total cost f.
                             *
                             * @param o The object to compare to.
                             * @see       Comparable#compareTo()
                             * @return <code>less than 0</code> This object is smaller
                             * than <code>0</code>;
                             *        <code>0</code> Object are the same.
                             *        <code>bigger than 0</code> This object is bigger
                             * than o.
                             */
                            public int compareTo(Object o){
                                            Path p = (Path)o;
                                            return (int)(f - p.f);
                            }

                            /**
                             * Get the last point on the path.
                             *
                             * @return The last point visited by the path.
                             */
                            public T getPoint(){
                                            return point;
                            }

                            /**
                             * Set the 
                             */
                            public void setPoint(T p){
                                            point = p;
                            }
            }

            /**
             * Check if the current node is a goal for the problem.
             *
             * @param node The node to check.
             * @return <code>true</code> if it is a goal, <code>false</else> otherwise.
             */
            protected abstract boolean isGoal(T node);

            /**
             * Cost for the operation to go to <code>to</code> from
             * <code>from</from>.
             *
             * @param from The node we are leaving.
             * @param to The node we are reaching.
             * @return The cost of the operation.
             */
            protected abstract Double g(T from, T to);

            /**
             * Estimated cost to reach a goal node.
             * An admissible heuristic never gives a cost bigger than the real
             * one.
             * <code>from</from>.
             *
             * @param from The node we are leaving.
             * @param to The node we are reaching.
             * @return The estimated cost to reach an object.
             */
            protected abstract Double h(T from, T to);


            /**
             * Generate the successors for a given node.
             *
             * @param node The node we want to expand.
             * @return A list of possible next steps.
             */
            protected abstract List<T> generateSuccessors(T node);


            private PriorityQueue<Path> paths;
            private HashMap<T, Double> mindists;
            private Double lastCost;
            private int expandedCounter;

            /**
             * Check how many times a node was expanded.
             *
             * @return A counter of how many times a node was expanded.
             */
            public int getExpandedCounter(){
                            return expandedCounter;
            }

            /**
             * Default c'tor.
             */
            public AStar(){
                            paths = new PriorityQueue<Path>();
                            mindists = new HashMap<T, Double>();
                            expandedCounter = 0;
                            lastCost = 0.0;
            }


            /**
             * Total cost function to reach the node <code>to</code> from
             * <code>from</code>.
             *  
             * The total cost is defined as: f(x) = g(x) + h(x).
             * @param from The node we are leaving.
             * @param to The node we are reaching.
             * @return The total cost.
             */
            protected Double f(Path p, T from, T to){
                            Double g =  g(from, to) + ((p.parent != null) ? p.parent.g : 0.0);
                            Double h = h(from, to);

                            p.g = g;
                            p.f = g + h;

                            return p.f;
            }

            /**
             * Expand a path.
             *
             * @param path The path to expand.
             */
            private void expand(Path path){
                            T p = path.getPoint();
                            Double min = mindists.get(path.getPoint());

                            /*
                             * If a better path passing for this point already exists then
                             * don't expand it.
                             */
                            if(min == null || min.doubleValue() > path.f.doubleValue())
                                            mindists.put(path.getPoint(), path.f);
                            else
                                            return;

                            List<T> successors = generateSuccessors(p);

                            for(T t : successors){
                                            Path newPath = new Path(path);
                                            newPath.setPoint(t);
                                            f(newPath, path.getPoint(), t);
                                            paths.offer(newPath);
                            }

                            expandedCounter++;
            }

            /**
             * Get the cost to reach the last node in the path.
             *
             * @return The cost for the found path.
             */
            public Double getCost(){
                            return lastCost;
            }


            /**
             * Find the shortest path to a goal starting from
             * <code>start</code>.
             *
             * @param start The initial node.
             * @return A list of nodes from the initial point to a goal,
             * <code>null</code> if a path doesn't exist.
             */
            public List<T> compute(T start){
                            try{
                                            Path root = new Path();
                                            root.setPoint(start);

                                            /* Needed if the initial point has a cost.  */
                                            f(root, start, start);

                                            expand(root);

                                            for(;;){
                                                            Path p = paths.poll();

                                                            if(p == null){
                                                                            lastCost = Double.MAX_VALUE;
                                                                            return null;
                                                            }

                                                            T last = p.getPoint();

                                                            lastCost = p.g;

                                                            if(isGoal(last)){
                                                                            LinkedList<T> retPath = new LinkedList<T>();

                                                                            for(Path i = p; i != null; i = i.parent){
                                                                                            retPath.addFirst(i.getPoint());
                                                                            }

                                                                            return retPath;
                                                            }
                                                            expand(p);
                                            }
                            }
                            catch(Exception e){
                                            e.printStackTrace();
                            }
                            return null;

            }
              }
Was it helpful?

Solution

You will need to:

  • change generateSuccessors to also add Node(x-1,y) and Node(x, y-1) to the list
  • change expand to maintain set of visited nodes to make sure you don't visit a node twice.
Licensed under: CC-BY-SA with attribution
Not affiliated with StackOverflow
scroll top