Posted By

fl0shizzle on 05/06/14


Tagged

pathfinding


Versions (?)

Pathfinding Implementation


 / Published in: Java
 

Implementation of the A* pathfinding algorithm

  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
Posted By: wilberthackel on August 3, 2015

moncler jackets | Cheap Moncler | Cheap Moncler Jackets outlet online

2014 New! Moncler Grenoble Bataillouse Jackets Mens Coffee - $270.00 : Professional Moncler Down Jacket Outlet Store, monclernewsworld.com

language:         
                                   

    Welcome!
        <a href="http://www.monclernewsworld.com/index.php?main_page=login">Sign In</a>
or    <a href="http://www.monclernewsworld.com/index.php?main_page=create_account">Register</a>

Your cart is empty


Currencies

US Dollar CNY Euro GB Pound Canadian Dollar Australian Dollar Jappen Yen Norske Krone Swedish Krone Danish Krone

Categories

Moncler 2014 New   - Moncler Men   - Moncler Women Moncler Women Coats Moncler Women Jackets Moncler Men Coats Moncler Men Jackets Moncler Vest Accessories

Featured -   [more] Moncler Jjackets Kids Down Multiple Logo Iron Grey$699.00  $229.00Save: 67% off2014 New! Moncler Grenoble Bataillouse Jackets Mens Blue$1,299.00  $270.00Save: 79% off2013 New! Moncler Tarn Quilted Men's Down Vest Fur Trimmed Hood$899.00  $239.00Save: 73% offMoncler Jjackets Kids Down Multiple Logo Black$699.00  $229.00Save: 67% off

  <a href="http://www.monclernewsworld.com/">Home</a>&nbsp;::&nbsp;

Moncler 2014 New ::    - Moncler Men ::  2014 New! Moncler Grenoble Bataillouse Jackets Mens Coffee

.jqzoom{

float:left;

position:relative;

padding:0px;

cursor:pointer; width:301px; height:300px; }

2014 New! Moncler Grenoble Bataillouse Jackets Mens Coffee

$1,299.00  $270.00Save: 79% off

Please Choose:

Men Size

2 / M / EUR48 3 / L / EUR50 4 / XL / EUR52 5 / XXL/ EUR54

Add to Cart:           
  • Description

Coffee padded jacket from Moncler Grenoble featuring a buttoned funnel neck with hood, Coffee top panel, Coffee body with horizontal stitch detailing, a single-breasted front button fastening, two vertical zip pockets, two front buttoned flap pockets and long sleeves with tabbed double-layered cuffs.

A small logo patch brands the outer sleeve. Front zip closure with outer snap placket. Elasticized cuffs. Slant pockets.Approx. length from shoulder: 24". Fully lined, with down fill. Polyamide; dry clean. By Moncler; imported coats.

Related Products

2014 New! Moncler Youri Fur-Trim Hooded Bomber Jacket Grey

2014 Newest! Moncler Beaumont Mens Camouflage Print Down Jacket

2014 New! Moncler Rouillac Mens Padded Down Jacket Blue

2014 New! Moncler Eusebe Hooded Bomber Jacket Coffee

.articles{width:900px; margin:0 auto;} .articles ul{width:900px; } .articles li{width:450px; float:left;}

    <ul>
    <li><a href="http://www.monclernewsworld.com/index.php">Home</a></li>
    <li><a href="http://www.monclernewsworld.com/index.php?main_page=shippinginfo">Shipping</a></li>
    <li><a href="http://www.monclernewsworld.com/index.php?main_page=Payment_Methods">Wholesale</a></li>
    <li><a href="http://www.monclernewsworld.com/index.php?main_page=shippinginfo">Order Tracking</a></li>
    <li><a href="http://www.monclernewsworld.com/index.php?main_page=Coupons">Coupons</a></li>
    <li><a href="http://www.monclernewsworld.com/index.php?main_page=Payment_Methods">Payment Methods</a></li>
    <li><a href="http://www.monclernewsworld.com/index.php?main_page=contact_us">Contact Us</a></li>      
<li><a href="http://www.monclernewsworld.com/index.php?main_page=Size">Size Chart</a></li>
    </ul>



 <ul>
    <li><a href="http://www.monclerpaschere.co/">Moncler Men Coats</a></li>
    <li><a href="http://www.monclerpaschere.co/">Moncler Men Jackets</a></li>
    <li><a href="http://www.monclerpaschere.co/">Moncler Women Coats</a></li>
    <li><a href="http://www.monclerpaschere.co/">Moncler Women Jackets</a></li>
    <li><a href="http://www.monclerpaschere.co/">Moncler Vest</a></li>
</ul>

Copyright © 2012-2014 All Rights Reserved.

moncler sale moncler outlet store

Posted By: wilberthackel on August 3, 2015

[b][url=http://www.monclernewsworld.com/]monclmoncler sale moncler outlet store

You need to login to post a comment.