Revision: 66437
Initial Code
Initial URL
Initial Description
Initial Title
Initial Tags
Initial Language
at May 6, 2014 20:44 by fl0shizzle
Initial Code
package jme3test.helloworld; import com.jme3.math.Vector3f; import java.util.ArrayList; public class PathFinder { private boolean[][] grid; private int[][] gridF; private int gridSizeX, gridSizeY; private ArrayList<Vector3f> pathList; private float worldX, worldY; public PathFinder(float worldXM, float worldYM, Buildings buildingsM) { worldX = worldXM; worldY = worldYM; createGrid(buildingsM); } private void createGrid(Buildings buildings) { gridSizeX = (int) worldX * 2; gridSizeY = (int) worldY * 2; Debug dbg = new Debug(); dbg.writeToFile(Integer.toString(gridSizeX)); dbg.writeToFile(Integer.toString(gridSizeY)); grid = new boolean[gridSizeX][gridSizeY]; gridF = new int[gridSizeX][gridSizeY]; VectorToInt vti = new VectorToInt(); int[] firstCorner, secondCorner; int horizontalDist; for (int x = 0; x < buildings.getNumberOfBuildings(); x++) { firstCorner = vti.getInt((Vector3f) buildings.getBuildings()[x][2][1]); secondCorner = vti.getInt((Vector3f) buildings.getBuildings()[x][2][2]); horizontalDist = Math.abs((vti.getInt((Vector3f) buildings.getBuildings()[x][2][1])[0]) - (vti.getInt((Vector3f) buildings.getBuildings()[x][2][0])[0])); //Back left corner - front left corner, to get distance (always a positive number) int firstCoord, secondCoord; int firstCoordRA, secondCoordRA; int temp; firstCoord = firstCorner[2] + (int) worldY; secondCoord = secondCorner[2] + (int) worldY; if (firstCoord > secondCoord) { temp = firstCoord; firstCoord = secondCoord; secondCoord = temp; } firstCoordRA = firstCorner[0] + (int) worldX; secondCoordRA = (firstCorner[0] + (int) worldX) - horizontalDist; if (firstCoordRA < secondCoordRA) { temp = firstCoordRA; firstCoordRA = secondCoordRA; secondCoordRA = temp; } for (int n = firstCoord; n <= secondCoord; n++) { //For back left corner X values to back right corner X values for (int i = firstCoordRA; i >= secondCoordRA; i--) { //For the length of the building dbg.writeToFile(Integer.toString(n) + " " + Integer.toString(i)); grid[n][i] = true; //Set building present in grid to true } } } } public void setPath(Vector3f start, Vector3f end) { ArrayList<int[]> openList = new ArrayList<int[]>(); ArrayList<int[]> closedList = new ArrayList<int[]>(); boolean inClosedList; boolean inOpenList; int[] currentSquare; int[] destination, startSquare, adjacentSquare; boolean nextStep = true; int gVal; VectorToInt vti = new VectorToInt(); int startX = vti.getInt(start)[0] + (int) worldX; int startY = vti.getInt(start)[2] + (int) worldY; int endX = vti.getInt(end)[0] + (int) worldX; int endY = vti.getInt(end)[2] + (int) worldY; startSquare = coordsArray(startX, startY, 0, 0, 0); destination = coordsArray(endX, endY, 0, 0, 0); //Change destination from vector to int coordinates (parent and gvalue are 0) openList.add(startSquare); //Add start square to open list (parent and gvalue are 0 as unknown) for (int i = (startSquare[0] - 1); i <= (startSquare[0] + 1); i++) { for (int n = (startSquare[1] - 1); n <= (startSquare[1] + 1); n++) { //For the current square's neighbours if (!(i == startSquare[0] && n == startSquare[1])) { //If it's not the start square if (!(i < 0) && !(i > (gridSizeX - 1)) && !(n < 0) && !(n > (gridSizeY - 1))) { if (grid[i][n] == false) { if (i != startSquare[0] && n != startSquare[1]) { //Diagonal gVal = 14; } else { //Straight line gVal = 10; } adjacentSquare = coordsArray(i, n, startSquare[0], startSquare[1], gVal); gridF[i][n] = calcFCost(gVal, calcHeuristic(adjacentSquare, destination)); //Save F cost to current square. openList.add(adjacentSquare); } } } } } closedList.add(startSquare); openList.remove(0); //Add start square to open list and remove from open list //Calculate F points for current open list int currValue, fValue; int[] lowestSquare = new int[5]; fValue = gridF[openList.get(0)[0]][openList.get(0)[1]]; for (int i = 1; i < openList.size(); i++) { currValue = gridF[openList.get(i)[0]][openList.get(i)[1]]; if (currValue < fValue) { fValue = currValue; lowestSquare = openList.get(i); } } closedList.add(lowestSquare); openList.remove(lowestSquare); currentSquare = lowestSquare; while (nextStep) { //Choose new current square for (int i = (currentSquare[0] - 1); i <= (currentSquare[0] + 1); i++) { for (int n = (currentSquare[1] - 1); n <= (currentSquare[1] + 1); n++) { //For the current square's neighbours inClosedList = inList(closedList, i, n); inOpenList = inList(openList, i, n); if (!(i < 0) && !(i > (gridSizeX - 1)) && !(n < 0) && !(n > (gridSizeY - 1))) { if (grid[i][n] == false && inClosedList == false && inOpenList == false) { if (i != currentSquare[0] && n != currentSquare[1]) { //Diagonal gVal = 14; } else { //Straight line gVal = 10; } gVal = currentSquare[4] + gVal; //gVal = g distance + parent's gVal adjacentSquare = coordsArray(i, n, currentSquare[0], currentSquare[1], gVal); gridF[i][n] = calcFCost(gVal, calcHeuristic(adjacentSquare, destination)); //Save F cost to current square. openList.add(adjacentSquare); //if the square is traversable, add to open list } if (grid[i][n] == false && inOpenList == true) { //Check if existing square has shorter path available int checkGVal = 0; if (i != currentSquare[0] && n != currentSquare[1]) { //Diagonal gVal = 14; } else { //Straight line gVal = 10; } gVal = currentSquare[4] + gVal; //gVal = g distance + parent's gVal int temp = 0; for (int x = 0; x < openList.size(); x++) { if (openList.get(x)[0] == i && openList.get(x)[1] == n) { checkGVal = openList.get(x)[4]; temp = x; break; } } if (gVal < checkGVal) { adjacentSquare = coordsArray(openList.get(temp)[0], openList.get(temp)[1], currentSquare[0], currentSquare[1], gVal); gridF[i][n] = calcFCost(gVal, calcHeuristic(adjacentSquare, destination)); openList.remove(temp); openList.add(adjacentSquare); } } } } } fValue = gridF[openList.get(0)[0]][openList.get(0)[1]]; for (int i = 1; i < openList.size(); i++) { currValue = gridF[openList.get(i)[0]][openList.get(i)[1]]; if (currValue < fValue) { fValue = currValue; lowestSquare = openList.get(i); } } closedList.add(lowestSquare); openList.remove(lowestSquare); currentSquare = lowestSquare; if (currentSquare[0] == destination[0] && currentSquare[1] == destination[1]) { //If final destination reached nextStep = false; } else if (openList.isEmpty()) { //Or target square can't be found nextStep = false; } } setVectorList(closedList); } private void setVectorList(ArrayList<int[]> closedList) { Vector3f vector; float x, y; pathList = new ArrayList<Vector3f>(); for (int i = 0; i < closedList.size(); i++) { x = (float) closedList.get(i)[0] - worldX; y = (float) closedList.get(i)[1] - worldY; vector = new Vector3f(x, 4f, y); pathList.add(vector); } } private boolean inList(ArrayList<int[]> list, int i, int n) { boolean isOnList = false; for (int x = 0; x < list.size(); x++) { //check if it's on the closed list if (list.get(x)[0] == i && list.get(x)[1] == n) { isOnList = true; break; } else { isOnList = false; } } return isOnList; } private int calcFCost(int gCost, int h) { int fCost; fCost = gCost + h; return fCost; } private int calcHeuristic(int[] currLocation, int[] dest) { int h; h = (((Math.abs(currLocation[0] - dest[0])) + (Math.abs(currLocation[1] - dest[1]))) - 1) * 10; return h; } private int[] coordsArray(int x, int y, int parentX, int parentY, int gVal) { int[] array = new int[5]; array[0] = x; array[1] = y; array[2] = parentX; array[3] = parentY; array[4] = gVal; return array; } public ArrayList<Vector3f> getPath() { return pathList; } }
Initial URL
Initial Description
Implementation of the A* pathfinding algorithm
Initial Title
Pathfinding Implementation
Initial Tags
Initial Language
Java