Add pathfinding.

src/org/apollo/game/model/entity/path/AStarPathfindingAlgorithm.java

@@ -0,0 +1,155 @@
+package org.apollo.game.model.entity.path;
+
+import java.util.ArrayDeque;
+import java.util.Deque;
+import java.util.HashMap;
+import java.util.HashSet;
+import java.util.Map;
+import java.util.PriorityQueue;
+import java.util.Queue;
+import java.util.Set;
+
+import org.apollo.game.model.Position;
+
+/**
+ * A {@link PathfindingAlgorithm} that utilises the A* algorithm to find a solution.
+ * <p>
+ * This implementation utilises a {@link PriorityQueue} of open {@link Node}s, in addition to the usual {@link HashSet}.
+ * This allows for logarithmic-time finding of the cheapest element (as opposed to the linear time associated with
+ * iterating over the set), whilst still maintaining the constant time contains and remove of the set.
+ * <p>
+ * This implementation also avoids the linear-time removal from the queue by polling until the first open node is found
+ * when identifying the cheapest node.
+ * 
+ * @author Major
+ */
+final class AStarPathfindingAlgorithm extends PathfindingAlgorithm {
+
+	/**
+	 * The heuristic.
+	 */
+	private final Heuristic heuristic;
+
+	/**
+	 * Creates the A* pathfinding algorithm with the specified heuristic.
+	 *
+	 * @param heuristic The heuristic.
+	 */
+	public AStarPathfindingAlgorithm(Heuristic heuristic) {
+		this.heuristic = heuristic;
+	}
+
+	@Override
+	public Deque<Position> find(Position origin, Position target) {
+		Map<Position, Node> nodes = new HashMap<>();
+		Node start = new Node(origin), end = new Node(target);
+		nodes.put(origin, start);
+		nodes.put(target, end);
+
+		Set<Node> open = new HashSet<>();
+		Queue<Node> sorted = new PriorityQueue<>();
+		open.add(start);
+		sorted.add(start);
+
+		do {
+			Node active = getCheapest(sorted);
+			Position position = active.getPosition();
+
+			if (position.equals(target)) {
+				break;
+			}
+
+			open.remove(active);
+			active.close();
+
+			int x = position.getX(), y = position.getY();
+			for (int nextX = x - 1; x <= x + 1; nextX++) {
+				for (int nextY = y - 1; y <= y + 1; nextY++) {
+					if (nextX == x && nextY == y) {
+						continue;
+					}
+
+					Position adjacent = new Position(nextX, nextY);
+					if (traversable(adjacent)) {
+						Node node = createIfAbsent(adjacent, nodes);
+						compare(active, node, open, sorted, heuristic);
+					}
+				}
+			}
+		} while (!open.isEmpty());
+
+		Deque<Position> shortest = new ArrayDeque<>();
+		Node active = end;
+
+		if (active.hasParent()) {
+			Position position = active.getPosition();
+
+			while (!origin.equals(position)) {
+				shortest.addFirst(position);
+				active = active.getParent(); // If the target has a parent then all of the others will.
+				position = active.getPosition();
+			}
+		}
+
+		return shortest;
+	}
+
+	/**
+	 * Compares the two specified {@link Node}s, adding the other node to the open {@link Set} if the estimation is
+	 * cheaper than the current cost.
+	 * 
+	 * @param active The active node.
+	 * @param other The node to compare the active node against.
+	 * @param open The set of open nodes.
+	 * @param sorted The sorted {@link Queue} of nodes.
+	 * @param heuristic The {@link Heuristic} used to estimate the cost of the node.
+	 */
+	private void compare(Node active, Node other, Set<Node> open, Queue<Node> sorted, Heuristic heuristic) {
+		int cost = active.getCost() + heuristic.estimate(active.getPosition(), other.getPosition());
+
+		if (other.getCost() > cost) {
+			open.remove(other);
+			other.close();
+		} else if (other.isOpen() && !open.contains(other)) {
+			other.setCost(cost);
+			other.setParent(active);
+			open.add(other);
+			sorted.add(other);
+		}
+	}
+
+	/**
+	 * Creates a {@link Node} and inserts it into the specified {@link Map} if one does not already exist, then returns
+	 * that node.
+	 *
+	 * @param position The {@link Position}.
+	 * @param nodes The map of positions to nodes.
+	 * @return The node.
+	 */
+	private Node createIfAbsent(Position position, Map<Position, Node> nodes) {
+		Node existing = nodes.get(position);
+		if (existing == null) {
+			existing = new Node(position);
+			nodes.put(position, existing);
+		}
+
+		return existing;
+	}
+
+	/**
+	 * Gets the cheapest open {@link Node} from the {@link Queue}.
+	 * 
+	 * @param nodes The queue of nodes.
+	 * @return The cheapest node.
+	 */
+	private Node getCheapest(Queue<Node> nodes) {
+		Node node = nodes.peek();
+		while (!node.isOpen()) {
+			nodes.poll();
+			node = nodes.peek();
+		}
+
+		return node;
+	}
+
+}

