source: lost-perception/astar/AStarMap.java

package astar;

import java.awt.Rectangle;
import java.awt.Shape;
import java.awt.geom.AffineTransform;
import java.util.Collection;
import java.util.LinkedList;
import java.awt.geom.Point2D;
import java.awt.Point;

import main.MapObject;

import collision.Bound;

public class AStarMap {
    public QuadTree[][] map;
    private int width;
    private int height;
    public static final int ROOT_SIZE = 40;
    public static final int MAX_TREE_DEPTH = 4;
    
    public AStarMap(final int width, final int height) {
        this.map = new QuadTree[width][height];
        this.width = width;
        this.height = height;
        for (int x = 0; x < width; ++x) {
            for (int y = 0; y < height; ++y) {
                this.map[x][y] = new QuadTree(x * 40, y * 40, 40, 40);
            }
        }
    }
    
    public AStarNode getNode(final int x, final int y) {
        return this.getLeaf(this.map[x / 40][y / 40], x, y);
    }
    
    public boolean validPath(final Point source, final Point dest) {
        final double frac = 5.0 / source.distance(dest);
        final int xTotal = dest.x - source.x;
        final int yTotal = dest.y - source.y;
        for (double xLen = frac * xTotal, yLen = frac * yTotal, xCur = 0.0, yCur = 0.0; Math.abs(xCur) < Math.abs(xTotal); xCur += xLen, yCur += yLen) {
            final AStarNode curNode = this.getNode((int)xCur + source.x, (int)yCur + source.y);
            if (!curNode.passable) {
                return false;
            }
        }
        return true;
    }
    
    private AStarNode getLeaf(final QuadTree qt, final int x, final int y) {
        if (qt.isLeaf()) {
            return qt.getNode();
        }
        int child = -1;
        if (x < qt.getX() + qt.getWidth() / 2) {
            if (y < qt.getY() + qt.getHeight() / 2) {
                child = 0;
            }
            else {
                child = 2;
            }
        }
        else if (y < qt.getY() + qt.getHeight() / 2) {
            child = 1;
        }
        else {
            child = 3;
        }
        return this.getLeaf(qt.getChildren()[child], x, y);
    }
    
    public void generateAllNeighbors() {
        for (int x = 0; x < this.width; ++x) {
            for (int y = 0; y < this.height; ++y) {
                this.generateNeighborsHelper(this.map[x][y]);
            }
        }
    }
    
    public void generateNeighborsHelper(final QuadTree qt) {
        if (qt.isLeaf()) {
            qt.getNode().neighbors = this.getNeighbors(qt.getNode());
        }
        else {
            this.generateNeighborsHelper(qt.getChildren()[0]);
            this.generateNeighborsHelper(qt.getChildren()[1]);
            this.generateNeighborsHelper(qt.getChildren()[2]);
            this.generateNeighborsHelper(qt.getChildren()[3]);
        }
    }
    
    public LinkedList<AStarNode> getNeighbors(final AStarNode n) {
        final LinkedList<AStarNode> neighbors = new LinkedList<AStarNode>();
        final int xTop = n.getParent().getX() / 40;
        final int yTop = (n.getParent().getY() - 1) / 40;
        if (xTop >= 0 && xTop < this.width && yTop >= 0 && yTop < this.height) {
            if (1 <= xTop) {
                neighbors.addAll(this.getTopNeighbors(this.map[xTop - 1][yTop], n));
            }
            neighbors.addAll(this.getTopNeighbors(this.map[xTop][yTop], n));
            if (xTop < this.width - 1) {
                neighbors.addAll(this.getTopNeighbors(this.map[xTop + 1][yTop], n));
            }
        }
        final int xBottom = n.getParent().getX() / 40;
        final int yBottom = (n.getParent().getY() + n.getParent().getHeight()) / 40;
        if (xBottom >= 0 && xBottom < this.width && yBottom >= 0 && yBottom < this.height) {
            if (1 <= xBottom) {
                neighbors.addAll(this.getBottomNeighbors(this.map[xBottom - 1][yBottom], n));
            }
            neighbors.addAll(this.getBottomNeighbors(this.map[xBottom][yBottom], n));
            if (xBottom < this.width - 1) {
                neighbors.addAll(this.getBottomNeighbors(this.map[xBottom + 1][yBottom], n));
            }
        }
        final int xLeft = (n.getParent().getX() - 1) / 40;
        final int yLeft = n.getParent().getY() / 40;
        if (xLeft >= 0 && xLeft < this.width && yLeft >= 0 && yLeft < this.height) {
            neighbors.addAll(this.getLeftNeighbors(this.map[xLeft][yLeft], n));
        }
        final int xRight = (n.getParent().getX() + n.getParent().getWidth()) / 40;
        final int yRight = n.getParent().getY() / 40;
        if (xRight >= 0 && xRight < this.width && yRight >= 0 && yRight < this.height) {
            neighbors.addAll(this.getRightNeighbors(this.map[xRight][yRight], n));
        }
        return neighbors;
    }
    
