PathFinder.java

import java.util.ArrayList;
import java.util.Collections;

public class PathFinder
{
    private static final int COST_NORM = 10;
    private static final int COST_DIAG = 14;

    private int[][] map;
    private Node[][] nodeMap;

    private Node endNode;

    NodeList openList = new NodeList();
    NodeList closedList = new NodeList();


    public PathFinder(int[][] map)
    {
        this.map = map;
        nodeMap = getNodes(map.length, map[0].length);
    }

    public void findPath(Node start, Node end)
    {
        System.out.println();
        openList.clear();
        closedList.clear();

        openList.add(start);

        Node currentNode = null;
        endNode = end;

        while (!openList.isEmpty())
        {

            currentNode = openList.getSmallest();
            //openList.remove(currentNode);
            closedList.add(currentNode);


            if (closedList.contains(end))
            {
                System.out.println("Path Found");
                return;
            }

            ArrayList<Movement> movements = getSuccessors(currentNode);
            for (Movement movement : movements)
            {
                Node node = movement.node;

                if (movement == null)
                    break;

                if (closedList.contains(node))
                    continue;

                if (isWalkable(node))
                {
                    if (!openList.contains(node))
                    {
                        node.parentNode = currentNode;
                        node.totalCost = movement.getTotalCost();
                        node.movementCost = movement.movementCost;
                        openList.add(node);
                    }
                }
            }
        }
        System.out.println("Path Not Found");
    }

    public ArrayList<Node> getPath()
    {
        ArrayList<Node> path = new ArrayList();
        Node node = closedList.getLast();
        path.add(node);
        while ((node = node.parentNode) != null)
        {
            path.add(node);
        }
        // path.add (startNode);
        Collections.reverse(path);
        return path;
    }

    public void displayPath()
    {
        for (Node n : getPath())
        {
            System.out.print("(" + n.xPosition + "," + n.yPosition + ")   ");
        }
    }

    /**
     * * Returns true if the Tile represented by this node is walkable * * @param
     * node * @return
     */
    boolean isWalkable(Node node)
    {
        return map[node.yPosition][node.xPosition] == 1;
    }

    /**
     * * Gets the Nodes surrounding the current node that can be walked on * @param
     * currentNode * @return
     */
    public ArrayList<Movement> getSuccessors(Node currentNode)
    {
        ArrayList<Movement> movements = new ArrayList();
        int x = currentNode.xPosition;
        int y = currentNode.yPosition;
        Node node = null;
        int heuristicDistance;
        int g = 0;

        if (x > 0) // LEFT
        {
            node = nodeMap[y][x - 1];
            g = currentNode.movementCost + COST_NORM;
            heuristicDistance = calculateHeuristic(node);
            movements.add(new Movement(node, g, heuristicDistance));
        }
        if (x < nodeMap[y].length - 1) // RIGHT
        {
            node = nodeMap[y][x + 1];
            g = currentNode.movementCost + COST_NORM;
            heuristicDistance = calculateHeuristic(node);
            movements.add(new Movement(node, g, heuristicDistance));
        }
        if (y > 0)
        {
            if (x > 0) // DIAGONAL Top-left
            {
                node = nodeMap[y - 1][x - 1];
                g = currentNode.movementCost + COST_DIAG;
                heuristicDistance = calculateHeuristic(node);
                movements.add(new Movement(node, g, heuristicDistance));
            }
            if (x < map[y].length - 1) // DIAGONAL Top-Right
            {
                node = nodeMap[y - 1][x + 1];
                g = currentNode.movementCost + COST_DIAG;
                heuristicDistance = calculateHeuristic(node);
                movements.add(new Movement(node, g, heuristicDistance));
            }
            node = nodeMap[y - 1][x]; // UP
            g = currentNode.movementCost + COST_NORM;
            heuristicDistance = calculateHeuristic(node);
            movements.add(new Movement(node, g, heuristicDistance));
        }
        if (y < map.length - 1)
        {
            if (x > 0) // DIAGONAL Bottom-left
            {
                node = nodeMap[y + 1][x - 1];
                g = currentNode.movementCost + COST_DIAG;
                heuristicDistance = calculateHeuristic(node);
                movements.add(new Movement(node, g, heuristicDistance));
            }
            if (x < map[y].length - 1) // DIAGONAL Bottom-Right
            {
                node = nodeMap[x + 1][y + 1];
                g = currentNode.movementCost + COST_DIAG;
                heuristicDistance = calculateHeuristic(node);
                movements.add(new Movement(node, g, heuristicDistance));
            }
            node = nodeMap[y + 1][x]; // DOWN
            g = currentNode.movementCost + COST_NORM;
            heuristicDistance = calculateHeuristic(node);
            movements.add(new Movement(node, g, heuristicDistance));
        }
        return movements;
    }

    public int calculateHeuristic(Node cNode)
    { // MANHATTAN
        return Math.abs((cNode.xPosition - endNode.xPosition)
                + (cNode.yPosition - endNode.yPosition));
        // EULIDEAN
        // return (int) Math.sqrt(Math.pow((cNode.xPosition -
        // endNode.xPosition), 2) + Math.pow((cNode.yPosition -
        // endNode.yPosition), 2));
    }

    private Node[][] getNodes(int height, int width)
    {
        Node[][] nodes = new Node[width][height];
        for (int w = 0; w < nodes.length; w++)
        {
            for (int h = 0; h < nodes[w].length; h++)
            {
                nodes[w][h] = new Node(w, h);
            }
        }
        return nodes;
    }
}