Pathfinding Implementation


/ Published in: Java
Save to your folder(s)

Implementation of the A* pathfinding algorithm


Copy this code and paste it in your HTML
  1. package jme3test.helloworld;
  2.  
  3. import com.jme3.math.Vector3f;
  4. import java.util.ArrayList;
  5.  
  6. public class PathFinder {
  7.  
  8. private boolean[][] grid;
  9. private int[][] gridF;
  10. private int gridSizeX, gridSizeY;
  11. private ArrayList<Vector3f> pathList;
  12. private float worldX, worldY;
  13.  
  14. public PathFinder(float worldXM, float worldYM, Buildings buildingsM) {
  15. worldX = worldXM;
  16. worldY = worldYM;
  17. createGrid(buildingsM);
  18. }
  19.  
  20. private void createGrid(Buildings buildings) {
  21. gridSizeX = (int) worldX * 2;
  22. gridSizeY = (int) worldY * 2;
  23. Debug dbg = new Debug();
  24. dbg.writeToFile(Integer.toString(gridSizeX));
  25. dbg.writeToFile(Integer.toString(gridSizeY));
  26. grid = new boolean[gridSizeX][gridSizeY];
  27. gridF = new int[gridSizeX][gridSizeY];
  28.  
  29. VectorToInt vti = new VectorToInt();
  30. int[] firstCorner, secondCorner;
  31. int horizontalDist;
  32.  
  33. for (int x = 0; x < buildings.getNumberOfBuildings(); x++) {
  34.  
  35. firstCorner = vti.getInt((Vector3f) buildings.getBuildings()[x][2][1]);
  36. secondCorner = vti.getInt((Vector3f) buildings.getBuildings()[x][2][2]);
  37.  
  38. horizontalDist = Math.abs((vti.getInt((Vector3f) buildings.getBuildings()[x][2][1])[0])
  39. - (vti.getInt((Vector3f) buildings.getBuildings()[x][2][0])[0]));
  40. //Back left corner - front left corner, to get distance (always a positive number)
  41.  
  42. int firstCoord, secondCoord;
  43. int firstCoordRA, secondCoordRA;
  44. int temp;
  45. firstCoord = firstCorner[2] + (int) worldY;
  46. secondCoord = secondCorner[2] + (int) worldY;
  47.  
  48. if (firstCoord > secondCoord) {
  49. temp = firstCoord;
  50. firstCoord = secondCoord;
  51. secondCoord = temp;
  52. }
  53.  
  54. firstCoordRA = firstCorner[0] + (int) worldX;
  55. secondCoordRA = (firstCorner[0] + (int) worldX) - horizontalDist;
  56.  
  57. if (firstCoordRA < secondCoordRA) {
  58. temp = firstCoordRA;
  59. firstCoordRA = secondCoordRA;
  60. secondCoordRA = temp;
  61. }
  62.  
  63. for (int n = firstCoord; n <= secondCoord; n++) {
  64. //For back left corner X values to back right corner X values
  65.  
  66. for (int i = firstCoordRA; i >= secondCoordRA; i--) {
  67. //For the length of the building
  68. dbg.writeToFile(Integer.toString(n) + " " + Integer.toString(i));
  69. grid[n][i] = true;
  70. //Set building present in grid to true
  71. }
  72. }
  73. }
  74. }
  75.  
  76. public void setPath(Vector3f start, Vector3f end) {
  77.  
  78. ArrayList<int[]> openList = new ArrayList<int[]>();
  79. ArrayList<int[]> closedList = new ArrayList<int[]>();
  80. boolean inClosedList;
  81. boolean inOpenList;
  82.  
  83. int[] currentSquare;
  84. int[] destination, startSquare, adjacentSquare;
  85. boolean nextStep = true;
  86. int gVal;
  87.  
  88.  
  89. VectorToInt vti = new VectorToInt();
  90. int startX = vti.getInt(start)[0] + (int) worldX;
  91. int startY = vti.getInt(start)[2] + (int) worldY;
  92. int endX = vti.getInt(end)[0] + (int) worldX;
  93. int endY = vti.getInt(end)[2] + (int) worldY;
  94.  
  95. startSquare = coordsArray(startX, startY, 0, 0, 0);
  96. destination = coordsArray(endX, endY, 0, 0, 0);
  97. //Change destination from vector to int coordinates (parent and gvalue are 0)
  98.  
  99. openList.add(startSquare);
  100. //Add start square to open list (parent and gvalue are 0 as unknown)
  101.  
  102.  
  103. for (int i = (startSquare[0] - 1); i <= (startSquare[0] + 1); i++) {
  104. for (int n = (startSquare[1] - 1); n <= (startSquare[1] + 1); n++) {
  105. //For the current square's neighbours
  106.  
  107. if (!(i == startSquare[0] && n == startSquare[1])) {
  108. //If it's not the start square
  109. if (!(i < 0) && !(i > (gridSizeX - 1)) && !(n < 0)
  110. && !(n > (gridSizeY - 1))) {
  111.  
  112. if (grid[i][n] == false) {
  113.  
  114. if (i != startSquare[0] && n != startSquare[1]) {
  115. //Diagonal
  116. gVal = 14;
  117. } else {
  118. //Straight line
  119. gVal = 10;
  120. }
  121.  
  122. adjacentSquare = coordsArray(i, n, startSquare[0], startSquare[1], gVal);
  123. gridF[i][n] = calcFCost(gVal, calcHeuristic(adjacentSquare, destination));
  124.  
  125. //Save F cost to current square.
  126.  
  127. openList.add(adjacentSquare);
  128.  
  129. }
  130. }
  131. }
  132. }
  133. }
  134.  
  135. closedList.add(startSquare);
  136. openList.remove(0);
  137. //Add start square to open list and remove from open list
  138.  
  139. //Calculate F points for current open list
  140. int currValue, fValue;
  141. int[] lowestSquare = new int[5];
  142. fValue = gridF[openList.get(0)[0]][openList.get(0)[1]];
  143. for (int i = 1; i < openList.size(); i++) {
  144.  
  145. currValue = gridF[openList.get(i)[0]][openList.get(i)[1]];
  146.  
  147. if (currValue < fValue) {
  148. fValue = currValue;
  149. lowestSquare = openList.get(i);
  150. }
  151.  
  152. }
  153.  
  154. closedList.add(lowestSquare);
  155. openList.remove(lowestSquare);
  156. currentSquare = lowestSquare;
  157.  
  158. while (nextStep) {
  159. //Choose new current square
  160.  
  161. for (int i = (currentSquare[0] - 1); i <= (currentSquare[0] + 1); i++) {
  162. for (int n = (currentSquare[1] - 1); n <= (currentSquare[1] + 1); n++) {
  163. //For the current square's neighbours
  164.  
  165. inClosedList = inList(closedList, i, n);
  166. inOpenList = inList(openList, i, n);
  167.  
  168. if (!(i < 0) && !(i > (gridSizeX - 1)) && !(n < 0)
  169. && !(n > (gridSizeY - 1))) {
  170.  
  171. if (grid[i][n] == false && inClosedList == false && inOpenList == false) {
  172.  
  173. if (i != currentSquare[0] && n != currentSquare[1]) {
  174. //Diagonal
  175. gVal = 14;
  176. } else {
  177. //Straight line
  178. gVal = 10;
  179. }
  180.  
  181. gVal = currentSquare[4] + gVal;
  182. //gVal = g distance + parent's gVal
  183. adjacentSquare = coordsArray(i, n, currentSquare[0], currentSquare[1], gVal);
  184. gridF[i][n] = calcFCost(gVal, calcHeuristic(adjacentSquare, destination));
  185.  
  186. //Save F cost to current square.
  187.  
  188. openList.add(adjacentSquare);
  189. //if the square is traversable, add to open list
  190. }
  191.  
  192. if (grid[i][n] == false && inOpenList == true) {
  193. //Check if existing square has shorter path available
  194. int checkGVal = 0;
  195.  
  196. if (i != currentSquare[0] && n != currentSquare[1]) {
  197. //Diagonal
  198. gVal = 14;
  199. } else {
  200. //Straight line
  201. gVal = 10;
  202. }
  203.  
  204. gVal = currentSquare[4] + gVal;
  205. //gVal = g distance + parent's gVal
  206. int temp = 0;
  207.  
  208. for (int x = 0; x < openList.size(); x++) {
  209. if (openList.get(x)[0] == i && openList.get(x)[1] == n) {
  210. checkGVal = openList.get(x)[4];
  211. temp = x;
  212. break;
  213. }
  214. }
  215.  
  216. if (gVal < checkGVal) {
  217. adjacentSquare = coordsArray(openList.get(temp)[0],
  218. openList.get(temp)[1], currentSquare[0], currentSquare[1], gVal);
  219. gridF[i][n] = calcFCost(gVal, calcHeuristic(adjacentSquare, destination));
  220. openList.remove(temp);
  221. openList.add(adjacentSquare);
  222. }
  223. }
  224. }
  225. }
  226. }
  227.  
  228.  
  229. fValue = gridF[openList.get(0)[0]][openList.get(0)[1]];
  230. for (int i = 1; i < openList.size(); i++) {
  231.  
  232. currValue = gridF[openList.get(i)[0]][openList.get(i)[1]];
  233.  
  234. if (currValue < fValue) {
  235. fValue = currValue;
  236. lowestSquare = openList.get(i);
  237. }
  238.  
  239. }
  240.  
  241. closedList.add(lowestSquare);
  242. openList.remove(lowestSquare);
  243. currentSquare = lowestSquare;
  244.  
  245.  
  246. if (currentSquare[0] == destination[0] && currentSquare[1] == destination[1]) {
  247. //If final destination reached
  248.  
  249. nextStep = false;
  250. } else if (openList.isEmpty()) {
  251. //Or target square can't be found
  252. nextStep = false;
  253. }
  254. }
  255.  
  256. setVectorList(closedList);
  257.  
  258. }
  259.  
  260. private void setVectorList(ArrayList<int[]> closedList) {
  261. Vector3f vector;
  262. float x, y;
  263. pathList = new ArrayList<Vector3f>();
  264.  
  265. for (int i = 0; i < closedList.size(); i++) {
  266. x = (float) closedList.get(i)[0] - worldX;
  267. y = (float) closedList.get(i)[1] - worldY;
  268.  
  269. vector = new Vector3f(x, 4f, y);
  270. pathList.add(vector);
  271. }
  272. }
  273.  
  274. private boolean inList(ArrayList<int[]> list, int i, int n) {
  275. boolean isOnList = false;
  276. for (int x = 0; x < list.size(); x++) {
  277. //check if it's on the closed list
  278. if (list.get(x)[0] == i && list.get(x)[1] == n) {
  279. isOnList = true;
  280. break;
  281. } else {
  282. isOnList = false;
  283. }
  284. }
  285.  
  286. return isOnList;
  287.  
  288. }
  289.  
  290. private int calcFCost(int gCost, int h) {
  291. int fCost;
  292. fCost = gCost + h;
  293. return fCost;
  294. }
  295.  
  296. private int calcHeuristic(int[] currLocation, int[] dest) {
  297. int h;
  298. h = (((Math.abs(currLocation[0] - dest[0]))
  299. + (Math.abs(currLocation[1] - dest[1]))) - 1) * 10;
  300. return h;
  301. }
  302.  
  303. private int[] coordsArray(int x, int y, int parentX, int parentY, int gVal) {
  304. int[] array = new int[5];
  305. array[0] = x;
  306. array[1] = y;
  307. array[2] = parentX;
  308. array[3] = parentY;
  309. array[4] = gVal;
  310. return array;
  311. }
  312.  
  313. public ArrayList<Vector3f> getPath() {
  314. return pathList;
  315. }
  316. }

Report this snippet


Comments

RSS Icon Subscribe to comments

You need to login to post a comment.