// TODO reverse sign private boolean isInValidPoint(Point point) { return (point.getX() < 0) || (point.getY() < 0) || (point.getX() >= context.getWidth()) || (point.getY() >= context.getHeight()) || (!context.isTraversable(point)); }
/** Implements method from {@link IPathFinder}. */ public List<Point> findPath(Point start, Point end) { assertPointIsInBounds(start); assertPointIsInBounds(end); PriorityQueue<PathNode> openSet = new PriorityQueue<PathNode>(); Map<Point, PathNode> openSetLookupMap = new HashMap<Point, PathNode>(); Set<Point> closedSet = new HashSet<Point>(); PathNode startNode = new PathNode(null, start, 0, context.h(start, end)); openSet.add(startNode); openSetLookupMap.put(startNode.location, startNode); while (!openSet.isEmpty()) { PathNode current = openSet.remove(); openSetLookupMap.remove(current.location); if (current.location.equals(end)) { return reconstructPath(current); } closedSet.add(current.location); for (Point neighbor : findNeighbors(current.location, context.allowDiagonalMovements())) { if (!isInValidPoint(neighbor) && !closedSet.contains(neighbor)) { PathNode neighborNode = openSetLookupMap.get(neighbor); double tentativeGvalue = current.g + context.g_factor(neighbor); if (neighborNode == null) { neighborNode = new PathNode(current, neighbor, tentativeGvalue, context.h(neighbor, end)); } else { neighborNode.g = Math.min(neighborNode.g, tentativeGvalue); } // TODO modify to use HashSet instead of PriorityQueue since remove is linear anyway // TODO the above should allow us to condense two openSets into a single one, replacing // openSet.remove() w/ linear time lookup // TODO consider inserting things into self sorting data structure, that way remove is // constant time and lookups are log n (binary search?) openSet.remove(neighborNode); openSet.add(neighborNode); } } } return new ArrayList<Point>(); }