src/org/apollo/game/model/entity/path/ChebyshevHeuristic.java

@@ -0,0 +1,19 @@
+package org.apollo.game.model.entity.path;
+
+import org.apollo.game.model.Position;
+
+/**
+ * The Chebyshev heuristic, ideal for a system that allows for 8-directional movement.
+ * 
+ * @author Major
+ */
+final class ChebyshevHeuristic extends Heuristic {
+
+	@Override
+	public int estimate(Position current, Position goal) {
+		int dx = Math.abs(current.getX() - goal.getX());
+		int dy = Math.abs(current.getX() - goal.getY());
+		return dx >= dy ? dx : dy;
+	}
+
+}

src/org/apollo/game/model/entity/path/Heuristic.java

@@ -0,0 +1,21 @@
+package org.apollo.game.model.entity.path;
+
+import org.apollo.game.model.Position;
+
+/**
+ * A heuristic used by the A* algorithm.
+ * 
+ * @author Major
+ */
+abstract class Heuristic {
+
+	/**
+	 * Estimates the value for this heuristic.
+	 * 
+	 * @param current The current {@link Position}.
+	 * @param target The target position.
+	 * @return The heuristic value.
+	 */
+	public abstract int estimate(Position current, Position target);
+
+}

src/org/apollo/game/model/entity/path/ManhattanHeuristic.java

@@ -0,0 +1,19 @@
+package org.apollo.game.model.entity.path;
+
+import org.apollo.game.model.Position;
+
+/**
+ * The Manhattan heuristic, ideal for a system that limits movement to 4 directions.
+ * 
+ * @author Major
+ */
+final class ManhattanHeuristic extends Heuristic {
+
+	@Override
+	public int estimate(Position current, Position goal) {
+		int dx = Math.abs(current.getX() - goal.getX());
+		int dy = Math.abs(current.getX() - goal.getY());
+		return dx + dy;
+	}
+
+}

src/org/apollo/game/model/entity/path/Node.java

@@ -0,0 +1,148 @@
+package org.apollo.game.model.entity.path;
+
+import java.util.NoSuchElementException;
+import java.util.Optional;
+
+import org.apollo.game.model.Position;
+
+/**
+ * A node representing a weighted {@link Position}.
+ * 
+ * @author Major
+ */
+final class Node {
+
+	/**
+	 * The cost of this node.
+	 */
+	private int cost;
+
+	/**
+	 * Whether or not this node is open.
+	 */
+	private boolean open = true;
+
+	/**
+	 * The parent node of this node.
+	 */
+	private Optional<Node> parent = Optional.empty();
+
+	/**
+	 * The point this node represents.
+	 */
+	private final Position position;
+
+	/**
+	 * Creates the node with the specified {@link Position} and cost.
+	 * 
+	 * @param position The position.
+	 */
+	public Node(Position position) {
+		this(position, 0);
+	}
+
+	/**
+	 * Creates the node with the specified {@link Position} and cost.
+	 * 
+	 * @param position The position.
+	 * @param cost The cost of the node.
+	 */
+	public Node(Position position, int cost) {
+		this.position = position;
+		this.cost = cost;
+	}
+
+	/**
+	 * Closes this node.
+	 */
+	public void close() {
+		open = false;
+	}
+
+	@Override
+	public boolean equals(Object obj) {
+		if (obj instanceof Node) {
+			Node other = (Node) obj;
+
+			return position.equals(other.position);
+		}
+
+		return false;
+	}
+
+	/**
+	 * Gets the cost of this node.
+	 * 
+	 * @return The cost.
+	 */
+	public int getCost() {
+		return cost;
+	}
+
+	/**
+	 * Gets the parent node of this node.
+	 * 
+	 * @return The parent node.
+	 * @throws NoSuchElementException If this node does not have a parent.
+	 */
+	public Node getParent() {
+		return parent.get();
+	}
+
+	/**
+	 * Gets the {@link Position} this node represents.
+	 * 
+	 * @return The position.
+	 */
+	public Position getPosition() {
+		return position;
+	}
+
+	@Override
+	public int hashCode() {
+		return position.getX() * 31 + position.getY();
+	}
+
+	/**
+	 * Returns whether or not this node has a parent node.
+	 * 
+	 * @return {@code true} if this node has a parent node, otherwise {@code false}.
+	 */
+	public boolean hasParent() {
+		return parent.isPresent();
+	}
+
+	/**
+	 * Returns whether or not this {@link Node} is open.
+	 * 
+	 * @return {@code true} if this node is open, otherwise {@code false}.
+	 */
+	public boolean isOpen() {
+		return open;
+	}
+
+	/**
+	 * Sets the cost of this node.
+	 * 
+	 * @param cost The cost.
+	 */
+	public void setCost(int cost) {
+		this.cost = cost;
+	}
+
+	/**
+	 * Sets the parent node of this node.
+	 * 
+	 * @param parent The parent node. May be {@code null}.
+	 */
+	public void setParent(Node parent) {
+		this.parent = Optional.ofNullable(parent);
+	}
+
+	@Override
+	public String toString() {
+		return Node.class.getSimpleName() + " [x=" + position.getX() + ", y=" + position.getY() + ", open=" + open + ", cost="
+				+ cost + "]";
+	}
+
+}

