package com.etheller.warsmash.viewer5.handlers.w3x.simulation.pathing;
import java.awt.geom.Point2D;
import java.util.Arrays;
import java.util.Collections;
import java.util.Comparator;
import java.util.Iterator;
import java.util.LinkedList;
import java.util.PriorityQueue;
import com.badlogic.gdx.math.Rectangle;
import com.etheller.warsmash.viewer5.handlers.w3x.environment.PathingGrid;
import com.etheller.warsmash.viewer5.handlers.w3x.environment.PathingGrid.MovementType;
import com.etheller.warsmash.viewer5.handlers.w3x.simulation.CSimulation;
import com.etheller.warsmash.viewer5.handlers.w3x.simulation.CUnit;
import com.etheller.warsmash.viewer5.handlers.w3x.simulation.CWorldCollision;
import com.etheller.warsmash.viewer5.handlers.w3x.simulation.behaviors.CBehaviorMove;
public class CPathfindingProcessor {
private static final Rectangle tempRect = new Rectangle();
private final PathingGrid pathingGrid;
private final CWorldCollision worldCollision;
private final LinkedList<PathfindingJob> moveQueue = new LinkedList<>();
// things with modified state per current job:
private final Node[][] nodes;
private final Node[][] cornerNodes;
private final Node[] goalSet = new Node[4];
private int goals = 0;
private int pathfindJobId = 0;
private int totalIterations = 0;
private int totalJobLoops = 0;
private final int pathingGridCellCount;
public CPathfindingProcessor(final PathingGrid pathingGrid, final CWorldCollision worldCollision) {
this.pathingGrid = pathingGrid;
this.worldCollision = worldCollision;
this.nodes = new Node[pathingGrid.getHeight()][pathingGrid.getWidth()];
this.cornerNodes = new Node[pathingGrid.getHeight() + 1][pathingGrid.getWidth() + 1];
for (int i = 0; i < this.nodes.length; i++) {
for (int j = 0; j < this.nodes[i].length; j++) {
this.nodes[i][j] = new Node(new Point2D.Float(pathingGrid.getWorldX(j), pathingGrid.getWorldY(i)));
}
}
for (int i = 0; i < this.cornerNodes.length; i++) {
for (int j = 0; j < this.cornerNodes[i].length; j++) {
this.cornerNodes[i][j] = new Node(
new Point2D.Float(pathingGrid.getWorldXFromCorner(j), pathingGrid.getWorldYFromCorner(i)));
}
}
this.pathingGridCellCount = pathingGrid.getWidth() * pathingGrid.getHeight();
}
/**
* Finds the path to a point using a naive, slow, and unoptimized algorithm.
* Does not have optimizations yet, do this for a bunch of units and it will
* probably lag like a walrus. The implementation here was created by reading
* the wikipedia article on A* to jog my memory from data structures class back
* in college, and is meant only as a first draft to get things working.
*
* @param collisionSize
*
*
* @param start
* @param goal
* @param playerIndex
* @param queueItem
* @return
*/
public void findNaiveSlowPath(final CUnit ignoreIntersectionsWithThisUnit,
final CUnit ignoreIntersectionsWithThisSecondUnit, final float startX, final float startY,
final Point2D.Float goal, final PathingGrid.MovementType movementType, final float collisionSize,
final boolean allowSmoothing, final CBehaviorMove queueItem) {
this.moveQueue.offer(new PathfindingJob(ignoreIntersectionsWithThisUnit, ignoreIntersectionsWithThisSecondUnit,
startX, startY, goal, movementType, collisionSize, allowSmoothing, queueItem));
}
public void removeFromPathfindingQueue(final CBehaviorMove behaviorMove) {
// TODO because of silly java things, this remove is O(N) for now,
// we could do some refactors to make it O(1) but do we care?
final Iterator<PathfindingJob> iterator = this.moveQueue.iterator();
while (iterator.hasNext()) {
final PathfindingJob job = iterator.next();
if (job.queueItem == behaviorMove) {
iterator.remove();
}
}
}
private boolean pathableBetween(final CUnit ignoreIntersectionsWithThisUnit,
final CUnit ignoreIntersectionsWithThisSecondUnit, final float startX, final float startY,
final PathingGrid.MovementType movementType, final float collisionSize, final float x, final float y) {
return this.pathingGrid.isPathable(x, y, movementType, collisionSize)
&& this.pathingGrid.isPathable(startX, y, movementType, collisionSize)
&& this.pathingGrid.isPathable(x, startY, movementType, collisionSize)
&& isPathableDynamically(x, y, ignoreIntersectionsWithThisUnit, ignoreIntersectionsWithThisSecondUnit,
movementType)
&& isPathableDynamically(x, startY, ignoreIntersectionsWithThisUnit,
ignoreIntersectionsWithThisSecondUnit, movementType)
&& isPathableDynamically(startX, y, ignoreIntersectionsWithThisUnit,
ignoreIntersectionsWithThisSecondUnit, movementType);
}
private boolean isPathableDynamically(final float x, final float y, final CUnit ignoreIntersectionsWithThisUnit,
final CUnit ignoreIntersectionsWithThisSecondUnit, final PathingGrid.MovementType movementType) {
return !this.worldCollision.intersectsAnythingOtherThan(tempRect.setCenter(x, y),
ignoreIntersectionsWithThisUnit, ignoreIntersectionsWithThisSecondUnit, movementType);
}
public static boolean isCollisionSizeBetterSuitedForCorners(final float collisionSize) {
return (((2 * (int) collisionSize) / 32) % 2) == 1;
}
public double f(final Node n) {
return n.g + h(n);
}
public double g(final Node n) {
return n.g;
}
private boolean isGoal(final Node n) {
for (int i = 0; i < this.goals; i++) {
if (n == this.goalSet[i]) {
return true;
}
}
return false;
}
public float h(final Node n) {
float bestDistance = 0;
for (int i = 0; i < this.goals; i++) {
final float possibleDistance = (float) n.point.distance(this.goalSet[i].point);
if (possibleDistance > bestDistance) {
bestDistance = possibleDistance; // always overestimate
}
}
return bestDistance;
}
public static final class Node {
public Direction cameFromDirection;
private final Point2D.Float point;
private double f;
private double g;
private Node cameFrom;
private int pathfindJobId;
private Node(final Point2D.Float point) {
this.point = point;
}
private void touch(final int pathfindJobId) {
if (pathfindJobId != this.pathfindJobId) {
this.g = Float.POSITIVE_INFINITY;
this.f = Float.POSITIVE_INFINITY;
this.cameFrom = null;
this.cameFromDirection = null;
this.pathfindJobId = pathfindJobId;
}
}
}
private static enum Direction {
NORTH_WEST(-1, 1), NORTH(0, 1), NORTH_EAST(1, 1), EAST(1, 0), SOUTH_EAST(1, -1), SOUTH(0, -1),
SOUTH_WEST(-1, -1), WEST(-1, 0);
public static final Direction[] VALUES = values();
private final int xOffset;
private final int yOffset;
private final double length;
private Direction(final int xOffset, final int yOffset) {
this.xOffset = xOffset;
this.yOffset = yOffset;
final double sqrt = Math.sqrt((xOffset * xOffset) + (yOffset * yOffset));
this.length = sqrt;
}
}
public static interface GridMapping {
int getX(PathingGrid grid, float worldX);
int getY(PathingGrid grid, float worldY);
public static final GridMapping CELLS = new GridMapping() {
@Override
public int getX(final PathingGrid grid, final float worldX) {
return grid.getCellX(worldX);
}
@Override
public int getY(final PathingGrid grid, final float worldY) {
return grid.getCellY(worldY);
}
};
public static final GridMapping CORNERS = new GridMapping() {
@Override
public int getX(final PathingGrid grid, final float worldX) {
return grid.getCornerX(worldX);
}
@Override
public int getY(final PathingGrid grid, final float worldY) {
return grid.getCornerY(worldY);
}
};
}
public void update(final CSimulation simulation) {
int workIterations = 0;
JobsLoop: while (!this.moveQueue.isEmpty()) {
this.totalJobLoops++;
final PathfindingJob job = this.moveQueue.peek();
if (!job.jobStarted) {
this.pathfindJobId++;
this.totalIterations = 0;
this.totalJobLoops = 0;
job.jobStarted = true;
System.out.println("starting job with smoothing=" + job.allowSmoothing);
workIterations += 5; // setup of job predicted cost
job.goalX = job.goal.x;
job.goalY = job.goal.y;
job.weightForHittingWalls = 1E9f;
if (!this.pathingGrid.isPathable(job.goalX, job.goalY, job.movementType, job.collisionSize)
|| !isPathableDynamically(job.goalX, job.goalY, job.ignoreIntersectionsWithThisUnit,
job.ignoreIntersectionsWithThisSecondUnit, job.movementType)) {
job.weightForHittingWalls = 5E2f;
}
System.out.println("beginning findNaiveSlowPath for " + job.startX + "," + job.startY + "," + job.goalX
+ "," + job.goalY);
if ((job.startX == job.goalX) && (job.startY == job.goalY)) {
job.queueItem.pathFound(Collections.emptyList(), simulation);
this.moveQueue.poll();
continue JobsLoop;
}
tempRect.set(0, 0, job.collisionSize * 2, job.collisionSize * 2);
if (isCollisionSizeBetterSuitedForCorners(job.collisionSize)) {
job.searchGraph = this.cornerNodes;
job.gridMapping = GridMapping.CORNERS;
System.out.println("using corners");
}
else {
job.searchGraph = this.nodes;
job.gridMapping = GridMapping.CELLS;
System.out.println("using cells");
}
final int goalCellY = job.gridMapping.getY(this.pathingGrid, job.goalY);
final int goalCellX = job.gridMapping.getX(this.pathingGrid, job.goalX);
final Node mostLikelyGoal = job.searchGraph[goalCellY][goalCellX];
mostLikelyGoal.touch(this.pathfindJobId);
final double bestGoalDistance = mostLikelyGoal.point.distance(job.goalX, job.goalY);
Arrays.fill(this.goalSet, null);
this.goals = 0;
for (int i = goalCellX - 1; i <= (goalCellX + 1); i++) {
for (int j = goalCellY - 1; j <= (goalCellY + 1); j++) {
if ((j >= 0) && (j <= job.searchGraph.length)) {
if ((i >= 0) && (i < job.searchGraph[j].length)) {
final Node possibleGoal = job.searchGraph[j][i];
possibleGoal.touch(this.pathfindJobId);
if (possibleGoal.point.distance(job.goalX, job.goalY) <= bestGoalDistance) {
this.goalSet[this.goals++] = possibleGoal;
}
}
}
}
}
final int startGridY = job.gridMapping.getY(this.pathingGrid, job.startY);
final int startGridX = job.gridMapping.getX(this.pathingGrid, job.startX);
job.openSet = new PriorityQueue<>(new Comparator<Node>() {
@Override
public int compare(final Node a, final Node b) {
return Double.compare(f(a), f(b));
}
});
job.start = job.searchGraph[startGridY][startGridX];
job.start.touch(this.pathfindJobId);
if (job.startX > job.start.point.x) {
job.startGridMinX = startGridX;
job.startGridMaxX = startGridX + 1;
}
else if (job.startX < job.start.point.x) {
job.startGridMinX = startGridX - 1;
job.startGridMaxX = startGridX;
}
else {
job.startGridMinX = startGridX;
job.startGridMaxX = startGridX;
}
if (job.startY > job.start.point.y) {
job.startGridMinY = startGridY;
job.startGridMaxY = startGridY + 1;
}
else if (job.startY < job.start.point.y) {
job.startGridMinY = startGridY - 1;
job.startGridMaxY = startGridY;
}
else {
job.startGridMinY = startGridY;
job.startGridMaxY = startGridY;
}
for (int cellX = job.startGridMinX; cellX <= job.startGridMaxX; cellX++) {
for (int cellY = job.startGridMinY; cellY <= job.startGridMaxY; cellY++) {
if ((cellX >= 0) && (cellX < this.pathingGrid.getWidth()) && (cellY >= 0)
&& (cellY < this.pathingGrid.getHeight())) {
final Node possibleNode = job.searchGraph[cellY][cellX];
possibleNode.touch(this.pathfindJobId);
final float x = possibleNode.point.x;
final float y = possibleNode.point.y;
if (pathableBetween(job.ignoreIntersectionsWithThisUnit,
job.ignoreIntersectionsWithThisSecondUnit, job.startX, job.startY, job.movementType,
job.collisionSize, x, y)) {
final double tentativeScore = possibleNode.point.distance(job.startX, job.startY);
possibleNode.g = tentativeScore;
possibleNode.f = tentativeScore + h(possibleNode);
job.openSet.add(possibleNode);
}
else {
final double tentativeScore = job.weightForHittingWalls;
possibleNode.g = tentativeScore;
possibleNode.f = tentativeScore + h(possibleNode);
job.openSet.add(possibleNode);
}
}
}
}
}
while (!job.openSet.isEmpty()) {
Node current = job.openSet.poll();
current.touch(this.pathfindJobId);
if (isGoal(current)) {
final LinkedList<Point2D.Float> totalPath = new LinkedList<>();
Direction lastCameFromDirection = null;
if ((current.cameFrom != null)
&& pathableBetween(job.ignoreIntersectionsWithThisUnit,
job.ignoreIntersectionsWithThisSecondUnit, current.point.x, current.point.y,
job.movementType, job.collisionSize, job.goalX, job.goalY)
&& pathableBetween(job.ignoreIntersectionsWithThisUnit,
job.ignoreIntersectionsWithThisSecondUnit, current.cameFrom.point.x,
current.cameFrom.point.y, job.movementType, job.collisionSize, current.point.x,
current.point.y)
&& pathableBetween(job.ignoreIntersectionsWithThisUnit,
job.ignoreIntersectionsWithThisSecondUnit, current.cameFrom.point.x,
current.cameFrom.point.y, job.movementType, job.collisionSize, job.goalX, job.goalY)
&& job.allowSmoothing) {
// do some basic smoothing to walk straight to the goal if it is not obstructed,
// skipping the last grid location
totalPath.addFirst(job.goal);
current = current.cameFrom;
}
else {
totalPath.addFirst(job.goal);
totalPath.addFirst(current.point);
}
lastCameFromDirection = current.cameFromDirection;
Node lastNode = null;
int stepsBackward = 0;
while (current.cameFrom != null) {
lastNode = current;
current = current.cameFrom;
if ((lastCameFromDirection == null) || (current.cameFromDirection != lastCameFromDirection)
|| (current.cameFromDirection == null)) {
if ((current.cameFromDirection != null) || (lastNode == null)
|| !pathableBetween(job.ignoreIntersectionsWithThisUnit,
job.ignoreIntersectionsWithThisSecondUnit, job.startX, job.startY,
job.movementType, job.collisionSize, current.point.x, current.point.y)
|| !pathableBetween(job.ignoreIntersectionsWithThisUnit,
job.ignoreIntersectionsWithThisSecondUnit, current.point.x, current.point.y,
job.movementType, job.collisionSize, lastNode.point.x, lastNode.point.y)
|| !pathableBetween(job.ignoreIntersectionsWithThisUnit,
job.ignoreIntersectionsWithThisSecondUnit, job.startX, job.startY,
job.movementType, job.collisionSize, lastNode.point.x, lastNode.point.y)
|| !job.allowSmoothing) {
// Add the point if it's not the first one, or if we can only complete
// the journey by specifically walking to the first one
totalPath.addFirst(current.point);
lastCameFromDirection = current.cameFromDirection;
}
}
if (stepsBackward > this.pathingGridCellCount) {
new IllegalStateException(
"PATHING SYSTEM ERROR: The path finding algorithm hit an infinite cycle at or near pt: "
+ current.cameFrom.point
+ ".\nThis means the A* search algorithm heuristic 'admissable' constraint was probably violated.\n\nUnit1:"
+ CUnit.maybeMeaningfulName(job.ignoreIntersectionsWithThisUnit)
+ "\nUnit2:"
+ CUnit.maybeMeaningfulName(job.ignoreIntersectionsWithThisSecondUnit))
.printStackTrace();
totalPath.clear();
break;
}
stepsBackward++;
}
job.queueItem.pathFound(totalPath, simulation);
this.moveQueue.poll();
System.out.println("Task " + this.pathfindJobId + " took " + this.totalIterations
+ " iterations and " + this.totalJobLoops + " job loops!");
continue JobsLoop;
}
for (final Direction direction : Direction.VALUES) {
final float x = current.point.x + (direction.xOffset * 32);
final float y = current.point.y + (direction.yOffset * 32);
if (this.pathingGrid.contains(x, y)) {
double turnCost;
if ((current.cameFromDirection != null) && (direction != current.cameFromDirection)) {
turnCost = 0.25;
}
else {
turnCost = 0;
}
double tentativeScore = current.g + ((direction.length + turnCost) * 32);
if (!pathableBetween(job.ignoreIntersectionsWithThisUnit,
job.ignoreIntersectionsWithThisSecondUnit, current.point.x, current.point.y,
job.movementType, job.collisionSize, x, y)) {
tentativeScore += (direction.length) * job.weightForHittingWalls;
}
final Node neighbor = job.searchGraph[job.gridMapping.getY(this.pathingGrid, y)][job.gridMapping
.getX(this.pathingGrid, x)];
neighbor.touch(this.pathfindJobId);
if (tentativeScore < neighbor.g) {
neighbor.cameFrom = current;
neighbor.cameFromDirection = direction;
neighbor.g = tentativeScore;
neighbor.f = tentativeScore + h(neighbor);
if (!job.openSet.contains(neighbor)) {
job.openSet.add(neighbor);
}
}
}
}
workIterations++;
this.totalIterations++;
if (this.totalIterations > 20000) {
break;
}
if (workIterations >= 1500) {
// breaking jobs loop will implicitly exit without calling pathFound() below
break JobsLoop;
}
}
job.queueItem.pathFound(Collections.emptyList(), simulation);
this.moveQueue.poll();
System.out.println("Task " + this.pathfindJobId + " took " + this.totalIterations + " iterations and "
+ this.totalJobLoops + " job loops!");
}
}
public static final class PathfindingJob {
private final CUnit ignoreIntersectionsWithThisUnit;
private final CUnit ignoreIntersectionsWithThisSecondUnit;
private final float startX;
private final float startY;
private final Point2D.Float goal;
private final MovementType movementType;
private final float collisionSize;
private final boolean allowSmoothing;
private final CBehaviorMove queueItem;
private boolean jobStarted;
public float goalY;
public float goalX;
public float weightForHittingWalls;
Node[][] searchGraph;
GridMapping gridMapping;
PriorityQueue<Node> openSet;
Node start;
int startGridMinX;
int startGridMinY;
int startGridMaxX;
int startGridMaxY;
public PathfindingJob(final CUnit ignoreIntersectionsWithThisUnit,
final CUnit ignoreIntersectionsWithThisSecondUnit, final float startX, final float startY,
final Point2D.Float goal, final PathingGrid.MovementType movementType, final float collisionSize,
final boolean allowSmoothing, final CBehaviorMove queueItem) {
this.ignoreIntersectionsWithThisUnit = ignoreIntersectionsWithThisUnit;
this.ignoreIntersectionsWithThisSecondUnit = ignoreIntersectionsWithThisSecondUnit;
this.startX = startX;
this.startY = startY;
this.goal = goal;
this.movementType = movementType;
this.collisionSize = collisionSize;
this.allowSmoothing = allowSmoothing;
this.queueItem = queueItem;
this.jobStarted = false;
}
}
}