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 November 29, 2014

high quality replica watches for men watches swiss Mechanical movement replica watches

Panerai Luminor Replica Watch Marina Unitas 6497 Movement Pvd Case Ar Coating - $276.00 : Professional replica watches stores, mensbraceletwatches.us

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: #BABABA; 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.mensbraceletwatches.us/index.php?main_page=login">Sign In</a>
or    <a href="http://www.mensbraceletwatches.us/index.php?main_page=create_account">Register</a>

Your cart is empty

<ul>



    <ul>
        <li><a href="http://www.mensbraceletwatches.us/index.php">Home</a></li>
    <li><a href="http://www.mensbraceletwatches.us/replica-rolex-watches-c-44.html">Replica Rolex Watches</a></li>
    <li><a href="http://www.mensbraceletwatches.us/replica-omega-watches-c-24.html">Replica OMEGA Watches</a></li>
    <li><a href="http://www.mensbraceletwatches.us/replica-tag-heuer-watches-c-59.html">Replica Tag Heuer 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

Omega Watches Vacheron Constantin U-Boat Watches Tag Heuer Watches Rolex Watches Rado Watches Piaget Watches Patek Philippe Watches Panerai Watches Panerai Ferrari Panerai Luminor Panerai Others Panerai Radiomir Longines Watches Hublot Watches Emporio Armani Watches Bell&Ross Watches Audemars Piguet A.Lange&Sohne Watches IWC Watches

Featured -   [more] Emporio Armani Replica Watch Classic Rose Gold Case Black Dial$213.00Emporio Armani Replica Watch Classic Roman Markers White Dial Lady Size$221.00Emporio Armani Replica Watch Classic Rose Gold Case Black Dial Couple Replica Watch$216.00

  <a href="http://www.mensbraceletwatches.us/">Home</a>&nbsp;::&nbsp;

Panerai Watches ::  Panerai Luminor ::  Panerai Luminor Replica Watch Marina Unitas 6497 Movement Pvd Case Ar Coating

