Alright, so I've run into a problem where when trying to find a path that you cannot reach because it is blocked, it will continually loop and loop and loop (the 'open' list is ALWAYS filled). I understand the open set cannot contain things that the closed set contains. I cannot understand where the open set is getting things from the closed set.
I make sure to check if the neighbor is within the closed set, so it shouldn't be added to the open set. I'm confused...
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());
}
}
```