src/org/apollo/game/model/entity/path/PathfindingAlgorithm.java

@@ -0,0 +1,84 @@
+package org.apollo.game.model.entity.path;
+
+import java.util.Deque;
+import java.util.Set;
+
+import org.apollo.game.model.Direction;
+import org.apollo.game.model.Position;
+import org.apollo.game.model.World;
+import org.apollo.game.model.area.Sector;
+import org.apollo.game.model.area.SectorRepository;
+import org.apollo.game.model.entity.Entity.EntityType;
+import org.apollo.game.model.entity.GameObject;
+
+/**
+ * An algorithm used to find a path between two {@link Position}s.
+ *
+ * @author Major
+ */
+abstract class PathfindingAlgorithm {
+
+	/**
+	 * The repository of sectors.
+	 */
+	private static final SectorRepository repository = World.getWorld().getSectorRepository();
+
+	/**
+	 * Finds a valid path from the origin {@link Position} to the target one.
+	 * 
+	 * @param origin The origin position.
+	 * @param target The target position.
+	 * @return The {@link Deque} containing the positions to go through.
+	 */
+	public abstract Deque<Position> find(Position origin, Position target);
+
+	/**
+	 * Returns whether or not the tile at the specified position is walkable. FIXME do this properly w/tile collision
+	 * data!
+	 *
+	 * @param position The {@link Position}.
+	 * @return {@code true} if the tile is walkable, otherwise {@code false}.
+	 */
+	protected boolean traversable(Position position) {
+		Sector sector = repository.get(position.getSectorCoordinates());
+		Set<GameObject> objects = sector.getEntities(position, EntityType.GAME_OBJECT);
+
+		return objects.stream().anyMatch(object -> object.getDefinition().isSolid());
+	}
+
+	/**
+	 * Returns whether or not the {@link Position}s walking one step in a specified {@link Direction} would lead to is
+	 * traversable.
+	 * 
+	 * @param position The starting position.
+	 * @param directions The directions that should be checked.
+	 * @return {@code true} if any of the directions lead to a traversable tile, otherwise {@code false}.
+	 */
+	protected boolean traversable(Position position, Direction... directions) {
+		int height = position.getHeight();
+
+		for (Direction direction : directions) {
+			int x = position.getX(), y = position.getY();
+			int value = direction.toInteger();
+
+			if (value >= Direction.NORTH_WEST.toInteger() && value <= Direction.NORTH_EAST.toInteger()) {
+				y++;
+			} else if (value >= Direction.SOUTH_WEST.toInteger() && value <= Direction.SOUTH_EAST.toInteger()) {
+				y--;
+			}
+
+			if (direction == Direction.NORTH_EAST || direction == Direction.EAST || direction == Direction.SOUTH_EAST) {
+				x++;
+			} else if (direction == Direction.NORTH_WEST || direction == Direction.WEST || direction == Direction.SOUTH_WEST) {
+				x--;
+			}
+
+			if (traversable(new Position(x, y, height))) {
+				return true;
+			}
+		}
+
+		return false;
+	}
+
+}

