@Override
public boolean find() {
try {
PathNode root = new PathNode();
root.point = start;
calculateTotalCost(root, start, start, false);
expand(root);
int iterations = 0;
while (true) {
PathNode p = paths.remove();
if (p == null) {
clear();
return false;
}
Point last = p.point;
if (isGoal(last)) {
calculatePath(p); // Iterate back.
clear();
return true;
}
expand(p);
++iterations;
if (iterations > 20000) {
Messaging.log("took too long");
return false;
}
}
} catch (Exception e) {
e.printStackTrace();
}
clear();
return false;
}
private int calculateTotalCost(PathNode p, Point from, Point to, boolean endPoint) {
int g = (calculateHeuristic(from, to, endPoint) + ((p.parent != null) ? p.parent.cost : 0));
int h = calculateHeuristic(from, to, endPoint);
p.cost = g;
p.totalCost = (g + h);
return p.totalCost;
}
private void expand(PathNode path) {
Point p = path.point;
Integer min = mindists.get(p);
if (min == null || min > path.totalCost) {
mindists.put(p, path.totalCost);
} else {
return;
}
Point[] successors = generateSuccessors(path);
for (Point t : successors) {
if (t == null) {
continue;
}
PathNode newPath = new PathNode(path);
newPath.point = t;
calculateTotalCost(newPath, p, t, false);
paths.add(newPath);
}
}