public static LinkedList<Node> findPath(World world, Node start, Node goal) {
        final PriorityQueue<Node> open = new PriorityQueue<>();
        final HashSet<Node> closed = new HashSet<>();

        open.add(start.setHeuristic(start.heuristic(goal)));
        Node current = null;
        Node best = start;
        while (!open.isEmpty()) {
            current = open.poll();
            if (current.equals(goal))
                return construct(start, current);
            closed.add(current);
            Tile tile = world.getTile(current.getX(), current.getY(), current.getZ());
            ArrayList<Tile> neighbors = tile.getNeighbors();
            for (Tile n : neighbors) {
                if (n == null)
                    continue;
                Node neighbor = new Node(n.getGlobalX(), n.getGlobalY(), n.getZ(), current);
                neighbor.setCost(neighbor.distance(current));
                neighbor.setHeuristic(neighbor.heuristic(goal));
                if (closed.contains(neighbor))
                    continue;
                closed.add(neighbor);
                if (!n.isWalkable())
                    continue;
                if (neighbor.equals(goal))
                    return construct(start, neighbor);
                double tentative_cost = current.getCost() + current.distance(neighbor);
                if (!open.contains(neighbor) || tentative_cost < neighbor.getCost()) {
                    best = neighbor;
                    neighbor.setCost(tentative_cost);
                    open.add(neighbor);
                }
            }
        }
        return construct(start, best);
    }

    private static LinkedList<Node> construct(Node start, Node goal) {
        LinkedList<Node> path = new LinkedList<>();
        Node last = goal;
        while (!last.equals(start)) {
            path.addFirst(last);
            last = last.getParent();
        }
        path.addFirst(start);
        return path;
    }
```

    public static LinkedList<Node> findPath(World world, Node start, Node goal) {
        final PriorityQueue<Node> open = new PriorityQueue<>();
        final HashSet<Node> closed = new HashSet<>();

        open.add(start.setHeuristic(start.heuristic(goal)));
        Node current = null;
        Node best = start;
        while (!open.isEmpty()) {
            current = open.poll();
            if (current.equals(goal))
                return construct(start, current);
            closed.add(current);
            Tile tile = world.getTile(current.getX(), current.getY(), current.getZ());
            ArrayList<Tile> neighbors = tile.getNeighbors();
            for (Tile n : neighbors) {
                if (n == null)
                    continue;
                Node neighbor = new Node(n.getGlobalX(), n.getGlobalY(), n.getZ(), current);
                neighbor.setCost(neighbor.distance(current));
                neighbor.setHeuristic(neighbor.heuristic(goal));
                if (closed.contains(neighbor))
                    continue;
                closed.add(neighbor);
                if (!n.isWalkable())
                    continue;
                if (neighbor.equals(goal))
                    return construct(start, neighbor);
                double tentative_cost = current.getCost() + current.distance(neighbor);
                if (!open.contains(neighbor) || tentative_cost < neighbor.getCost()) {
                    best = neighbor;
                    neighbor.setCost(tentative_cost);
                    open.add(neighbor);
                }
            }
        }
        return construct(start, best);
    }

    private static LinkedList<Node> construct(Node start, Node goal) {
        LinkedList<Node> path = new LinkedList<>();
        Node last = goal;
        while (!last.equals(start)) {
            path.addFirst(last);
            last = last.getParent();
        }
        path.addFirst(start);
        return path;
    }

Node class

package com.entity.world.path;

import java.util.Objects;

/**
 * @author Albert Beaupre
 * 
 */
public class Node implements Comparable<Node> {

    private final int x, y, z;
    private final Node parent;
    private double cost, heuristic;

    public Node(int x, int y, int z) {
        this.x = x;
        this.y = y;
        this.z = z;
        parent = this;
    }

    public Node(int x, int y, int z, Node parent) {
        this.x = x;
        this.y = y;
        this.z = z;
        this.parent = parent;
    }

    public double distance(Node from) {
        double dx = Math.pow(Math.abs(from.x - x), 2);
        double dy = Math.pow(Math.abs(from.y - y), 2);
        return Math.sqrt(dx + dy);
    }

    public int getX() {
        return x;
    }

    public int getY() {
        return y;
    }

    public int getZ() {
        return z;
    }

    public Node getParent() {
        return parent;
    }

    public double getCost() {
        return cost;
    }

    public Node setCost(double cost) {
        this.cost = cost;
        return this;
    }

    public double getHeuristic() {
        return heuristic;
    }

    public Node setHeuristic(double heuristic) {
        this.heuristic = heuristic;
        return this;
    }

    public double getF() {
        return cost + heuristic;
    }

    public double heuristic(Node goal) {
        double dx = Math.abs(x - goal.x);
        double dy = Math.abs(y - goal.y);
        return Math.sqrt(dx + dy);
    }

    @Override
    public int hashCode() {
        return Objects.hash(x, y, z);
    }

    /**
     * {@inheritDoc}
     */
    public boolean equals(Object o) {
        if (o instanceof Node) {
            Node n = (Node) o;
            return x == n.x && y == n.y && z == n.z;
        }
        return false;
    }

    /**
     * {@inheritDoc}
     */
    public String toString() {
        return "[x=" + x + ", y=" + y + "]";
    }

    /*
     * (non-Javadoc)
     * 
     * @see java.lang.Comparable#compareTo(java.lang.Object)
     */
    public int compareTo(Node o) {
        return Double.compare(getF(), o.getF());
    }

}
```