src/org/apollo/game/model/entity/path/SimplePathfindingAlgorithm.java

@@ -0,0 +1,113 @@
+package org.apollo.game.model.entity.path;
+
+import java.util.ArrayDeque;
+import java.util.Deque;
+
+import org.apollo.game.model.Direction;
+import org.apollo.game.model.Position;
+
+/**
+ * A very simple pathfinding algorithm that simply walks in the direction of the target until it either reaches it or is
+ * blocked. TODO diagonal movement support.
+ *
+ * @author Major
+ */
+final class SimplePathfindingAlgorithm extends PathfindingAlgorithm {
+
+	@Override
+	public Deque<Position> find(Position origin, Position target) {
+		int approximation = (int) (origin.getLongestDelta(target) * 1.5);
+		Deque<Position> positions = new ArrayDeque<>(approximation);
+
+		return addHorizontal(origin, target, positions);
+	}
+
+	/**
+	 * Adds the necessary and possible horizontal {@link Position}s to the existing {@link Deque}.
+	 * <p>
+	 * This method:
+	 * <ul>
+	 * <li>Adds positions horizontally until we are either horizontally aligned with the target, or the next step is not
+	 * traversable.
+	 * <li>Checks if we are not at the target, and that either of the horizontally-adjacent positions are traversable:
+	 * if so, we traverse horizontally (see {@link #addHorizontal}); if not, return the current path.
+	 * </ul>
+	 * 
+	 * @param current The current position.
+	 * @param target The target position.
+	 * @param positions The deque of positions.
+	 * @return The deque of positions containing the path.
+	 */
+	private Deque<Position> addHorizontal(Position current, Position target, Deque<Position> positions) {
+		int x = current.getX(), y = current.getY(), height = current.getHeight();
+		int dx = x - target.getX();
+
+		if (dx > 0) {
+			Position west = new Position(x - 1, y, height);
+
+			while (traversable(west) && dx-- > 0) {
+				west = new Position(--x, y, height);
+				positions.addLast(west);
+			}
+		} else if (dx < 0) {
+			Position east = new Position(x + 1, y, height);
+
+			while (traversable(east) && dx++ < 0) {
+				east = new Position(++x, y, height);
+				positions.addLast(east);
+			}
+		}
+
+		Position last = new Position(x, y, height);
+		if (!current.equals(last) && traversable(last, Direction.NORTH, Direction.SOUTH)) {
+			return addVertical(last, target, positions);
+		}
+
+		return positions;
+	}
+
+	/**
+	 * Adds the necessary and possible vertical {@link Position}s to the existing {@link Deque}.
+	 * <p>
+	 * This method:
+	 * <ul>
+	 * <li>Adds positions vertically until we are either vertically aligned with the target, or the next step is not
+	 * traversable.
+	 * <li>Checks if we are not at the target, and that either of the horizontally-adjacent positions are traversable:
+	 * if so, we traverse horizontally (see {@link #addHorizontal}); if not, return the current path.
+	 * </ul>
+	 * 
+	 * @param current The current position.
+	 * @param target The target position.
+	 * @param positions The deque of positions.
+	 * @return The deque of positions containing the path.
+	 */
+	private Deque<Position> addVertical(Position current, Position target, Deque<Position> positions) {
+		int x = current.getX(), y = current.getY(), height = current.getHeight();
+		int dy = y - target.getY();
+
+		if (dy > 0) {
+			Position south = new Position(x, y - 1, height);
+
+			while (traversable(south) && dy-- > 0) {
+				south = new Position(x, --y, height);
+				positions.addLast(south);
+			}
+		} else if (dy < 0) {
+			Position north = new Position(x, y + 1, height);
+
+			while (traversable(north) && dy++ < 0) {
+				north = new Position(x, ++y, height);
+				positions.addLast(north);
+			}
+		}
+
+		Position last = new Position(x, y, height);
+		if (!last.equals(target) && traversable(last, Direction.EAST, Direction.WEST)) {
+			return addHorizontal(last, target, positions);
+		}
+
+		return positions;
+	}
+
+}

src/org/apollo/game/model/entity/path/package-info.java

@@ -0,0 +1,4 @@
+/**
+ * Contains pathfinding-related classes.
+ */
+package org.apollo.game.model.entity.path;