So I've got some code here that I whipped up in an attempt to get A* pathfinding to work. It's definitely calculating, but not at all reaching the goal.
I haven't yet written any code to get the path, and haven't implemented replacing parents with new, shorter parent paths. For now I'm just hoping I can get atGoal() to return true.
The code isn't exactly beautiful but it's fairly neat and legible. Square is a nearly empty class, it just uses Graphics2D to make a visible square.
Then the accompanying classes:
If anyone takes a look over this and sees something wrong (I may have messed up compareTo(), since I've never implemented Comparable before), any advice is appreciated!
import greenfoot.*; // (World, Actor, GreenfootImage, Greenfoot and MouseInfo)
import java.util.List;
import java.util.ArrayList;
import java.util.Collections;
public class PathWorld extends World
{
final int mult = 20;
List<Node> open = new ArrayList();
List<Node> closed = new ArrayList();
int goalX = 270;
int goalY = 270;
public PathWorld()
{
super(601, 401, 1);
placeSquares();
//pathFind(20, 30);
}
public void placeSquares()
{
for(int x = mult/2; x < 600; x = x + mult)
{
for(int y = mult/2; y < 400; y = y + mult)
{
addObject(new Square(), x, y);
}
}
}
public void pathFind(int startX, int startY)
{
Node start = new Node(new coord(startX, startY), null, 0);
open.add(start);
addAdj(start);
while(!atGoal())
{
Collections.sort(open);
Node closest = open.get(0);
for( Node node : closed )
{
int a = 1;
if(node == closest)
{
//if(a > open.size() - 1)
closest = open.get(a);
a = a + 1;
}
}
addAdj(closest);
}
}
public void addAdj(Node n)
{
closed.add(n);
open.remove(n);
int x = n.getX();
int y = n.getY();
int G = n.G;
Node nt = new Node(x - mult, y - mult, n, G + 28);
calcHeur(nt);
checkNode(n, nt);
nt = new Node(x - mult, y, n, G + 20);
calcHeur(nt);
checkNode(n, nt);
nt = new Node(x - mult, y + mult, n, G + 28);
calcHeur(nt);
checkNode(n, nt);
nt = new Node(x, y - mult, n, G + 20);
calcHeur(nt);
checkNode(n, nt);
nt = new Node(x, y + mult, n, G + 20);
calcHeur(nt);
checkNode(n, nt);
nt = new Node(x + mult, y - mult, n, G + 28);
calcHeur(nt);
checkNode(n, nt);
nt = new Node(x + mult, y, n, G + 20);
calcHeur(nt);
checkNode(n, nt);
nt = new Node(x + mult, y + mult, n, G + 28);
calcHeur(nt);
checkNode(n, nt);
}
public void calcHeur(Node n)
{
n.setH((int)Math.sqrt(Math.pow(Math.abs(n.getX() - goalX), 2) + Math.pow(Math.abs(n.getY() - goalY), 2)));
}
public void checkNode(Node parent, Node nt)
{
boolean available = true;
for( Node node : open )
{
if(node.getX() == nt.getX() && node.getY() == nt.getY())
{
available = false;
if(node.G > nt.G)
{
open.remove(node);
open.add(nt);
break;
}
}
}
if(available) {open.add(nt);}
}
public boolean atGoal()
{
for( Node node : open )
{
if(node.getX() == goalX && node.getY() == goalY)
{
return true;
}
}
for( Node node : closed )
{
if(node.getX() == goalX && node.getY() == goalY)
{
return true;
}
}
return false;
}
}public class Node implements Comparable
{
coord c;
Node parent;
int G;
int H;
int F;
public Node(coord c, Node p, int G)
{
this.c = c;
this.parent = p;
this.G = G;
}
public Node(int x, int y, Node p, int G)
{
this.c = new coord(x, y);
this.parent = p;
this.G = G;
}
public void setH(int H)
{
this.H = H;
F = H + G;
}
public void setG(int G)
{
this.G = G;
F = H + G;
}
public int getX() { return c.x; }
public int getY() { return c.y; }
public int compareTo(Object o)
{
Node n = (Node)o;
return this.F - n.F;
}
}
public class coord
{
int x;
int y;
public coord(int x, int y)
{
this.x = x;
this.y = y;
}
public boolean atLoc(int x, int y)
{
if(this.x == x && this.y == y)
{ return true; }
else
return false;
}
}