    private LinkedList<AStarNode> getTopNeighbors(final QuadTree p, final AStarNode n) {
        final LinkedList<AStarNode> neighbors = new LinkedList<AStarNode>();
        final int y = n.getParent().getY();
        if (p.isLeaf()) {
            final int xMin = n.getParent().getX();
            final int xMax = xMin + n.getParent().getWidth();
            if (p.getY() + p.getHeight() == y && xMax >= p.getX() && p.getX() + p.getWidth() >= xMin) {
                neighbors.add(p.getNode());
            }
        }
        else if (y <= p.getY() + p.getHeight() / 2) {
            neighbors.addAll(this.getTopNeighbors(p.getChildren()[0], n));
            neighbors.addAll(this.getTopNeighbors(p.getChildren()[1], n));
        }
        else {
            neighbors.addAll(this.getTopNeighbors(p.getChildren()[2], n));
            neighbors.addAll(this.getTopNeighbors(p.getChildren()[3], n));
        }
        return neighbors;
    }
    
    private LinkedList<AStarNode> getBottomNeighbors(final QuadTree p, final AStarNode n) {
        final LinkedList<AStarNode> neighbors = new LinkedList<AStarNode>();
        final int y = n.getParent().getY() + n.getParent().getHeight();
        if (p.isLeaf()) {
            final int xMin = n.getParent().getX();
            final int xMax = xMin + n.getParent().getWidth();
            if (p.getY() == y && xMax >= p.getX() && p.getX() + p.getWidth() >= xMin) {
                neighbors.add(p.getNode());
            }
        }
        else if (y < p.getY() + p.getHeight() / 2) {
            neighbors.addAll(this.getBottomNeighbors(p.getChildren()[0], n));
            neighbors.addAll(this.getBottomNeighbors(p.getChildren()[1], n));
        }
        else {
            neighbors.addAll(this.getBottomNeighbors(p.getChildren()[2], n));
            neighbors.addAll(this.getBottomNeighbors(p.getChildren()[3], n));
        }
        return neighbors;
    }
    
    private LinkedList<AStarNode> getLeftNeighbors(final QuadTree p, final AStarNode n) {
        final LinkedList<AStarNode> neighbors = new LinkedList<AStarNode>();
        final int x = n.getParent().getX();
        if (p.isLeaf()) {
            final int yMin = n.getParent().getY();
            final int yMax = yMin + n.getParent().getHeight();
            if (p.getX() + p.getWidth() == x && yMax >= p.getY() && p.getY() + p.getHeight() >= yMin) {
                neighbors.add(p.getNode());
            }
        }
        else if (x <= p.getX() + p.getWidth() / 2) {
            neighbors.addAll(this.getLeftNeighbors(p.getChildren()[0], n));
            neighbors.addAll(this.getLeftNeighbors(p.getChildren()[2], n));
        }
        else {
            neighbors.addAll(this.getLeftNeighbors(p.getChildren()[1], n));
            neighbors.addAll(this.getLeftNeighbors(p.getChildren()[3], n));
        }
        return neighbors;
    }
    
    private LinkedList<AStarNode> getRightNeighbors(final QuadTree p, final AStarNode n) {
        final LinkedList<AStarNode> neighbors = new LinkedList<AStarNode>();
        final int x = n.getParent().getX() + n.getParent().getWidth();
        if (p.isLeaf()) {
            final int yMin = n.getParent().getY();
            final int yMax = yMin + n.getParent().getHeight();
            if (p.getX() == x && yMax >= p.getY() && p.getY() + p.getHeight() >= yMin) {
                neighbors.add(p.getNode());
            }
        }
        else if (x < p.getX() + p.getWidth() / 2) {
            neighbors.addAll(this.getRightNeighbors(p.getChildren()[0], n));
            neighbors.addAll(this.getRightNeighbors(p.getChildren()[2], n));
        }
        else {
            neighbors.addAll(this.getRightNeighbors(p.getChildren()[1], n));
            neighbors.addAll(this.getRightNeighbors(p.getChildren()[3], n));
        }
        return neighbors;
    }
    
    public void addObstacle(final MapObject o) {
        if (o.getBound() == null) {
            return;
        }
        final Bound realBound = new Bound(o.getBound().createTransformedArea(new AffineTransform(1.0f, 0.0f, 0.0f, 1.0f, o.getBoundX(), o.getBoundY())));
        final Rectangle rect = realBound.getBounds();
        int lowXBound = rect.x / 40;
        int lowYBound = rect.y / 40;
        int highXBound = (rect.x + rect.width) / 40 + 1;
        int highYBound = (rect.y + rect.height) / 40 + 1;
        if (lowXBound < 0) {
            lowXBound = 0;
        }
        if (highXBound > this.width - 1) {
            highXBound = this.width - 1;
        }
        if (lowYBound < 0) {
            lowYBound = 0;
        }
        if (highYBound > this.height - 1) {
            highYBound = this.height - 1;
        }
        for (int x = lowXBound; x <= highXBound; ++x) {
            for (int y = lowYBound; y <= highYBound; ++y) {
                this.map[x][y].addObstacle(o);
            }
        }
    }
    
    public void coalesceQuadTrees() {
        for (int x = 0; x < this.width; ++x) {
            for (int y = 0; y < this.height; ++y) {
                this.map[x][y].coalesce();
            }
        }
    }
}