private ArrayList<Location> tracePath(PathFindBuffers.GeoNode f) {
ArrayList<Location> locations = new ArrayList<Location>();
do {
locations.add(0, f.getLoc());
f = f.parent;
} while (f.parent != null);
return locations;
}
public List<Location> findPath() {
startNode =
buff.nodes[startPoint.x - buff.offsetX][startPoint.y - buff.offsetY].set(
startPoint.x, startPoint.y, (short) startPoint.z);
GeoEngine.NgetHeightAndNSWE(startPoint.x, startPoint.y, (short) startPoint.z, hNSWE, geoIndex);
startNode.z = hNSWE[0];
startNode.nswe = hNSWE[1];
startNode.costFromStart = 0f;
startNode.state = GeoNode.OPENED;
startNode.parent = null;
endNode =
buff.nodes[endPoint.x - buff.offsetX][endPoint.y - buff.offsetY].set(
endPoint.x, endPoint.y, (short) endPoint.z);
startNode.costToEnd = pathCostEstimate(startNode);
startNode.totalCost = startNode.costFromStart + startNode.costToEnd;
buff.open.add(startNode);
long nanos = System.nanoTime();
long searhTime = 0;
int itr = 0;
while ((searhTime = System.nanoTime() - nanos) < Config.PATHFIND_MAX_TIME
&& (currentNode = buff.open.poll()) != null) {
itr++;
if (currentNode.x == endPoint.x
&& currentNode.y == endPoint.y
&& Math.abs(currentNode.z - endPoint.z) < 64) {
path = tracePath(currentNode);
break;
}
handleNode(currentNode);
currentNode.state = GeoNode.CLOSED;
}
buff.totalTime += searhTime;
buff.totalItr += itr;
if (path != null) buff.successUses++;
else if (searhTime > Config.PATHFIND_MAX_TIME) buff.overtimeUses++;
return path;
}
