Advertisement
Guest User

aStar for Java

a guest
Jun 25th, 2014
58
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Java 8.09 KB | None | 0 0
  1. import java.util.ArrayList;
  2. import java.util.List;
  3. import java.util.Collections;
  4.  
  5. public class Pathfinding
  6. {
  7.     List<Node> closedList = new ArrayList<Node>();
  8.     List<Node> openList = new ArrayList<Node>();
  9.     int j = 0;
  10.  
  11.     public Node aStar(TiledMap tiles, Node start, Node goal)
  12.     {
  13.  
  14.         Node currentNode = new Node(start.row, start.col, start.gCost, start.fCost, null);
  15.        // closedList.clear();
  16.         //openList.clear();
  17.  
  18.         while (!reachedGoal(currentNode, goal))
  19.         {
  20.             int row = currentNode.row;
  21.             int col = currentNode.col;
  22.  
  23.             //right child
  24.             col++;
  25.             addChild(row, col, tiles, currentNode, goal);
  26.  
  27.             //left child
  28.             col -= 2;
  29.             addChild(row, col, tiles, currentNode, goal);
  30.  
  31.             //top child
  32.             col++;
  33.             row--;
  34.             addChild(row, col, tiles, currentNode, goal);
  35.  
  36.             //bottom child
  37.             row += 2;
  38.             addChild(row, col, tiles, currentNode, goal);
  39.  
  40.             //bottom right
  41.             col++;
  42.             addChild(row, col, tiles, currentNode, goal);
  43.  
  44.             //bottom left
  45.             col -= 2;
  46.             addChild(row, col, tiles, currentNode, goal);
  47.  
  48.             //top left
  49.             row -= 2;
  50.             addChild(row, col, tiles, currentNode, goal);
  51.  
  52.             //top right
  53.             col += 2;
  54.             addChild(row, col, tiles, currentNode, goal);
  55.  
  56.             //Put currentNode in the closedList
  57.             closedList.add(currentNode);
  58.             //Sort the openList
  59.             Collections.sort(openList);
  60.             //Assign currentNode to the last element in the List
  61.             currentNode = openList.remove(openList.size() - 1);
  62.             //System.out.println("Curr Node Row " +  currentNode.row + ", Curr Node Col " + currentNode.col);
  63.         }
  64.         return currentNode;
  65.     }
  66.  
  67.     public boolean reachedGoal(Node currentNode, Node goalNode)
  68.     {
  69.         return (currentNode.col == goalNode.col) && (currentNode.row == goalNode.row);
  70.     }
  71.  
  72.     public boolean isNodeClosed(double row, double col)
  73.     {
  74.         for (int i = 0; i < closedList.size(); ++i)
  75.         {
  76.             if (closedList.get(i).col == col && closedList.get(i).row == row)
  77.             {
  78.                 return true;
  79.             }
  80.         }
  81.         return false;
  82.     }
  83.  
  84.     public Node getChildFromOpen(double row, double col, List<Node> openList)
  85.     {
  86.         for (int i = 0; i < openList.size(); ++i)
  87.         {
  88.             if (openList.get(i).col == col && openList.get(i).row == row)
  89.             {
  90.                 return openList.get(i);
  91.             }
  92.         }
  93.         return null;
  94.     }
  95.  
  96.     public void addChild(int row, int col, TiledMap tiles, Node currentNode, Node target)
  97.     {
  98.         if((row >= 0 && col >= 0) && (row <= 14 && col <= 39))
  99.         {
  100.             if (tiles.isPassable(row, col))
  101.             {
  102.                 if (!isNodeClosed(row, col))
  103.                 {
  104.                     double g = currentNode.gCost + getDistanceFromParent(row, col, currentNode);
  105.                     double f = g + getDistance(row, col, target);
  106.                     Node child = getChildFromOpen(row, col, openList);
  107.  
  108.                     if (child == null)
  109.                     {
  110.                         child = new Node(row, col, g, f, currentNode);
  111.                         openList.add(child);
  112.                     }
  113.                     else if (child.gCost > g)
  114.                     {
  115.                         child.fCost = f;
  116.                         child.gCost = g;
  117.                         child.parentNode = currentNode;
  118.                     }
  119.                 }
  120.             }
  121.         }
  122.     }
  123.  
  124.     public double getDistance(int row, int col, Node goal)
  125.     {
  126.         return Math.sqrt((goal.row - row) * (goal.row - row) + (goal.col - col) * (goal.col - col));
  127.     }
  128.  
  129.     public double getDistanceFromParent(int row, int col, Node parent)
  130.     {
  131.         return Math.sqrt((row - parent.row) * (row - parent.row) + (col - parent.col) * (col - parent.col));
  132.     }
  133.  
  134.     public boolean LOSCheck(Node start, Node goal, TiledMap tiles)
  135.     {
  136.         double deltaX = goal.col - start.col;
  137.         double deltaY = goal.row - start.row;
  138.  
  139.         //determine which octant we are in
  140.         if (deltaX >= 0 && deltaY >= 0)
  141.         {
  142.             if (deltaX < deltaY)
  143.             {
  144.                 double slope = Math.abs(deltaX / deltaY);
  145.  
  146.                 double error = 0;
  147.                 double currX = start.col;
  148.  
  149.                 for (int currY = start.row; currY <= goal.row; ++currY)
  150.                 {
  151.                     //check tile
  152.                     if (tiles.isPassable((int) Math.floor(currX / tiles.tileHeight), (int) Math.floor(currY / tiles.tileHeight)))
  153.                     {
  154.                         return false;
  155.                     }
  156.  
  157.                     error += slope;
  158.                     if (error >= 0.5)
  159.                     {
  160.                         currX++;
  161.                         error -= 1.0;
  162.                     }
  163.                 }
  164.  
  165.                 return true;
  166.             }
  167.             else
  168.             {
  169.                 double slope = Math.abs(deltaY / deltaX);
  170.  
  171.                 double error = 0;
  172.                 double currY = start.row;
  173.  
  174.                 for (int currX = start.col; currX <= goal.col; ++currX)
  175.                 {
  176.                     //check tile
  177.                     if (tiles.isPassable((int) Math.floor(currX / tiles.tileHeight), (int) Math.floor(currY / tiles.tileHeight)))
  178.                     {
  179.                         return false;
  180.                     }
  181.  
  182.                     error += slope;
  183.                     if (error >= 0.5)
  184.                     {
  185.                         currY++;
  186.                         error -= 1.0;
  187.                     }
  188.                 }
  189.  
  190.                 return true;
  191.             }
  192.         } else if (deltaX < 0 && deltaY >= 0)
  193.         {
  194.             if (-deltaX < deltaY)
  195.             {
  196.                 double slope = Math.abs(-deltaX / deltaY);
  197.  
  198.                 double error = 0;
  199.                 double currX = start.col;
  200.  
  201.                 for (int currY = start.row; currY <= goal.row; ++currY)
  202.                 {
  203.                     //check tile
  204.                     if (tiles.isPassable((int) Math.floor(currX / tiles.tileHeight), (int) Math.floor(currY / tiles.tileHeight)))
  205.                     {
  206.                         return false;
  207.                     }
  208.  
  209.                     error += slope;
  210.                     if (error >= 0.5)
  211.                     {
  212.                         currX--;
  213.                         error -= 1.0;
  214.                     }
  215.                 }
  216.  
  217.                 return true;
  218.             }
  219.             else
  220.             {
  221.                 double slope = Math.abs(deltaY / -deltaX);
  222.  
  223.                 double error = 0;
  224.                 double currY = start.row;
  225.  
  226.  
  227.                 for (int currX = start.col; currX >= goal.col; --currX)
  228.                 {
  229.                     //check tile
  230.                     if (tiles.isPassable((int) Math.floor(currX / tiles.tileHeight), (int) Math.floor(currY / tiles.tileHeight)))
  231.                     {
  232.                         return false;
  233.                     }
  234.  
  235.                     error += slope;
  236.                     if (error >= 0.5)
  237.                     {
  238.                         currY++;
  239.                         error -= 1.0;
  240.                     }
  241.                 }
  242.  
  243.                 return true;
  244.             }
  245.         }
  246.         return true;
  247.     }
  248.  
  249.     public void removeRedundant(Node path, TiledMap tiles)
  250.     {
  251.         Node currNode = path.parentNode;
  252.         //while there is a line of sight  from start to current node, set path parent to node and go to next node
  253.         while (currNode.parentNode != null)
  254.         {
  255.             if (LOSCheck(path, currNode, tiles))
  256.             {
  257.                 path.parentNode = currNode;
  258.             }
  259.             currNode = currNode.parentNode;
  260.         }
  261.         if (path.parentNode.parentNode != null)
  262.         {
  263.             removeRedundant(path.parentNode, tiles);
  264.         }
  265.     }
  266. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement