fork download
copy 
  1. import java.util.*;
  2.  
  3. /**
  4.  * A Star Algorithm
  5.  *
  6.  * @author Marcelo Surriabre
  7.  * @version 2.1, 2017-02-23
  8.  */
  9. public class Main {
  10.  
  11. 	public static void main(String[] args) {
  12.         Node initialNode = new Node(2, 1);
  13.         Node finalNode = new Node(2, 5);
  14.         int rows = 6;
  15.         int cols = 7;
  16.         AStar aStar = new AStar(rows, cols, initialNode, finalNode);
  17.         int[][] blocksArray = new int[][]{{1, 3}, {2, 3}, {3, 3}};
  18.         aStar.setBlocks(blocksArray);
  19.         List<Node> path = aStar.findPath();
  20.         //a path with intermediate nodes removed
  21.         List<Node> filteredPath = new ArrayList<>(path.size());
  22.         for(int i=0; i<path.size(); i++) {
  23.             Node current = path.get(i);
  24.             //the first and the last element get into the result in any case
  25.             if(i==0 || i==path.size()-1) {
  26.                 filteredPath.add(current);
  27.             } else {
  28.                 //for the elements in between we are detecting the direction change
  29.                 Node previous = path.get(i-1);
  30.                 Node next = path.get(i+1);
  31.                 //is the step from the previous node to this one vertical
  32.                 boolean isPreviousStepVertical = current.getCol()==previous.getCol();
  33.                 //is the step from this node to the next one vertical
  34.                 boolean isNextStepVertical = current.getCol()==next.getCol();
  35.                 //we only add the nodes for which the direction has changed
  36.                 if(isPreviousStepVertical!=isNextStepVertical) {
  37.                     filteredPath.add(current);
  38.                 }
  39.             } 
  40.         }
  41.         System.out.println("Filtered result");
  42.         for (Node node : filteredPath) {
  43.             System.out.println(node);
  44.         }
  45.     }
  46.  
  47. }
  48.  
  49. class AStar {
  50.  
  51.     private static int DEFAULT_HV_COST = 10; // Horizontal - Vertical Cost
  52.     private static int DEFAULT_DIAGONAL_COST = 14;
  53.     private int hvCost;
  54.     private int diagonalCost;
  55.     private Node[][] searchArea;
  56.     private PriorityQueue<Node> openList;
  57.     private Set<Node> closedSet;
  58.     private Node initialNode;
  59.     private Node finalNode;
  60.  
  61.     public AStar(int rows, int cols, Node initialNode, Node finalNode, int hvCost, int diagonalCost) {
  62.         this.hvCost = hvCost;
  63.         this.diagonalCost = diagonalCost;
  64.         setInitialNode(initialNode);
  65.         setFinalNode(finalNode);
  66.         this.searchArea = new Node[rows][cols];
  67.         this.openList = new PriorityQueue<Node>(new Comparator<Node>() {
  68.             @Override
  69.             public int compare(Node node0, Node node1) {
  70.                 return Integer.compare(node0.getF(), node1.getF());
  71.             }
  72.         });
  73.         setNodes();
  74.         this.closedSet = new HashSet<>();
  75.     }
  76.  
  77.     public AStar(int rows, int cols, Node initialNode, Node finalNode) {
  78.         this(rows, cols, initialNode, finalNode, DEFAULT_HV_COST, DEFAULT_DIAGONAL_COST);
  79.     }
  80.  
  81.     private void setNodes() {
  82.         for (int i = 0; i < searchArea.length; i++) {
  83.             for (int j = 0; j < searchArea[0].length; j++) {
  84.                 Node node = new Node(i, j);
  85.                 node.calculateHeuristic(getFinalNode());
  86.                 this.searchArea[i][j] = node;
  87.             }
  88.         }
  89.     }
  90.  
  91.     public void setBlocks(int[][] blocksArray) {
  92.         for (int i = 0; i < blocksArray.length; i++) {
  93.             int row = blocksArray[i][0];
  94.             int col = blocksArray[i][1];
  95.             setBlock(row, col);
  96.         }
  97.     }
  98.  
  99.     public List<Node> findPath() {
  100.         openList.add(initialNode);
  101.         while (!isEmpty(openList)) {
  102.             Node currentNode = openList.poll();
  103.             closedSet.add(currentNode);
  104.             if (isFinalNode(currentNode)) {
  105.                 return getPath(currentNode);
  106.             } else {
  107.                 addAdjacentNodes(currentNode);
  108.             }
  109.         }
  110.         return new ArrayList<Node>();
  111.     }
  112.  
  113.     private List<Node> getPath(Node currentNode) {
  114.         List<Node> path = new ArrayList<Node>();
  115.         path.add(currentNode);
  116.         Node parent;
  117.         while ((parent = currentNode.getParent()) != null) {
  118.             path.add(0, parent);
  119.             currentNode = parent;
  120.         }
  121.         return path;
  122.     }
  123.  
  124.     private void addAdjacentNodes(Node currentNode) {
  125.         addAdjacentUpperRow(currentNode);
  126.         addAdjacentMiddleRow(currentNode);
  127.         addAdjacentLowerRow(currentNode);
  128.     }
  129.  
  130.     private void addAdjacentLowerRow(Node currentNode) {
  131.         int row = currentNode.getRow();
  132.         int col = currentNode.getCol();
  133.         int lowerRow = row + 1;
  134.         if (lowerRow < getSearchArea().length) {
  135.             if (col - 1 >= 0) {
  136.                 //checkNode(currentNode, col - 1, lowerRow, getDiagonalCost()); // Comment this line if diagonal movements are not allowed
  137.             }
  138.             if (col + 1 < getSearchArea()[0].length) {
  139.                 //checkNode(currentNode, col + 1, lowerRow, getDiagonalCost()); // Comment this line if diagonal movements are not allowed
  140.             }
  141.             checkNode(currentNode, col, lowerRow, getHvCost());
  142.         }
  143.     }
  144.  
  145.     private void addAdjacentMiddleRow(Node currentNode) {
  146.         int row = currentNode.getRow();
  147.         int col = currentNode.getCol();
  148.         int middleRow = row;
  149.         if (col - 1 >= 0) {
  150.             checkNode(currentNode, col - 1, middleRow, getHvCost());
  151.         }
  152.         if (col + 1 < getSearchArea()[0].length) {
  153.             checkNode(currentNode, col + 1, middleRow, getHvCost());
  154.         }
  155.     }
  156.  
  157.     private void addAdjacentUpperRow(Node currentNode) {
  158.         int row = currentNode.getRow();
  159.         int col = currentNode.getCol();
  160.         int upperRow = row - 1;
  161.         if (upperRow >= 0) {
  162.             if (col - 1 >= 0) {
  163.                 //checkNode(currentNode, col - 1, upperRow, getDiagonalCost()); // Comment this if diagonal movements are not allowed
  164.             }
  165.             if (col + 1 < getSearchArea()[0].length) {
  166.                 //checkNode(currentNode, col + 1, upperRow, getDiagonalCost()); // Comment this if diagonal movements are not allowed
  167.             }
  168.             checkNode(currentNode, col, upperRow, getHvCost());
  169.         }
  170.     }
  171.  
  172.     private void checkNode(Node currentNode, int col, int row, int cost) {
  173.         Node adjacentNode = getSearchArea()[row][col];
  174.         if (!adjacentNode.isBlock() && !getClosedSet().contains(adjacentNode)) {
  175.             if (!getOpenList().contains(adjacentNode)) {
  176.                 adjacentNode.setNodeData(currentNode, cost);
  177.                 getOpenList().add(adjacentNode);
  178.             } else {
  179.                 boolean changed = adjacentNode.checkBetterPath(currentNode, cost);
  180.                 if (changed) {
  181.                     // Remove and Add the changed node, so that the PriorityQueue can sort again its
  182.                     // contents with the modified "finalCost" value of the modified node
  183.                     getOpenList().remove(adjacentNode);
  184.                     getOpenList().add(adjacentNode);
  185.                 }
  186.             }
  187.         }
  188.     }
  189.  
  190.     private boolean isFinalNode(Node currentNode) {
  191.         return currentNode.equals(finalNode);
  192.     }
  193.  
  194.     private boolean isEmpty(PriorityQueue<Node> openList) {
  195.         return openList.size() == 0;
  196.     }
  197.  
  198.     private void setBlock(int row, int col) {
  199.         this.searchArea[row][col].setBlock(true);
  200.     }
  201.  
  202.     public Node getInitialNode() {
  203.         return initialNode;
  204.     }
  205.  
  206.     public void setInitialNode(Node initialNode) {
  207.         this.initialNode = initialNode;
  208.     }
  209.  
  210.     public Node getFinalNode() {
  211.         return finalNode;
  212.     }
  213.  
  214.     public void setFinalNode(Node finalNode) {
  215.         this.finalNode = finalNode;
  216.     }
  217.  
  218.     public Node[][] getSearchArea() {
  219.         return searchArea;
  220.     }
  221.  
  222.     public void setSearchArea(Node[][] searchArea) {
  223.         this.searchArea = searchArea;
  224.     }
  225.  
  226.     public PriorityQueue<Node> getOpenList() {
  227.         return openList;
  228.     }
  229.  
  230.     public void setOpenList(PriorityQueue<Node> openList) {
  231.         this.openList = openList;
  232.     }
  233.  
  234.     public Set<Node> getClosedSet() {
  235.         return closedSet;
  236.     }
  237.  
  238.     public void setClosedSet(Set<Node> closedSet) {
  239.         this.closedSet = closedSet;
  240.     }
  241.  
  242.     public int getHvCost() {
  243.         return hvCost;
  244.     }
  245.  
  246.     public void setHvCost(int hvCost) {
  247.         this.hvCost = hvCost;
  248.     }
  249.  
  250.     private int getDiagonalCost() {
  251.         return diagonalCost;
  252.     }
  253.  
  254.     private void setDiagonalCost(int diagonalCost) {
  255.         this.diagonalCost = diagonalCost;
  256.     }
  257. }
  258.  
  259.  
  260. class Node {
  261.  
  262.     private int g;
  263.     private int f;
  264.     private int h;
  265.     private int row;
  266.     private int col;
  267.     private boolean isBlock;
  268.     private Node parent;
  269.  
  270.     public Node(int row, int col) {
  271.         super();
  272.         this.row = row;
  273.         this.col = col;
  274.     }
  275.  
  276.     public void calculateHeuristic(Node finalNode) {
  277.         this.h = Math.abs(finalNode.getRow() - getRow()) + Math.abs(finalNode.getCol() - getCol());
  278.     }
  279.  
  280.     public void setNodeData(Node currentNode, int cost) {
  281.         int gCost = currentNode.getG() + cost;
  282.         setParent(currentNode);
  283.         setG(gCost);
  284.         calculateFinalCost();
  285.     }
  286.  
  287.     public boolean checkBetterPath(Node currentNode, int cost) {
  288.         int gCost = currentNode.getG() + cost;
  289.         if (gCost < getG()) {
  290.             setNodeData(currentNode, cost);
  291.             return true;
  292.         }
  293.         return false;
  294.     }
  295.  
  296.     private void calculateFinalCost() {
  297.         int finalCost = getG() + getH();
  298.         setF(finalCost);
  299.     }
  300.  
  301.     @Override
  302.     public boolean equals(Object arg0) {
  303.         Node other = (Node) arg0;
  304.         return this.getRow() == other.getRow() && this.getCol() == other.getCol();
  305.     }
  306.  
  307.     @Override
  308.     public String toString() {
  309.         return "Node [row=" + row + ", col=" + col + "]";
  310.     }
  311.  
  312.     public int getH() {
  313.         return h;
  314.     }
  315.  
  316.     public void setH(int h) {
  317.         this.h = h;
  318.     }
  319.  
  320.     public int getG() {
  321.         return g;
  322.     }
  323.  
  324.     public void setG(int g) {
  325.         this.g = g;
  326.     }
  327.  
  328.     public int getF() {
  329.         return f;
  330.     }
  331.  
  332.     public void setF(int f) {
  333.         this.f = f;
  334.     }
  335.  
  336.     public Node getParent() {
  337.         return parent;
  338.     }
  339.  
  340.     public void setParent(Node parent) {
  341.         this.parent = parent;
  342.     }
  343.  
  344.     public boolean isBlock() {
  345.         return isBlock;
  346.     }
  347.  
  348.     public void setBlock(boolean isBlock) {
  349.         this.isBlock = isBlock;
  350.     }
  351.  
  352.     public int getRow() {
  353.         return row;
  354.     }
  355.  
  356.     public void setRow(int row) {
  357.         this.row = row;
  358.     }
  359.  
  360.     public int getCol() {
  361.         return col;
  362.     }
  363.  
  364.     public void setCol(int col) {
  365.         this.col = col;
  366.     }
  367. }
Success #stdin #stdout 0.16s 36604KB
comments (?)
stdin
copy 
Standard input is empty
stdout
copy 
Filtered result
Node [row=2, col=1]
Node [row=2, col=2]
Node [row=0, col=2]
Node [row=0, col=4]
Node [row=2, col=4]
Node [row=2, col=5]
https://ideone.com/77MAxg
language:
Java (HotSpot 12)
created:
6 years ago
visibility:
public

Share or Embed source code

Discover > Sphere Engine API

The brand new service which powers Ideone!

Discover > IDE Widget

Widget for compiling and running the source code in a web browser!