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