Im building a tile based game and im implementing my path finding algorithm. Currently, it can find a path from point A to point B with no problems. However, if there are obstacles between point A and B, the algorithm will often select the wrong tiles.
When computing a tile's F score, I calculate the cost by adding the number of squares weve moved away from the starting point + the manhattan distance to the destination square. This seems to work fine, but if I encounter a square later on that weve already added to the open list, it will often choose that one as the closest even though the number of squares weve moved away from that the starting point has changed since I originally computed that value.
For example:
S = Starting at 0,0
F = Finish at 3,3
X = Obstacle
E = Empty
S E E E E E
E E E E E E
E E E X X E
E E X F E E
E E E E E E
My algorithm will compute this path: (0,0), (1,0),(2,0),(3,0),(3,1),(2,1),(2,2),(1,1)
I think when we get to (2,1), my algorithm should be noticing that (1,1) has already been added to the open list. We then re evaluate (1,1)'s total cost to 10 (6 moves away from start, + 4 away from Finish (3,3).
Logically, the next step should be ...(2,2),(1,2),(1,3),(1,4)...
I feel like there is something fundamentally wrong with my algorithm. I just cant seem to figure out what it is. Ive been looking at this code for quite some time now. Any help would be appreciated.
Sorry for the absolute wall of code.
private List FindPath_(Node start, Vector2 end)
{
if (start.X == end.X && start.Y == end.Y)
{
List final = mClosed.Select(node => node.Position).ToList();
return final;
}
int xLeft = start.X - 1;
int xRight = start.X + 1;
int yUp = start.Y - 1;
int yDown = start.Y + 1;
TileNode left = GetNearestTile_( xLeft, start.Y);
TileNode right = GetNearestTile_( xRight, start.Y);
TileNode up = GetNearestTile_(start.X, yUp);
TileNode down = GetNearestTile_(start.X, yDown);
List adjacentNodes = new List();
Node lNode = null;
Node rNode = null;
Node uNode = null;
Node dNode = null;
if (up != null)
{
uNode = new Node(new Vector2(start.X, yUp), start.CostFromA + 1, Math.Abs((int)end.X - start.X) + Math.Abs((int)end.Y - (start.Y - 1)), start);
adjacentNodes.Add(uNode);
}
if (left != null)
{
lNode = new Node(new Vector2(xLeft, start.Y), start.CostFromA + 1, Math.Abs((int)end.X - (start.X - 1)) + Math.Abs((int)end.Y - start.Y), start);
adjacentNodes.Add(lNode);
}
if (down != null)
{
dNode = new Node(new Vector2(start.X, yDown), start.CostFromA + 1, Math.Abs((int)end.X - start.X) + Math.Abs((int)end.Y - (start.Y + 1)), start);
adjacentNodes.Add(dNode);
}
if (right != null)
{
rNode = new Node(new Vector2(xRight, start.Y), start.CostFromA + 1, Math.Abs((int)end.X - (start.X + 1)) + Math.Abs((int)end.Y - start.Y), start);
adjacentNodes.Add(rNode);
}
foreach (Node node in adjacentNodes)
{
if (ClosedListContainsTile_(node))
{
continue;
}
else if (!OpenListContainsTile_(node))
{
InsertNode(node);
}
else if (OpenListContainsTile_(node))
{
if (node.Parent != null && node.CostFromA + 1 < node.Parent.CostFromA)
{
node.Parent.CostFromA = node.CostFromA + 1;
Node parentNode = mOpen.FirstOrDefault(n => n.X == node.Parent.X && n.Y == node.Parent.Y);
int index = mOpen.IndexOf(parentNode);
mOpen.RemoveAt(index);
InsertNode(parentNode);
}
}
}
if (mOpen.Count == 0)
return new List();
int cost = int.MaxValue;
Node closest = mOpen[0];
mOpen.Remove(closest);
mClosed.Add(closest);
return FindPath_(closest, end);
}
private TileNode GetNearestTile_(int x, int y)
{
TileNode tile = null;
try
{
tile = TileMap.GetTileNode(x, y);
if (tile.Type != TileType.Empty)
return null;
}
catch
{
return null;
}
return tile;
}
private bool OpenListContainsTile_(Node node)
{
return mOpen.FirstOrDefault(t => t.X == node.X && t.Y == node.Y) != null;
}
private bool ClosedListContainsTile_(Node node)
{
return mClosed.FirstOrDefault(t => t.X == node.X && t.Y == node.Y) != null;
}
private void InsertNode(Node node)
{
int i = 0;
for (; i < mOpen.Count; i++)
{
Node n = mOpen.ElementAt(i);
if (node.Total <= n.Total)
{
break;
}
}
if (i < mOpen.Count)
{
mOpen.Insert(i, node);
}
else
{
mOpen.Add(node);
}
}
public class Node
{
private Vector2 mPosition;
private Node mParent;
public Node(Vector2 position, int cost, int estimated, Node parent)
{
mPosition = position;
CostFromA = cost;
EstimatedCostToB = estimated;
Total = CostFromA + EstimatedCostToB;
mParent = parent;
}
/// F Score
public int Total { get; set; }
/// G Score
public int CostFromA { get; set; }
/// H Score
public int EstimatedCostToB { get; set; }
public int X { get { return (int) mPosition.X; } }
public int Y { get { return (int) mPosition.Y; } }
public Vector2 Position
{
get { return mPosition; }
}
public Node Parent
{
get { return mParent; }
}
}
Answer
Here is my working AStar implementation built in Java on the LibGdx framework (it should not be too difficult to adapt to C#):
public class AStar {
/**
* Makes the end passable, if it wasn't already, and then finds the shortest path towards it, using the AStar algorithm.
* @param start The node to start the search from.
* @param end The node the search will end at.
* @param map The map of nodes.
* @return The path from the start node to the final node (not including the start node).
*/
public static Path findShortestPath (Node start, Node end, Array map) {
Path path = new Path();
Array open, closed;
open = new Array();
closed = new Array();
// initialize heuristics
for (Node n : map)
n.setHeuristic((int)(Math.abs(n.position.x - end.position.x) + Math.abs(n.position.y - end.position.y)));
open.add(start);
end.setImpassable(false);
while (open.size > 0) {
Node current = getNextNode(open);
open.removeValue(current, true);
closed.add(current);
for (Node neighbor : current.getNeighbors()) {
if (neighbor.isImpassable() || closed.contains(neighbor, true)) continue;
if (!open.contains(neighbor, true)) {
neighbor.setG(neighbor.getMoveCost() + current.g());
neighbor.setParent(current);
open.add(neighbor);
} else {
if (neighbor.g() > current.g() + neighbor.getMoveCost()) {
neighbor.setG(current.g() + neighbor.getMoveCost());
neighbor.setParent(current);
}
}
}
if(open.contains(end, true)) {
path.add(end.addParents(path));
path.getPath().removeIndex(0);
return path;
}
}
return null;
}
private static Node getNextNode (Array open) {
Node n = open.first();
for (Node n2 : open)
n = n.f() < n2.f() ? n : n2;
return n;
}
}
Path is just a container for an Array of type Node.
This is the Node class:
public class Node {
private Array neighbors;
public Vector2 position;
private int heuristic, moveCost, G;
private Node parent;
private boolean impassable;
public Node(int x, int y, int moveCost) {
neighbors = new Array();
position = new Vector2(x, y);
this.moveCost = moveCost;
G = moveCost;
impassable = false;
}
public void addNeighbor(Node n) {
neighbors.add(n);
}
public Array getNeighbors () {
return neighbors;
}
public int f() {
return heuristic + moveCost;
}
public int g() {
return G;
}
public void setG(int newG) {
G = newG;
}
/**
* Also resets the node for the next path search.
* @param newHeuristic the new heuristic value. To be set by {@link AStar}.
*/
public void setHeuristic(int newHeuristic) {
heuristic = newHeuristic;
G = moveCost;
parent = null;
}
public void setParent(Node n) {
parent = n;
}
public Node getParent() {
return parent;
}
public int getMoveCost () {
return moveCost;
}
public Node addParents(Path path) {
if(parent != null)
path.add(parent.addParents(path));
return this;
}
public boolean isImpassable () {
return moveCost <= 0 || impassable;
}
public void setImpassable (boolean b) {
impassable = b;
}
}
My game has different movement costs depending the tile, but if yours doesn't just make moveCost a constant (and alter it for diagonals, if you're using those).
No comments:
Post a Comment