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