AStarPathFinder.java

package com.osroyale.game.world.pathfinding.path.impl;
 
import com.osroyale.game.world.Interactable;
import com.osroyale.game.world.entity.mob.Direction;
import com.osroyale.game.world.entity.mob.Mob;
import com.osroyale.game.world.pathfinding.distance.Distance;
import com.osroyale.game.world.pathfinding.path.Path;
import com.osroyale.game.world.pathfinding.path.PathFinder;
import com.osroyale.game.world.position.Position;
import com.osroyale.game.world.region.Region;
 
import java.util.*;

 
public final class AStarPathFinder extends PathFinder {

    private final Distance heuristic;
    private final Mob character;
    private final Map<Position, Node> nodes = new HashMap<>(Region.SIZE * Region.SIZE);
    private final Set<Node> open = new HashSet<>(Region.SIZE * Region.SIZE);
    private final Queue<Node> sorted = new PriorityQueue<>(Region.SIZE * Region.SIZE);
    private final Deque<Position> shortest = new ArrayDeque<>(100);

    public AStarPathFinder(Mob character, Distance heuristic) {
        this.character = character;
        this.heuristic = heuristic;
    }

    public Path find(Position destination) {
        Position origin = character.getPosition();
        if (origin.getHeight() != destination.getHeight())
            return new Path(null);
        return find(character, destination);
    }

    public Path find(Interactable destination) {
        Position origin = character.getPosition();
        if (origin.getHeight() != destination.getHeight())
            return new Path(null);
        return find(character, destination);
    }

    @Override
    public Path find(Mob source, Position target, int targetWidth, int targetLength) {
        if (source.getHeight() != target.getHeight())
            return new Path(null);
 
        Node start = new Node(source.getPosition()), end = new Node(target);
 
        open.clear();
        nodes.clear();
        sorted.clear();
        shortest.clear();
 
        nodes.put(source.getPosition(), start);
        nodes.put(target, end);
 
        open.add(start);
        sorted.add(start);
 
        Node active;
        Position next;
        boolean found = false;
        int distance = (int) (source.getPosition().getDistance(target) * Region.SIZE);
 
        do {
            active = getCheapest(sorted);
            Position position = active.getPosition();
 
            if (position.equals(target)) {
                found = true;
                break;
            }
 
            open.remove(active);
            active.close();
 
            for (Direction direction : Direction.valid()) {
                next = position.transform(direction.getFaceLocation());
                if (traversable(position, source.width(), direction) && traversable(next, source.width(), Direction.getOppositeDirection(direction))) {
                    Node node = nodes.computeIfAbsent(next, Node::new);
                    compare(active, node);
                }
            }
        } while (!open.isEmpty() && sorted.size() < distance);
 
        if (found) {
 
            if (end.hasParent()) {
                active = end;
            }
 
            if (active.hasParent()) {
                Position position = active.getPosition();
 
                while (!source.getPosition().equals(position)) {
                    shortest.addFirst(position);
                    active = active.getParent();
                    position = active.getPosition();
 
                    if (shortest.size() >= 100) {
                        break;
                    }
                }
            }
 
            return new Path(shortest);
        }
        return new Path(null);
    }
 
    private void compare(Node active, Node other) {
        int cost = active.getCost() + heuristic.calculate(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);
        }
    }

    private Node getCheapest(Queue<Node> nodes) {
        Node node = nodes.peek();
        while (!node.isOpen()) {
            nodes.poll();
            node = nodes.peek();
        }
        return node;
    }
 
}

final class Node implements Comparable<Node> {

    private int cost;

    private boolean open = true;

    private Node parent;

    private final Position position;

    Node(Position position) {
        this(position, 0);
    }

    private Node(Position position, int cost) {
        this.position = position;
        this.cost = cost;
    }

    void setCost(int cost) {
        this.cost = cost;
    }

    int getCost() {
        return cost;
    }

    public void close() {
        open = false;
    }

    public boolean isOpen() {
        return open;
    }

    void setParent(Node parent) {
        this.parent = parent;
    }

    Node getParent() {
        return parent;
    }

    boolean hasParent() {
        return parent != null;
    }

    public Position getPosition() {
        return position;
    }

    @Override
    public int compareTo(Node other) {
        return Integer.compare(cost, other.cost);
    }

    @Override
    public final boolean equals(Object obj) {
        if (this == obj) return true;
        if (!(obj instanceof Node)) return false;
        Node other = (Node) obj;
        return getPosition().getX() == other.getPosition().getX()
            && getPosition().getY() == other.getPosition().getY()
            && getPosition().getHeight() == other.getPosition().getHeight()
            && getCost() == other.getCost()
            && getParent().getCost() == other.getParent().getCost()
            && getParent().getPosition().getX() == other.getParent().getPosition().getX()
            && getParent().getPosition().getY() == other.getParent().getPosition().getY()
            && getParent().getPosition().getHeight() == other.getPosition().getHeight();
    }

    @Override
    public final int hashCode() {
        final int prime = 31;
        int result = 1;
        result = prime * result + position.getX();
        result = prime * result + position.getY();
        result = prime * result + position.getHeight();
        if (hasParent()) {
            Position p = parent.getPosition();
            result = prime * result + p.getX();
            result = prime * result + p.getY();
            result = prime * result + p.getHeight();
        }
        result = prime * result + cost;
        return result;
    }
 
}