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;
    }
}

Implementation of the A* pathfinding algorithm

Pathfinding Implementation

Java