.jqzoom{

float:left;

position:relative;

padding:0px;

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

Panerai Luminor Replica Watch Marina Unitas 6497 Movement Pvd Case Ar Coating

$276.00

Add to Cart:           
  • Description

Welcome to replica watches outlet stores, the site for all your replica watches needs. The internet is full of vendors and sites trying to sell you replica watches and it isn't always easy finding the most reliable sites. We guarantee the best services with the best replica watches online. replica watches are everywhere, and it's important that you're getting the best available on the market today.

                            <p>Panerai,a Florentine watch maker,is the maker of luxury watches and is well known throughout the world by watch enthusiasts.Panerai watches are world renowned and stand the test of time.</p> <p></p> <p>Top quality Asia Unitas 6497 Movement, Manual Winding (17 Jewel)High quality PVD CasingHigh quality genuine Leather Strap with PVD BuckleSapphire Crystal Glass Face with Anti-Reflective CoatingCase Diameter:44 mmWater- Resistant</p>                                                                                           

Related Products

Panerai Luminor Replica Watch Marina Unitas 6497 Movement Black Dial Ar Coating

Panerai Luminor Replica Watch Pam 089 Titanium Antracite Marina Gmt Movement Tita

Panerai Luminor Replica Watch Marina Manual Winding Titanium Case Wit

Panerai Luminor Replica Watch Marina Swan Neck Ar Coating With Black

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

Home   Shipping   Wholesale   Order Tracking   Coupons   Payment Methods   Contact Us  

    <a href="http://www.newbizpacks.com/replica-omega-watches-c-4.html">REPLICA OMEGA</a> &nbsp;&nbsp;
    <a href="http://www.newbizpacks.com/replica-patek-philippe-c-24.html">REPLICA PATEK PHILIPPE </a> &nbsp;&nbsp;
    <a href="http://www.newbizpacks.com/replica-rolex-watches-c-3.html">REPLICA ROLEX </a> &nbsp;&nbsp;
    <a href="http://www.newbizpacks.com/replica-iwc-watches-c-7.html">REPLICA IWC </a>&nbsp;&nbsp;
    <a href="http://www.newbizpacks.com/replica-cartier-watches-c-16.html">REPLICA CARTIER </a>&nbsp;&nbsp;
    <a href="http://www.newbizpacks.com/replica-breitling-c-2.html">REPLICA BREITLING </a>&nbsp;&nbsp;

Copyright © 2012-2014 All Rights Reserved.

swiss replica watches aaa+ swiss replica watches

Posted By: wilberthackel on November 29, 2014

[b][url=http://www.mensbraceletwatches.us/]swiss repswiss replica watches aaa+ swiss replica watches

Posted By: wilberthackel on November 29, 2014

Tiffany And Co Venetian Link Bracelet - $62.00 : Professional tiffany outlet stores, tiffany1837outlet.com

language:         
                                   

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

Your cart is empty

    <li><a href="http://www.tiffany1837outlet.com/">Home</a></li>
            <li><a href="http://www.tiffany1837outlet.com/tiffany-amp-co-bracelets-c-2.html">Tiffany &amp Co Bracelets</a></li>
            <li><a href="http://www.tiffany1837outlet.com/tiffany-amp-co-rings-c-9.html">Tiffany &amp Co Rings</a></li>
            <li><a href="http://www.tiffany1837outlet.com/tiffany-amp-co-earrings-c-6.html">Tiffany &amp Co Earrings</a></li>

Currencies

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

Categories

Tiffany &amp Co Bangles Tiffany &amp Co Bracelets Tiffany &amp Co Chains Tiffany &amp Co Charms Tiffany &amp Co Cuff Link Tiffany &amp Co Earrings Tiffany &amp Co Necklace Tiffany &amp Co Pendant Tiffany &amp Co Rings Tiffany &amp Co Sets

Featured -   [more] Tiffany Outlet Dice Drop Toggle Bracelet$344.00  $62.00Save: 82% offTiffany Outlet Charm Bead Black Toggle Set$572.00  $103.00Save: 82% offTiffany & Co outlet Leaf Silver Earrings$448.00  $61.00Save: 86% offTiffany & Co Outlet Keys Quatrefoil Key Pendant$311.00  $65.00Save: 79% off

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

Tiffany &amp Co Bracelets ::  Tiffany And Co Venetian Link Bracelet

.jqzoom{

float:left;

position:relative;

padding:0px;

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

Tiffany And Co Venetian Link Bracelet

$241.00  $62.00Save: 74% off

Add to Cart:           
  • Description

Tiffany Co Bracelets give everyone a unique visual experience. The innovative and lovely design makes it be loved by everyone. You can wear it in any occation.Your own personal Tiffany letter. Every Tiffany Bracelets is special and unique, and you can never find the same one at the whole party. Our Tiffany Co Outlet are likely to satisfy your all desires. Moreover. What are you waiting for? Please have a visit to our online store. Welcome!Product Features:Name:Tiffany And Co Venetian Link BraceletMaterial: 925 sterling silverGuarantee: Top Quality Guarantee,100% Satisfaction Guarantee.Manufacturer: Tiffany Co OutletPackage: All of our Discount Tiffany Jewellery comes with Tiffany bag,a set of Tiffany pouch, a gift Tiffany box, Tiffany care silver card and Tiffany polishing cloth.Tiffany Outlet Recommends :Tiffany And Co Venetian Link BraceletThe old retro style compromises the modern fashion elements and is full of a sense of retro design with smart delication to create personality and gentle classical temperament. you could feel the fashion perfeclty. Tiffany Outlet carefully introduce this retro necklace style .Looking forward to meet with you and bring your unique qualities.

Brand story:    Since 1837, the masterpieces of Tiffany & Co. have defined style and celebrated the world’s great love stories.

    Tiffany published its first Blue Book catalogue in 1845. This annual presentation of flawless craftsmanship and peerless design heralds the fall season with one of the most extensive and exquisite collections of couture jewelry on earth. These breathtaking masterpieces of exceedingly rare gems are eagerly anticipated by the world’s jewelry connoisseurs who flock to Tiffany to be the first to see and buy these one-of-a-kind treasures.

    Tiffany has always been the leader in exploring new materials and set the standard of purity for sterling silver and platinum in the U.S. In 2012, Tiffany’s RUBEDO? metal honored the company’s 175th anniversary. Capturing the light of dawn, its beauty truly glows on the skin.

Related Products

Tiffany And Co Heart Chain Bracelet

Return To Tiffany & Co Outlet Heart Tag and Box Drop Bracelet

Tiffany And Co Middle Heart Bracelet

Tiffany And Co Dog Tag Toggle Bracelet

    <a href="http://www.tiffanyandco2u.com/">TIFFANY JEWELRY</a> &nbsp;&nbsp;
    <a href="http://www.tiffanyandco2u.com/">TIFFANY IMITATE</a> &nbsp;&nbsp;
    <a href="http://www.tiffanyandco2u.com/">TIFFANY DISCOUNT RING</a> &nbsp;&nbsp;
    <a href="http://www.tiffanyandco2u.com/">TIFFANY CHEAP STOER</a> &nbsp;&nbsp;
    <a href="http://www.tiffanyandco2u.com/">TIFFANY HIGH IMITATE</a>&nbsp;&nbsp;

Copyright © 2012-2014 All Rights Reserved.

tiffany jewelry tiffany & co

Posted By: wilberthackel on November 29, 2014

[b][url=http://www.tiffany1837outlet.com/]tiftiffany jewelry tiffany & co

You need to login to post a comment.