public static ArrayList<Cell> findPath(Cell start, Cell goal, Map<String, Cell> gridCells) { Set<Cell> closedSet = new HashSet<>(); Set<Cell> openSet = new HashSet<>(); openSet.add(start); start.g = 0; start.h = heuristicCostEstimate(start, goal); start.f = start.g + start.h; while (!openSet.isEmpty()) { double min = Integer.MAX_VALUE; Cell x = null; for (Cell cell : openSet) if (cell.f < min) { x = cell; min = cell.f; } if (goal.equals(x)) return reconstructPath(start, goal); openSet.remove(x); closedSet.add(x); for (Cell y : neighborNodes(x, gridCells)) { if (closedSet.contains(y)) continue; boolean tentativeIsBetter; // <tentative> double tentativeG = x.g + 2; if (x.cameFrom != null) { if (x.cameFrom.cameFrom != null && Math.abs(x.cameFrom.cameFrom.x - y.x) + Math.abs(x.cameFrom.cameFrom.y - y.y) == 1) { tentativeG += 2; } if (x.cameFrom.y == x.y && y.y != x.y || x.cameFrom.x == x.x && y.x != x.x) { tentativeG += (2 * Math.sqrt(2) - 4); } } if (y.isPenult) tentativeG += 10; // </tentative> if (!openSet.contains(y)) { openSet.add(y); tentativeIsBetter = true; } else { tentativeIsBetter = tentativeG < y.g; } if (tentativeIsBetter) { y.cameFrom = x; y.g = tentativeG; y.h = heuristicCostEstimate(y, goal); y.f = y.g + y.h; } } } return new ArrayList<>(); }