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 January 28, 2015

toms shoes outlet sale | Toms Outlet | toms shoes outlet

New Arrival Toms Grid Rubber sole shoes Black - $28.00 : Professional toms shoes stores, tomsshoes2u.com

sddm

{ margin: 0 auto; padding: 0; z-index: 30; background-color:#F4F4F4; width: 80px; height:23px; float: right; margin-right: 70px;}

sddm li

{ margin: 0; padding: 0; list-style: none; float: left; font: bold 12px arial}

sddm li a

{ display: block; margin: 0 1px 0 0; padding: 4px 10px; width: 60px; background: #f4762a; color: #666; text-align: center; text-decoration: none}

sddm li a:hover

{ background: #49A3FF}

sddm div

{ position: absolute; visibility: hidden; margin: 0; padding: 0; background: #EAEBD8; border: 1px solid #5970B2}

#sddm div a
{   position: relative;
    display: block;
    margin: 0;
    padding: 5px 10px;
    width: auto;
    white-space: nowrap;
    text-align: left;
    text-decoration: none;
    background: #EAEBD8;
    color: #2875DE;
    font: 12px arial}

#sddm div a:hover
{   background: #49A3FF;
    color: #FFF}
    <a href="http://www.tomsshoes2u.com/free-shipping">
     WELCOME TO TOMS SHOES STORES
    </a>













<a href="http://www.tomsshoes2u.com/"></a>

Your cart is empty

            <li><a href="http://www.tomsshoes2u.com/2013-new-arrival-toms-shoes-c-1.html">New Arrival Toms Shoes</a></li> 
    <li><a href="http://www.tomsshoes2u.com/womens-toms-shoes-c-6.html">Toms Women shoes</a></li>
    <li><a href="http://www.tomsshoes2u.com/mens-toms-shoes-c-2.html">Toms Men shoes</a></li>

Currencies

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

Categories

New Arrival Toms Shoes Womens Toms shoes Mens Toms shoes

Featured -   [more] Toms Womens shoes Black$85.00  $35.00Save: 59% offToms Womens Bamboo pattern shoes Red$85.00  $26.00Save: 69% offToms Womens Hearts Classic Shoes Black$99.00  $38.00Save: 62% offToms Womens Hearts Classic Shoes Blue$99.00  $38.00Save: 62% off

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

New Arrival Toms Shoes ::  New Arrival Toms Grid Rubber sole shoes Black

.jqzoom{

float:left;

position:relative;

padding:0px;

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

New Arrival Toms Grid Rubber sole shoes Black

$88.00  $28.00Save: 68% off

Please Choose:

Size

M10=US/CA7=UK6=EUR 40 M11=US/CA8=UK7.5=EUR 41 M12=US/CA9=UK8=EUR 42 M13=US/CA10=UK9=EUR 43 M14=US/CA11=UK10=EUR 44 M15=US/CA12=UK11=EUR 45 W10=US/CA9=UK8=EUR40 W5=US/CA5=UK3=EUR35 W6=US/CA5.5=UK4=EUR36 W7=US/CA6.5=UK5=EUR37 W8=US/CA7.5=UK6=EUR38 W9=US/CA8.5=UK7=EUR39

  Size Chart

Add to Cart:           
  • Description

Description:Looking for the freshest styles of toms shoes. Huge Selection Of Latest toms collection. Warm head, creative mind. Thats just how it works. So pull on one of these soft beanies and see what you come up with. Its going to be good.With a great philosophy that with every pair of TOMS Shoes.The toms shoes has various different kinds of series of products such as women toms shoes, toms shoes for kids, toms shoes clearance and so on.Good ventilation ensure a comfortable shoe environment.Let's go for the trends in toms & fashion. Come to find the latest styles toms shoes for you.Model:New Arrival Toms30Product categories: Canvas Rubber sole shoesFunction: warmUpper height: low helpSuitable for seasons:spring,autumn

Related Products

New Arrival Toms Women Zebra Canvas shoes linen stripe flat shoes

New Arrival Toms Grid Rubber sole shoes Red

New Arrivals Toms Women Nation Style Shoes

New Arrival Toms women shoes Red flag

&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;Help Center

    <a href="http://www.tomsshoes2u.com/index.php?main_page=shippinginfo">Order Tracking</a>
    <a href="http://www.tomsshoes2u.com/index.php?main_page=Coupons">Coupons</a>
    <a href="http://www.tomsshoes2u.com/index.php?main_page=contact_us">Contact Us</a>

   Payment & Shipping

    <a href="http://www.tomsshoes2u.com/index.php?main_page=shippinginfo">Shipping</a>
    <a href="http://www.tomsshoes2u.com/index.php?main_page=Payment_Methods">Wholesale</a>
<a href="http://www.tomsshoes2u.com/index.php?main_page=Payment_Methods">Payment Methods</a>

   Customer Service

    <a href="http://www.tomsshoes2u.com/index.php?main_page=Size">Size Chart</a>
<a href="http://www.tomsshoes2u.com/index.php?main_page=contact_us">About Us</a>





&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; Hot Sales

    <a href="http://www.lojsp.com/">Toms Women shoes</a>
    <a href="http://www.lojsp.com/">Toms Men shoes</a>
    <a href="http://www.lojsp.com/">Toms Kids Shoes</a>
    <a href="http://www.lojsp.com/">New Arrival Toms Shoes</a>

Copyright © 2012-2014 All Rights Reserved.

2014 New style toms shoes TOMS Official Outlet Store

Posted By: wilberthackel on January 28, 2015

[b][url=http://www.tomsshoes2u.com/]2014 New styl2014 New style toms shoes TOMS Official Outlet Store

Posted By: wilberthackel on January 29, 2015

Replica Watches - Rolex, Omega, Breitling Replica Luxury Watches

sddm

{ margin: 0 auto; padding: 0; z-index: 30; background-color:#F4F4F4; width: 80px; height:23px; float: right; margin-top: 4px; margin-right: 70px;}

sddm li

{ margin: 0; padding: 0; list-style: none; float: left; font: bold 12px arial}

sddm li a

{ display: block; margin: 0 1px 0 0; padding: 4px 10px; width: 60px; background: #F4F4F4; color: #D5D5D5; text-align: center; text-decoration: none}

sddm li a:hover

{ background: #49A3FF}

sddm div

{ position: absolute; visibility: hidden; margin: 0; padding: 0; background: #EAEBD8; border: 1px solid #5970B2}

#sddm div a
{   position: relative;
    display: block;
    margin: 0;
    padding: 5px 10px;
    width: auto;
    white-space: nowrap;
    text-align: left;
    text-decoration: none;
    background: #EAEBD8;
    color: #2875DE;
    font: 12px arial}

#sddm div a:hover
{   background: #49A3FF;
    color: #FFF}
    Welcome!
        <a href="http://www.otswatches.org/index.php?main_page=login">Sign In</a>
or    <a href="http://www.otswatches.org/index.php?main_page=create_account">Register</a>

Your cart is empty

<ul>



    <ul>
        <li><a href="http://www.otswatches.org/index.php">Home</a></li>
    <li><a href="http://www.otswatches.org/fake-rolex-c-2.html">Fake Rolex Watches</a></li>
    <li><a href="http://www.otswatches.org/fake-omega-c-4.html">Fake OMEGA Watches</a></li>
    <li><a href="http://www.otswatches.org/fake-cartier-c-7.html">Fake Cartier Watches</a></li>
    </ul>




</ul>

Currencies

US Dollar (USD) Euro (EUR) GB Pound (GBP) Canadian Dollar (CAD) Australian Dollar (AUD) Jappen Yen (JPY) Norske Krone (NOK) Swedish Krone (SEK) Danish Krone (DKK) CNY (CNY)

Categories

Replica Rolex Replica Omega Replica Hublot Replica Montblanc Replica Audemars Piguet Replica Cartier Replica Chopard Replica Ferrari Replica Franck Muller Replica Iwc Replica Jaeger Le Coultre Replica Longines Replica Panerai Replica Vacheron Constantin Replica Zenith Replica Bvlgari Replica Breitling Replica Bell Ross Replica Emporio Armani Replica Tag Heuer

Bestsellers

  1. Replica Rolex Datejust II Watch - Rolex Timeless Luxury Watches $21,550.00  $203.00Save: 99% off
  2. Replica Rolex Submariner Date Watch: Yellow Rolesor - combination of 904L steel and 18 ct yellow gold – M116613LB-0001 $6,871.00  $208.00Save: 97% off
  3. Replica Rolex Datejust Special Edition Watch: 18 ct Everose gold – M81315-0003 $10,596.00  $212.00Save: 98% off

Featured -   [more] Fake Vintage Rolex Yachtmaster II AAA Watches [Q2T3]$209.00Fake Vintage Rolex Submariner AAA Watches [T7G8]$204.00Fake Vintage Rolex Yachtmaster II AAA Watches [X9W8]$206.00

New Products For OctoberFake Popular Cartier Classic Quartz Rose Gold Case with Black Dial-Lady Size AAA Watches [A4F4]$204.00 Fake Popular Cartier Montre Santos Demoiselle Man Size Full Gold with White Dial AAA Watches [V1B8]$205.00 Fake Popular Cartier Pasha Automatic Diamond Bezel with Black Dial AAA Watches [Q2B1]$213.00 Fake Popular Cartier Ballon Bleu de Cartier Full Rose Gold with Double Diamond AAA Watches [B4I7]$208.00 Fake Popular Cartier Classic Diamond Bezel with White Dial AAA Watches [M7D3]$204.00 Fake Popular Cartier Ballon Bleu de Cartier Full Gold with White Dial-Ladys AAA Watches [S1I9]$204.00 Fake Popular Cartier Ballon Bleu De Cartier Full Gold With Diamond Bezel AAA Watches [E4P7]$206.00 Fake Popular Cartier Montre Santos Demoiselle Man Size Full Gold with White Dial AAA Watches [V7N8]$205.00 Fake Popular Cartier Ballon Bleu de Cartier Full Rose Gold with Diamond Bezel AAA Watches [C5O1]$213.00

Featured ProductsFake Fancy Tag Heuer Link Working Chronograph with White Dial AAA Watches [A3P4]$206.00 Fake Gorgeous Tag Heuer Carrera 50th Anniversary Of J.M Fangio's Same Chassis As Movement AAA Watches [F9A4]$231.00 Fake Fancy TAG Heuer Monaco LS Calibre 12 AAA Watches [N3C8]$210.00 Fake Fancy Tag Heuer Monaco quartz with White Dial-Rubber Strap AAA Watches [S8L6]$210.00 Fake Fancy Tag Heuer Link 200 Meters Working Chronograph Two Tone AAA Watches [H4V6]$205.00 Fake Fancy Tag Heuer Link 200 Meters Working Chronograph with Blue Dial AAA Watches [U6T4]$204.00 Fake Fancy Tag Heuer Grand Carrera Chronograph Calibre 17 RS CAV511B.FC6225 R AAA Watches [C5G1]$204.00 Fake Gorgeous Tag Heuer Aquaracer Chronograph Grand-Date R AAA Watches [G3C8]$228.00 Fake Fancy Tag Heuer Leather Strap with Deployment Buckle AAA Watches [X1L5]$209.00

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

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

    </ul>



 <ul>
    <li><a href="http://www.5watches5.com/">REPLICA OMEGA</a></li>
    <li><a href="http://www.5watches5.com/">REPLICA PATEK PHILIPPE </a></li>
    <li><a href="http://www.5watches5.com/">REPLICA ROLEX</a></li>
    <li><a href="http://www.5watches5.com/">REPLICA CARTIER</a></li>
    <li><a href="http://www.5watches5.com/">REPLICA BREITLING </a></li>
</ul>

Copyright © 2012-2014 All Rights Reserved.

swiss replica watches aaa+ swiss replica watches

Posted By: wilberthackel on January 29, 2015

[b][url=http://www.otswatches.org/]swiss replicswiss replica watches aaa+ swiss replica watches

You need to login to post a comment.