Advertisement
Guest User

aStar for Java

a guest
Jun 25th, 2014
301
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  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.     //Currently unused and is not run
  135.     public boolean LOSCheck(Node start, Node goal, TiledMap tiles)
  136.     {
  137.         double deltaX = goal.col - start.col;
  138.         double deltaY = goal.row - start.row;
  139.  
  140.         //determine which octant we are in
  141.         if (deltaX >= 0 && deltaY >= 0)
  142.         {
  143.             if (deltaX < deltaY)
  144.             {
  145.                 double slope = Math.abs(deltaX / deltaY);
  146.  
  147.                 double error = 0;
  148.                 double currX = start.col;
  149.  
  150.                 for (int currY = start.row; currY <= goal.row; ++currY)
  151.                 {
  152.                     //check tile
  153.                     if (tiles.isPassable((int) Math.floor(currX / tiles.tileHeight), (int) Math.floor(currY / tiles.tileHeight)))
  154.                     {
  155.                         return false;
  156.                     }
  157.  
  158.                     error += slope;
  159.                     if (error >= 0.5)
  160.                     {
  161.                         currX++;
  162.                         error -= 1.0;
  163.                     }
  164.                 }
  165.  
  166.                 return true;
  167.             }
  168.             else
  169.             {
  170.                 double slope = Math.abs(deltaY / deltaX);
  171.  
  172.                 double error = 0;
  173.                 double currY = start.row;
  174.  
  175.                 for (int currX = start.col; currX <= goal.col; ++currX)
  176.                 {
  177.                     //check tile
  178.                     if (tiles.isPassable((int) Math.floor(currX / tiles.tileHeight), (int) Math.floor(currY / tiles.tileHeight)))
  179.                     {
  180.                         return false;
  181.                     }
  182.  
  183.                     error += slope;
  184.                     if (error >= 0.5)
  185.                     {
  186.                         currY++;
  187.                         error -= 1.0;
  188.                     }
  189.                 }
  190.  
  191.                 return true;
  192.             }
  193.         } else if (deltaX < 0 && deltaY >= 0)
  194.         {
  195.             if (-deltaX < deltaY)
  196.             {
  197.                 double slope = Math.abs(-deltaX / deltaY);
  198.  
  199.                 double error = 0;
  200.                 double currX = start.col;
  201.  
  202.                 for (int currY = start.row; currY <= goal.row; ++currY)
  203.                 {
  204.                     //check tile
  205.                     if (tiles.isPassable((int) Math.floor(currX / tiles.tileHeight), (int) Math.floor(currY / tiles.tileHeight)))
  206.                     {
  207.                         return false;
  208.                     }
  209.  
  210.                     error += slope;
  211.                     if (error >= 0.5)
  212.                     {
  213.                         currX--;
  214.                         error -= 1.0;
  215.                     }
  216.                 }
  217.  
  218.                 return true;
  219.             }
  220.             else
  221.             {
  222.                 double slope = Math.abs(deltaY / -deltaX);
  223.  
  224.                 double error = 0;
  225.                 double currY = start.row;
  226.  
  227.  
  228.                 for (int currX = start.col; currX >= goal.col; --currX)
  229.                 {
  230.                     //check tile
  231.                     if (tiles.isPassable((int) Math.floor(currX / tiles.tileHeight), (int) Math.floor(currY / tiles.tileHeight)))
  232.                     {
  233.                         return false;
  234.                     }
  235.  
  236.                     error += slope;
  237.                     if (error >= 0.5)
  238.                     {
  239.                         currY++;
  240.                         error -= 1.0;
  241.                     }
  242.                 }
  243.  
  244.                 return true;
  245.             }
  246.         }
  247.         return true;
  248.     }
  249.  
  250.     //Currently unused and is not run
  251.     public void removeRedundant(Node path, TiledMap tiles)
  252.     {
  253.         Node currNode = path.parentNode;
  254.         //while there is a line of sight  from start to current node, set path parent to node and go to next node
  255.         while (currNode.parentNode != null)
  256.         {
  257.             if (LOSCheck(path, currNode, tiles))
  258.             {
  259.                 path.parentNode = currNode;
  260.             }
  261.             currNode = currNode.parentNode;
  262.         }
  263.         if (path.parentNode.parentNode != null)
  264.         {
  265.             removeRedundant(path.parentNode, tiles);
  266.         }
  267.     }
  268. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement