I'm having an issue with my algorithm that is supposed to implement dijkstra's algorithm for path finding.
The issue is that occasionally the system locks up and I think it has to do with either connections not being made properly, nodes not updating properly or something in my lists that isn't being added (so it kinda sounds like what I'm saying is that nothing works)...except that in SOME cases, it works and finds the correct path.
I've simplified the grid by making the map size small since it's easier to see if there are errors in the outputs. Hopefully someone has an idea of where I am going wrong. I did post something similar to this in gameDev but it got no responses and I've moved passed that problem and onto this one.
This was made with libgdx.
Connections.java
package com.comp452.tme21;
public class Connections {
private int cost;
private int x;
private int y;
public Connections(int cost, int x, int y) {
this.cost = cost;
this.x = x;
this.y = y;
}
public int getCost() {
return cost;
}
public void setCost(int cost) {
this.cost = cost;
}
public int getX() {
return x;
}
public void setX(int x) {
this.x = x;
}
public int getY() {
return y;
}
public void setY(int y) {
this.y = y;
}
}
FullNodeComparator.java
package com.comp452.tme21;
import java.util.Comparator;
public class FullNodeComparator implements Comparator<MapNode> {
@Override
public int compare(MapNode n1, MapNode n2) {
if (n1.getX() == n2.getX()) {
if (n1.getY() == n2.getY()) {
return 1;
}
}
return 0;
}
}
MapNode.java
package com.comp452.tme21;
import java.util.ArrayList;
public class MapNode {
private int costSoFar;
private int x;
private int y;
private int connectionX;
private int connectionY;
private final ArrayList connectionsList;
private final int MAPSIZE = Pathfinder.MAPSIZE - 1;
public ArrayList getConnectionsList() {
return connectionsList;
}
public MapNode(int costSoFar, int x, int y, int connectionX, int connectionY) {
this.costSoFar = costSoFar;
this.x = x;
this.y = y;
this.connectionX = connectionX;
this.connectionY = connectionY;
connectionsList = new ArrayList();
}
public int getCostSoFar() {
return costSoFar;
}
public void setCostSoFar(int costSoFar) {
this.costSoFar = costSoFar;
}
public int getX() {
return x;
}
public void setX(int x) {
this.x = x;
}
public int getY() {
return y;
}
public void setY(int y) {
this.y = y;
}
public int getConnectionX() {
return connectionX;
}
public void setConnectionX(int connectionX) {
this.connectionX = connectionX;
}
public int getConnectionY() {
return connectionY;
}
public void setConnectionY(int connectionY) {
this.connectionY = connectionY;
}
public void setConnections() {
int tileMap[][] = Pathfinder.getTileMap();
int testX = 0;
int testY = 0;
// Is node not at the top of the map...
if (this.y != MAPSIZE) {
// If not obstacle...
if (tileMap[this.x][this.y + 1] != 0) {
// If connection isn't back to start node...
testX = this.x;
testY = this.y + 1;
if (testX == 1 && testY == 1) {
}
else {
// Add the connection to the node directly above.
connectionsList.add(new Connections(tileMap[this.x][this.y + 1], this.x, this.y + 1));
System.out.println("TM Added connection from " + this.x + "," + this.y + " to " + this.x + "," + (this.y+1));
}
}
// Is node not at the left side of the map...
if (this.x != 1) {
// If not obstacle...
if (tileMap[this.x - 1][this.y + 1] != 0) {
// If connection isn't back to start node...
testX = this.x - 1;
testY = this.y + 1;
if (testX == 1 && testY == 1) {
}
else {
// Add the connection to the top left node.
connectionsList.add(new Connections(tileMap[this.x - 1][this.y + 1], this.x - 1, this.y + 1));
System.out.println("TL Added connection from " + this.x + "," + this.y + " to " + (this.x-1) + "," + (this.y+1));
}
}
}
// Is node not at the right side of the map...
if (this.x != MAPSIZE) {
// If not obstacle...
if (tileMap[this.x + 1][this.y + 1] != 0) {
// If connection isn't back to start node...
testX = this.x + 1;
testY = this.y + 1;
if (testX == 1 && testY == 1) {
}
else {
// Add the connection to the top right node.
connectionsList.add(new Connections(tileMap[this.x + 1][this.y + 1 ], this.x + 1, this.y + 1));
System.out.println("TR Added connection from " + this.x + "," + this.y + " to " + (this.x+1) + "," + (this.y+1));
}
}
}
}
// Is node not at the left side of the screen...
if (this.x != 1) {
// If not obstacle...
if (tileMap[this.x - 1][this.y] != 0) {
// If connection isn't back to start node...
testX = this.x - 1;
testY = this.y;
if (testX == 1 && testY == 1) {
}
else {
// Add the connection to the left node.
connectionsList.add(new Connections(tileMap[this.x - 1][this.y], this.x - 1, this.y));
System.out.println("LM Added connection from " + this.x + "," + this.y + " to " + (this.x-1) + "," + this.y);
}
}
}
// Is node not at the right side of the screen...
if (this.x != MAPSIZE) {
// If not obstacle...
if (tileMap[this.x + 1][this.y] != 0) {
// If connection isn't back to start node...
testX = this.x + 1;
testY = this.y;
if (testX == 1 && testY == 1) {
}
else {
// Add the connection to the right node.
connectionsList.add(new Connections(tileMap[this.x + 1][this.y], this.x + 1, this.y));
System.out.println("RM Added connection from " + this.x + "," + this.y + " to " + (this.x+1) + "," + this.y);
}
}
}
// Is node not at the bottom of the map...
if (this.y != 1) {
// If not obstacle...
if (tileMap[this.x][this.y - 1] != 0) {
// If connection isn't back to start node...
testX = this.x;
testY = this.y - 1;
if (testX == 1 && testY == 1) {
}
else {
// Add the connection to the node directly below.
connectionsList.add(new Connections(tileMap[this.x][this.y - 1 ], this.x, this.y - 1));
System.out.println("BM Added connection from " + this.x + "," + this.y + " to " + this.x + "," + (this.y - 1));
}
}
// Is node not at the left side of the map...
if (this.x != 1) {
// If not obstacle...
if (tileMap[this.x - 1][this.y - 1] != 0) {
// If connection isn't back to start node...
testX = this.x - 1;
testY = this.y - 1;
if (testX == 1 && testY == 1) {
}
else {
// Add the connection to the bottom left node.
connectionsList.add(new Connections(tileMap[this.x - 1][this.y - 1], this.x - 1, this.y - 1));
System.out.println("BL Added connection from " + this.x + "," + this.y + " to " + (this.x-1) + "," + (this.y-1));
}
}
}
// Is node not at the right side of the map...
if (this.x != MAPSIZE) {
// If not obstacle...
if (tileMap[this.x + 1][this.y - 1] != 0) {
// If connection isn't back to start node...
testX = this.x + 1;
testY = this.y - 1;
if (testX == 1 && testY == 1) {
}
else {
// Add the connection to the bottom right node.
connectionsList.add(new Connections(tileMap[this.x + 1][this.y - 1], this.x + 1, this.y - 1));
System.out.println("BR Added connection from " + this.x + "," + this.y + " to " + (this.x+1) + "," + (this.y-1));
}
}
}
}
}
public int compareTo(MapNode n2) {
if (x == n2.getX()) {
if (y == n2.getY()) {
return 1;
}
}
return 0;
}
}
NodeComparator.java
package com.comp452.tme21;
import java.util.Comparator;
public class NodeComparator implements Comparator<MapNode> {
@Override
public int compare(MapNode n1, MapNode n2) {
return Integer.compare(n1.getCostSoFar(), n2.getCostSoFar());
}
}
PathFinder.java
package com.comp452.tme21;
import com.badlogic.gdx.ApplicationAdapter;
import com.badlogic.gdx.Gdx;
import com.badlogic.gdx.graphics.GL20;
import com.badlogic.gdx.graphics.Texture;
import com.badlogic.gdx.graphics.g2d.Sprite;
import com.badlogic.gdx.graphics.g2d.SpriteBatch;
import java.util.ArrayList;
import java.util.Collections;
import java.util.Random;
import java.util.Stack;
public class Pathfinder extends ApplicationAdapter {
protected final static int MAPSIZE = 5; //17
SpriteBatch batch;
Texture openTile, grassTile, swampTile, obstacleTile;
Texture startTile, finishTile,drawingTile;
private Player player;
private Sprite playerSprite;
private Texture texture;
private final static int[][] tileMap = new int[MAPSIZE][MAPSIZE];
public static int[][] getTileMap() {
return tileMap;
}
private int startX;
private int startY;
private int finishX;
private int finishY;
private ArrayList closed;
private ArrayList open;
private MapNode startNode;
private MapNode currentNode;
private MapNode nextNode;
private MapNode testNode;
private ArrayList nodeConnectionsList;
private Connections nextConnection;
private Stack pathList;
@Override
public void create () {
generateTiles();
batch = new SpriteBatch();
openTile = new Texture("open.jpg");
grassTile = new Texture("grass.jpg");
swampTile = new Texture("swamp.jpg");
obstacleTile = new Texture("obstacle.jpg");
startTile = new Texture("start.jpg");
finishTile = new Texture("finish.jpg");
texture = new Texture(Gdx.files.internal("player.png"));
playerSprite = new Sprite(texture);
player = new Player(playerSprite);
player.setGridX(startX);
player.setGridY(startY);
findThePath();
}
@Override
public void render () {
update(Gdx.graphics.getDeltaTime());
Gdx.gl.glClearColor(0, 0, 0, 1);
Gdx.gl.glClear(GL20.GL_COLOR_BUFFER_BIT);
batch.begin();
drawTiles();
player.draw(batch);
batch.end();
}
public void drawTiles() {
for (int i = 1; i < MAPSIZE; i ++) {
for (int j = 1; j < MAPSIZE; j++) {
if (tileMap[i][j] == 1) {
drawingTile = openTile;
}
else if (tileMap[i][j] == 3) {
drawingTile = grassTile;
}
else if (tileMap[i][j] == 4) {
drawingTile = swampTile;
}
else if (tileMap[i][j] == 0) {
drawingTile = obstacleTile;
}
else if (tileMap[i][j] == -1) {
drawingTile = startTile;
}
else if (tileMap[i][j] == 2) {
drawingTile = finishTile;
}
batch.draw(drawingTile, i * 32, j * 32);
}
}
}
public void update(float dt) {
player.update(dt);
}
public void generateTiles() {
int cost = 0;
Random rand = new Random();
for (int i = 1; i < MAPSIZE; i++) {
for (int j = 1; j < MAPSIZE; j++) {
int randomNum = rand.nextInt((4-1) +1) + 1;
switch (randomNum) {
case 1:
cost = 1;
break;
case 2:
cost = 3;
break;
case 3:
cost = 4;
break;
case 4:
cost = 0;
break;
}
tileMap[i][j] = cost;
}
}
startX = rand.nextInt(((MAPSIZE - 1) -1) +1) + 1;
startY = rand.nextInt(((MAPSIZE - 1) -1) +1) + 1;
startX = 1;
startY = 1;
tileMap[startX][startY] = -1;
finishX = rand.nextInt(((MAPSIZE - 1) -1) +1) + 1;
finishY = rand.nextInt(((MAPSIZE - 1) -1) +1) + 1;
finishX = 4;
finishY = 4;
tileMap[finishX][finishY] = 2;
}
public void findThePath() {
System.out.println("Start node is at " + startX + " " + startY);
startNode = new MapNode(0, startX, startY, 0, 0);
closed = new ArrayList();
open = new ArrayList();
open.add(startNode);
closed.clear();
while (open.size() > 0) {
Collections.sort(open, new NodeComparator());
currentNode = (MapNode) open.get(0);
currentNode.setConnections();
boolean nodeIsClosed = false;
boolean nodeIsOpen = false;
// If the smallest element is the goal node, stop.
if (currentNode.getX() == finishX && currentNode.getY() == finishY) {
System.out.println("Found last node at " + currentNode.getX() + " " + currentNode.getY());
closed.add(currentNode);
break;
}
else {
nodeConnectionsList = currentNode.getConnectionsList();
for (int i = 0; i < nodeConnectionsList.size(); i++) {
nextConnection = (Connections) nodeConnectionsList.get(i);
int nextX = nextConnection.getX();
int nextY = nextConnection.getY();
int nextCost = nextConnection.getCost();
nextNode = new MapNode(currentNode.getCostSoFar() + nextCost, nextX, nextY, currentNode.getX(), currentNode.getY());
// If node is already in the closed list, set flag to continue.
for (int j = 0; j < closed.size(); j++) {
testNode = (MapNode) closed.get(j);
if (nextNode.compareTo(testNode) == 1) {
nodeIsClosed = true;
}
}
// If flag is set, continue to next iteration.
if (nodeIsClosed) {
nodeIsClosed = false;
continue;
}
// If node is already in open list, check to see if it's better.
for (int j = 0; j < open.size(); j++) {
testNode = (MapNode) open.get(j);
if (nextNode.compareTo(testNode) == 1) {
nodeIsOpen = true;
if (nextNode.getCostSoFar() > testNode.getCostSoFar()){
// Do nothing.
break;
}
else {
System.out.println("Found better route to " + nextNode.getX() + " " + nextNode.getY());
System.out.println("From " + currentNode.getX() + " " + currentNode.getY());
testNode.setCostSoFar(nextNode.getCostSoFar());
testNode.setConnectionX(currentNode.getX());
testNode.setConnectionY(currentNode.getY());
break;
}
}
}
if (nodeIsOpen) {
nodeIsOpen = false;
}
else {
open.add(nextNode);
}
}
closed.add(new MapNode(currentNode.getCostSoFar(), currentNode.getX(), currentNode.getY(), currentNode.getConnectionX(), currentNode.getConnectionY()));
}
open.remove(0);
}
int totalCost = 0;
pathList = new Stack();
// Find the end node and get it's first connection.
for (int i = 0; i < closed.size(); i++) {
testNode = (MapNode) closed.get(i);
if (testNode.getX() == finishX) {
if (testNode.getY() == finishY) {
pathList.push(new Connections(totalCost, finishX, finishY));
System.out.println("finish x: " + finishX);
System.out.println("finish y: " + finishY);
System.out.println("FromX: " + testNode.getConnectionX());
System.out.println("FromY: " + testNode.getConnectionY());
totalCost = testNode.getCostSoFar();
System.out.println("Total Cost: " + totalCost);
System.out.println("---------");
closed.remove(i);
break;
}
}
}
int findingX = testNode.getConnectionX();
int findingY = testNode.getConnectionY();
do {
for (int i = 0; i < closed.size(); i++) {
testNode = (MapNode) closed.get(i);
if (testNode.getX() == findingX) {
if (testNode.getY() == findingX) {
System.out.println("x: " + findingX);
System.out.println("y: " + findingY);
System.out.println("FromX: " + testNode.getConnectionX());
System.out.println("FromY: " + testNode.getConnectionY());
totalCost = testNode.getCostSoFar();
System.out.println(totalCost);
System.out.println("---------");
pathList.push(new Connections(totalCost, findingX, findingY));
findingX = testNode.getConnectionX();
findingY = testNode.getConnectionY();
closed.remove(i);
break;
}
}
}
if (closed.isEmpty()) {
System.out.println("Closed list is empty");
break;
}
} while((findingX != 0 && findingY != 0) && (findingX != startX && findingY != startY));
Connections testConnection;
System.out.println("Path Size: " + pathList.size());
while (!pathList.empty()) {
testConnection = (Connections) pathList.pop();
System.out.println("TC X: " + testConnection.getX());
System.out.println("TC Y: " + testConnection.getY());
}
System.out.println("Done");
}
}
Player.java
package com.comp452.tme21;
import com.badlogic.gdx.graphics.g2d.Batch;
import com.badlogic.gdx.graphics.g2d.Sprite;
public class Player extends Sprite {
public int gridX;
public int gridY;
public Player(Sprite sprite) {
super(sprite);
}
@Override
public void draw(Batch batch) {
batch.draw(this.getTexture(), this.getX(), this.getY());
}
public void update(float dt) {
}
public int getCenterX() {
int centerX = (int) (getX() + getWidth() /2);
return centerX;
}
public int getCenterY() {
int centerY = (int) (getY() + getHeight() /2);
return centerY;
}
public int getGridX() {
int gX = (int) (getX() / 32);
return gX;
}
public int getGridY() {
int gY = (int) (getY() / 32);
return gY;
}
public void setGridX(int x) {
setX(x * 32);
}
public void setGridY(int y) {
setY(y * 32);
}
}
So as it turns out, it was a pretty simple and stupid mistake.
Look at this line of code. Can you see the error? It took me hours of searching and tons and tons of println statements at just about every inch of code (my version of debugging) and I finally saw it. After correcting the issue, everything works as expected (with some minor additions and code cleanup, as well as a catch if no path exists...but this was never the problem).
if (testNode.getX() == findingX) {
if (testNode.getY() == findingX) {
Once I finally saw this I thought "how stupid!!!". Do you see it now?