I am trying to add pathfinding into a game that I am creating and have tried to use the A* pathfinding algorithm. It kind of works as it does eventually find a route however it can take a very long time when nodes are far apart and when I look at it when it searches it looks like it goes over the same nodes over and over again and I'm not too sure why?
This was how I did it and I'm not sure if I messed up somewhere.
internal class PathFinding
{
const int NO_PARENT = -1; //Constant to show that starting node and end node have no parent nodes
private List<Object> _obstacles = new List<Object>();
private List<Node> _open = new List<Node>();
private List<Node> _closed = new List<Node>();
private struct Node
{
//Position
int x;
int y;
//Parent node position
int parentX;
int parentY;
int gCost; //Distance from node to starting node
int hCost; //Distance from node to target node
int fCost; //Which node should be explored next gCost + hCost
public Node(int nodeXPosition, int nodeYPosition, int parentXPosition, int parentYPosition)
{
x = nodeXPosition;
y = nodeYPosition;
parentX = parentXPosition;
parentY = parentYPosition;
gCost = 0;
hCost = 0;
fCost = 0;
}
public void CalculateCosts(Node startingNode, Node targetNode)
{
gCost = Math.Abs(x - startingNode.x) + Math.Abs(y - startingNode.y);
hCost = Math.Abs(x - targetNode.x) + Math.Abs (y - targetNode.y);
fCost = gCost + hCost;
}
public int GetFCost()
{
return fCost;
}
public int GetXPosition()
{
return x;
}
public int GetYPosition()
{
return y;
}
public Node[] ExploreNeighbours()
{
//0 = up
//1 = right
//2 = down
//3 = left
Node[] neighbours = new Node[4];
neighbours[0].x = x;
neighbours[0].y = y - 1;
neighbours[1].x = x + 1;
neighbours[1].y = y;
neighbours[2].x = x;
neighbours[2].y = y + 1;
neighbours[3].x = x - 1;
neighbours[3].y = y;
//Set parent node for explored nodes
for (int i = 0; i < neighbours.Count(); i++)
{
neighbours[i].parentX = x;
neighbours[i].parentY = y;
}
return neighbours;
}
}
public PathFinding(List<Object> obstacles)
{
_obstacles = obstacles;
}
public void FindRoute(int startPositionX, int startPositionY, int endPositionX, int endPositionY)
{
bool routeFound = false;
Node startingNode = new Node(startPositionX,startPositionY,NO_PARENT,NO_PARENT);
Node endNode = new Node(endPositionX,endPositionY,NO_PARENT,NO_PARENT);
Node[] exploredNodes;
startingNode.CalculateCosts(startingNode,endNode);
_open.Add(startingNode);
while(routeFound == false)
{
//Sort list by lowest F cost
_open = _open.OrderBy(x => x.GetFCost()).ToList();
//Explore the node in open list with lowest F cost (First in list due to sorting)
exploredNodes = _open[0].ExploreNeighbours();
//Check to see if explored nodes already exist as well as calculate their costs
//If explored node already then remove it from the open list.
//Once it has checked to makes sure that duplicates are gone add all explored nodes to the open list
for (int i = 0; i < exploredNodes.Length; i++)
{
exploredNodes[i].CalculateCosts(startingNode,endNode);
for (int j = 0; j < _open.Count; j++)
{
if (exploredNodes[i].GetXPosition() == _open[j].GetXPosition() && exploredNodes[i].GetYPosition() == _open[j].GetYPosition())
{
_open.RemoveAt(j);
}
}
}
_open.AddRange(exploredNodes);
_closed.Add(_open[0]);
_open.RemoveAt(0);
}
}
}
You are adding nodes to the closed set, but as far as I can see you are never checking it. You do seem to be checking the open set, but that is incorrect. The open set can contain the same node multiple times, with different costs. You can remove nodes from the open set, but then you need to make sure to keep the node with the lowest cost.
Your loop also looks wrong to me. The general algorithm should be:
There are also several other issues
HashSet<T> for the closed list and PriorityQueue<TElement,TPriority> for the open set. Both require your nodes to be equatable, so either implement Equals method, or create a IEqualityComparer<Node> to do the comparison.Overall I would suggest starting by simplifying the code as much as possible.