I have been reading some articles to learn A* pathfinding and I was able to make a test program based on it, but the the program I made is giving strange paths which are not a shortest path to the target node.
In the images, the green square unit represents the starting node, the red unit represents target node, blue units are impassable tiles (walls) and the purple units represent the path found from starting node to target node
https://i.sstatic.net/xwhuk.jpg
https://i.sstatic.net/B0WxU.jpg
If anybody could find a problem with the pathfinding source code I would be much thankful. I'm burned out from trying to know what caused it to act strange.
Its allowed to cut corners and go diagonal
package com.streak324.pathfinding;
import java.util.Comparator;
import java.util.HashSet;
import java.util.PriorityQueue;
import com.badlogic.gdx.utils.Array;
public class PathFinder {
private boolean foundTarget;
private int width, height;
//list of nodes that leads from starting node to target node is stored here
private Array<PathNode> path;
//all nodes stored in this array
private PathNode[][] nodes;
private PriorityQueue<PathNode> open;
private HashSet<PathNode> closed;
//nodes that must be referenced
private PathNode start, target, current;
//how far the current node can reach for other nodes from its own position
private int range = 1;
public PathFinder(int width, int height, boolean map[][]) {
this.width = width;
this.height = height;
nodes = new PathNode[width][height];
for(int i=0; i<width; i++) {
for(int j=0; j<height; j++) {
nodes[i][j] = new PathNode(i, j);
//if wall tile is spotted, mark the node unwalkable
if(map[i][j] != true) { nodes[i][j].passable = false; }
}
}
open = new PriorityQueue<PathNode>(new CostComparator());
closed = new HashSet<PathNode>();
}
public Array<PathNode> getPath(int sx, int sy ,int tx, int ty) {
path = new Array<PathNode>();
open.clear();
closed.clear();
start = nodes[sx][sy];
start.movementCost = 0;
addToOpenList(start);
target = nodes[tx][ty];
while(foundTarget != true) {
if(open.size() == 0) { return null; }
current = open.poll();
addToClosedList(current);
checkNeighbors(current);
}
traceBack();
return path;
}
// makes its way back adding the parent node until start
private void traceBack() {
while(current != start) {
path.add(current);
current = current.parent;
}
}
//checks for nodes within certain range
private void checkNeighbors(PathNode node) {
//continues loop if i or j goes out of bounds of nodes array
for(int i = node.x - range; i <= (node.x + range); i++) {
if(i >= width || i < 0) { continue; }
for(int j = node.y - range; j <= (node.y + range); j++) {
if( j >= height || j < 0) { continue; }
if((i == node.x && j == node.y) ) { continue; }
PathNode neighbor = nodes[i][j];
identifyNode(neighbor);
}
}
}
//if node is not on open list, add node and calculate it
private void identifyNode(PathNode node) {
if(!node.passable || closed.contains(node) ) return;
if(node == target) {
foundTarget = true;
System.out.println("Target Found: " + node.x + ", " + node.y);
return;
}
else if(!open.contains(node)) {
addToOpenList(node);
calcHeuristic(node);
updateNode(node, current);
}
else {
checkForReparenting(node);
}
}
//is the movement cost less to go from the current node?
private void checkForReparenting(PathNode node) {
float cost = node.movementCost;
float reCost = calcMovementCost(node, current);
if(reCost < cost) {
System.out.println("Reparenting");
updateNode(node, current);
}
}
//updates parent and cost
private void updateNode(PathNode child, PathNode parent) {
child.parent = parent;
child.movementCost = calcMovementCost(child, parent);
child.totalCost = child.movementCost + child.heuristic;
}
private float calcMovementCost(PathNode n1, PathNode n2) {
float dx = n1.x - n2.x;
float dy = n1.y - n2.y;
return (float) Math.sqrt( (dx*dx + dy*dy) ) + n2.movementCost;
}
private float calcHeuristic(PathNode node) {
float dx = node.x - target.x;
float dy = node.y - target.y;
return (float) Math.sqrt( (dx*dx + dy*dy) );
}
private void addToOpenList(PathNode node) {
if(!open.contains(node) && !closed.contains(node)) {
open.add(node);
}
}
private void addToClosedList(PathNode node) {
if(!closed.contains(node)) {
closed.add(node);
}
}
public class PathNode {
public int x, y;
public PathNode parent;
//g, h and f
public float movementCost, heuristic, totalCost;
public boolean passable;
public PathNode(int x, int y) {
this.x = x;
this.y = y;
passable = true;
}
}
private class CostComparator implements Comparator<PathNode> {
@Override
public int compare(PathNode a, PathNode b) {
if(a.totalCost < b.totalCost) return 1;
else return -1;
}
}
}
no comments http://pastebin.com/rSv7pUrB
I'm guessing something is wrong in the way that the priority queue is ordering the elements or I may have not properly calculated the totalCost, movementCost, and heuristic variables, but I see nothing wrong with it.
Someone that could point me to the right direction of a probable problem or solution is much appreciated
There are several issues with your code:
You never really use the heuristic. The following statement (the only call to calcHeuristic) just "throws the result away".
calcHeuristic(node);
That alone can't be the error here, since it's a valid admissible heuristic to guess the distance to the target to be 0. However the algorithm degenerates that way (to what I think is the Dijkstra algorithm).
You never update the position of the node in the priority queue. That means a node with updated totalDistance will never move up in the proirity queue, even if it's totalCost becomes less than the totalCost of another node. You have to remove the node and add it again to do that with a PriorityQueue:
open.remove(node);
// ... update totalDistance
open.add(node);
totalDistance is equal to the real distance, for expanded neighbors of the target IF you use the heuristic; here the distance real distance is different by either sqrt(2) or 1). In general the distance heuristic for the last step can be arbitrary bad (and here it's bad, see (1.)) and you can only be sure you found the real solution, if you run the algorithm to the point where you would expand the target node.