package com.brice.pathfinding;
import com.badlogic.gdx.utils.Array;
import com.badlogic.gdx.utils.GdxRuntimeException;
/**
* A pathing class based on the popular A* algorithm. The heuristic used is the Manhattan method.
*
*
* <p>Reference:<br />
* G: The movement cost to to move to a square. In this case, we're making diagnal movement 1.414 * the cost of horizontal movement (if horizontal costs 10 movement, diagnal would roughly be 14).<br />
* H: Heuristic; estimated cost from this square to destination (using Manhattan Heuristic in this case)<br />
* F: G + H</p>
*
* <p>Once created, use step (for one pathing logic step) or run (to process the whole path). The complete path is returned in an {@link Array} of {@link PathNode}s.<br />
* If returnBestOnFail flag is true, even if no path is found, a path with the closest endpoint is returned.</p>
*
* <p>The cutCorners flag means that the pathing will not travel diagnally through a wall when determining a path (corners will always be right angles).</p>
*
* @author Brice
*/
public class AStar {
private boolean isDone;
private boolean pathFound;
private Array<PathNode> openList;
private Array<PathNode> closedList;
private int startX;
private int startY;
private int destX;
private int destY;
private Array<PathNode> path;
private int nodeWidth;
private int maxNodes;
/**
* 2D array of the cost to move to a given node.<br />
* Special cases are: -1(Blocks NPCs Only) -2(Blocks Players Only) -3(A wall: blocks everything)
*/
private static MapNode[][] mapNodes;
private int horizontalMovementCost;
private int diagnalMovementCost;
private boolean cutCorners;
private boolean returnBestOnFail;
private PathNode currentNode = null;
private int mapWidth;
private int mapHeight;
/**
* Creates an AStar object with parameters that are not likely to change.
* You may then call AStar.clear().setPoints(sx, sy, ex, ey).run() to obtain a new path.
* @param nWidth Width/Height of a tile (Can be any small number, this is only used for the cost of horizontal movement; 10 is a good number).
* @param maxNodes The maximum nodes in the closed list before we give up.
* @param mapWidth Number of tiles wide the map is, so total width is mapWidth * nWidth.
* @param mapHeight Number of tiles high the map is, so total width is mapHeight * nWidth.
*/
public AStar(int nWidth, int maxNodes, int mapWidth, int mapHeight) {
if(mapWidth <= 0 || mapHeight <= 0)
throw new RuntimeException("Map height and width must be positive.");
mapNodes = new MapNode[mapHeight][mapWidth];
for(int y=0; y<mapNodes.length; y++)
for(int x=0; x<mapNodes[0].length; x++)
mapNodes[y][x] = new MapNode();
this.nodeWidth = nWidth;
this.maxNodes = maxNodes;
this.mapWidth = mapWidth;
this.mapHeight = mapHeight;
// Movement costs. Diagonal movement is Math.sqrt(2), or roughly 1.414 times the cost of 90 degree movement
this.horizontalMovementCost = nWidth;
this.diagnalMovementCost = (int) Math.floor(nWidth * 1.414);
openList = new Array<PathNode>(true, 10, PathNode.class);
closedList = new Array<PathNode>(true, 10, PathNode.class);
}
/**
* Sets the starting and ending points for a path.
* @param sX
* @param sY
* @param dX
* @param dY
* @return this object, for chaining.
*/
public AStar setPoints(int sX, int sY, int dX, int dY) {
// Clamp values to map
if(sX < 0 || sY < 0 || dX < 0 || dY < 0 ||
sX > mapWidth || sY > mapHeight || dX > mapWidth || dY > mapHeight)
throw new RuntimeException("The origin point, or destination point is outside the bounds of the world.");
this.startX = sX;
this.startY = sY;
this.destX = dX;
this.destY = dY;
PathNode start = new PathNode(sX, sY);
start.f = 10000; // Something high so the start is not picked as the current node after the first time.
openList.add(start);
return this;
}
/**
* Clears open and closed lists.
* @return this object, for chaining.
*/
public AStar reset() {
openList.clear();
closedList.clear();
isDone = false;
pathFound = false;
return this;
}
/**
* Run the whole path all at once.
* @param caller One of {@link MapNode.BLOCK_NPC}, {@link MapNode.BLOCK_PLAYER} or {@link MapNode.BLOCK_ALL}
* @return
*/
public Array<PathNode> run(int caller) {
while(true) {
// if resolve returns false, there is no path.
if(!pathStep(caller)) {
isDone = true;
if(returnBestOnFail)
return completePath();
break;
}
// Check to see if the current Node is our end, if so fill the path array and return true.
if(currentNode.x == destX && currentNode.y == destY) {
isDone = true;
return completePath();
}
}
return null;
}
/**
* Run through the pathing one step at a time.
* Good for animating the path.
* @param caller One of {@link MapNode.BLOCK_NPC}, {@link MapNode.BLOCK_PLAYER} or {@link MapNode.BLOCK_ALL}
* @return
*/
public Array<PathNode> step(int caller) {
// Check for no path
if(!pathStep(caller)) {
isDone = true;
if(returnBestOnFail)
return completePath();
}
else if(currentNode.x == destX && currentNode.y == destY) {
isDone = true;
return completePath();
}
return null;
}
/**
* Traverses back up a complete path.
* @return The path in an array of {@link PathNode}s.
*/
private Array<PathNode> completePath() {
path = new Array<PathNode>(true, 10, PathNode.class);
PathNode pn = null;
// Use the end node, or best path we can find if we failed and returnBestOnFail is true;
if(returnBestOnFail) {
pn = closedList.items[closedList.size-1];
for(int i=0; i<closedList.size; i++) {
if(Math.abs(destX - closedList.items[i].x) + Math.abs(destY - closedList.items[i].y)
< Math.abs(destX - pn.x) + Math.abs(destY - pn.y) )
pn = closedList.items[i];
}
}
else
pn = closedList.items[closedList.size-1];
// Run through closed list backward and build the path based on the parents.
path.add(pn);
while(pn.parent != null) {
// TODO: Small optimization: Check nodes going backward for lowest G, you can find a faster path sometimes.
path.add(pn.parent);
pn = pn.parent;
}
pathFound = true;
return path;
}
/**
* This function is a single step in the process. It can be called one at a time for demonstration
* purposes, or looped until false returns(no path), or until the current node == the end node (see run(), or step()).
* @param caller
* @return false if the path cannot continue (there are no more spaces, or we looked past the maximum attempts).
*/
private boolean pathStep(int caller) {
// make the first item in open list the current node...
if(openList.size > 0)
currentNode = openList.items[0];
// There is no path if the open list is empty.
else
return false;
// Check to see if path is too long... give up!
if(closedList.size > maxNodes)
return false;
// Look for lowest F cost node on the open list and make it the current node
for(int i=0; i<openList.size; i++) {
if(openList.items[i].f < currentNode.f)
currentNode = openList.items[i];
}
if(currentNode != null) {
// switch current node to closed list
closedList.add(currentNode);
openList.removeValue(currentNode, true);
int xOffset = 0;
int yOffset = 0;
// for each of the 8 nodes adjacent to the current node...
for(int i=0; i<8; i++) {
boolean diagnal = false;
switch(i) {
case(0): xOffset = -1; yOffset = -1; diagnal = true; break; // Lower left
case(1): xOffset = 0; yOffset = -1; break; // Lower center
case(2): xOffset = 1; yOffset = -1; diagnal = true; break; // lower right
case(3): xOffset = -1; yOffset = 0; break; // Left
case(4): xOffset = 1; yOffset = 0; break; // Right
case(5): xOffset = -1; yOffset = 1; diagnal = true; break; // Upper left
case(6): xOffset = 0; yOffset = 1; break; // Upper center
case(7): xOffset = 1; yOffset = 1; diagnal = true; break; // Upper right
default: xOffset = 0; yOffset = 0; diagnal = false; break; // Center (should never happen)
}
// First... make sure we're in bounds
if(currentNode.x+xOffset < 0 || currentNode.y+yOffset < 0 || currentNode.x+xOffset > mapNodes[0].length-1 || currentNode.y+yOffset > mapNodes.length-1)
continue;
// Ignore if it's on the closed list
if(isOnList(closedList, currentNode.x+xOffset, currentNode.y+yOffset))
continue;
// Ignore if the node if it is not walkable by the calling type
if(mapNodes[currentNode.y+yOffset][currentNode.x+xOffset].blocks(caller))
continue;
// // Ignore if we're a NPC, and the cost is -1 (NPC blocker)
// if(caller == CallerType.NPC && mapNodes[currentNode.y+yOffset][currentNode.x+xOffset].blocks(MapNode.BLOCK_NPC))
// continue;
//
// // Ignore if we're a player, and the cost is -2 (player blocker)
// if(caller == CallerType.PLAYER && mapNodes[currentNode.y+yOffset][currentNode.x+xOffset].blocks(MapNode.BLOCK_PLAYER))
// continue;
// Check to not cut corners diagonally if there's a wall
if(!cutCorners && diagnal) {
// Don't worry about checking out of bounds... if the corner is in bounds, then the centers have to be also.
// Upper left: Check up and left for walls
if(xOffset == -1 && yOffset == 1) {
if(mapNodes[currentNode.y][currentNode.x-1].blocks(caller) || mapNodes[currentNode.y+1][currentNode.x].blocks(caller) )
continue;
}
// Upper right: Check up and right for walls
else if(xOffset == 1 && yOffset == 1) {
if(mapNodes[currentNode.y][currentNode.x+1].blocks(caller) || mapNodes[currentNode.y+1][currentNode.x].blocks(caller) )
continue;
}
// Lower right: Check down and right for walls
else if(xOffset == 1 && yOffset == -1) {
if(mapNodes[currentNode.y][currentNode.x+1].blocks(caller) || mapNodes[currentNode.y-1][currentNode.x].blocks(caller) )
continue;
}
// Lower left: Check down and left for walls
else if(xOffset == -1 && yOffset == -1) {
if(mapNodes[currentNode.y][currentNode.x-1].blocks(caller) || mapNodes[currentNode.y-1][currentNode.x].blocks(caller) )
continue;
}
}
// If it isn't on open list, add to open list
if(!isOnList(openList, currentNode.x+xOffset, currentNode.y+yOffset)) {
PathNode pn = new PathNode(currentNode.x+xOffset, currentNode.y+yOffset);
// Set parent to current node
pn.parent = currentNode;
// Set G (movement cost to move to this square)
if(diagnal)
pn.g = currentNode.g + this.diagnalMovementCost + mapNodes[pn.y][pn.x].cost;
else
pn.g = currentNode.g + this.horizontalMovementCost + mapNodes[pn.y][pn.x].cost;
// Set H (using the heuristic -- Manhattan Method in this case)
pn.h = (Math.abs(this.destX - pn.x) + Math.abs(this.destY - pn.y)) * horizontalMovementCost;
// Set F cost
pn.f = pn.g + pn.h;
// Add to open list
openList.add(pn);
}
// Already on the open list
else {
PathNode pn = getNode(openList, currentNode.x+xOffset, currentNode.y+yOffset);
if(pn == null)
throw new GdxRuntimeException("PathNode at ("+(currentNode.x+xOffset)+","+(currentNode.y+yOffset)+") claimed to be on the open list but returned null.");
// Calculate total g cost to move to potential square THROUGH the current square
int gThroughCurrent = 0;
if(diagnal)
gThroughCurrent = currentNode.g + diagnalMovementCost;
else
gThroughCurrent = currentNode.g + horizontalMovementCost;
// If it's shorter to go through our current square
if(gThroughCurrent > pn.g) {
// TODO: This optimization is broken, and I don't understand it.
// currentNode.parent = pn;
// currentNode.f = gThroughCurrent + currentNode.h;
}
}
}
}
return true;
}
private PathNode getNode(Array<PathNode> list, int x, int y) {
for(int i=0; i<list.size; i++)
if(list.items[i].x == x && list.items[i].y == y)
return list.items[i];
return null;
}
private boolean isOnList(Array<PathNode> list, int x, int y) {
for(int j=0; j<list.size; j++)
if(list.items[j].x == x && list.items[j].y == y)
return true;
return false;
}
public int getMaxNodes() {
return maxNodes;
}
/**
* Sets the max nodes on the closed list before giving up the search.
* @param maxNodes
* @return AStar object for chaining.
*/
public AStar setMaxNodes(int maxNodes) {
this.maxNodes = maxNodes;
return this;
}
/**
* Sets whether cutting corners diagnally is a valid path or not.
* @param cutCorners
* @return AStar object for chaining.
*/
public AStar setCutCorners(boolean cutCorners) {
this.cutCorners = cutCorners;
return this;
}
/**
* Instructs the pathing to return the best path available if the destination is unreachable.
* @param b
* @return
*/
public AStar setReturnBestOnFail(boolean b) {
returnBestOnFail = b;
return this;
}
public MapNode[][] getMap() {
return mapNodes;
}
public Array<PathNode> getPath() {
return path;
}
public boolean pathFound() {
return pathFound;
}
public boolean isDone() {
return isDone;
}
public void setPathCost(int x, int y, short newCost) {
if(x < 0 || y < 0)
return;
if(y > mapNodes.length || x > mapNodes[0].length)
return;
mapNodes[y][x].cost = newCost;
}
public void setBlocking(int x, int y, int blockId) {
if(x < 0 || y < 0)
return;
if(y > mapNodes.length || x > mapNodes[0].length)
return;
mapNodes[y][x].block(blockId);
}
public boolean isBlockOpen(int blockX, int blockY) {
if(blockX < 0 || blockY < 0)
return false;
if(blockY > mapNodes.length-1 || blockX > mapNodes[0].length-1)
return false;
return mapNodes[blockY][blockX].cost >= 0;
}
public Array<PathNode> getOpenList() {
return openList;
}
public Array<PathNode> getClosedList() {
return closedList;
}
}