I've successfully implemented A* pathfinding in C# but it is very slow, and I don't understand why. I even tried not sorting the openNodes list but it's still the same.
The map is 80x80, and there are 10-11 nodes.
I took the pseudocode from here Wikipedia
And this is my implementation:
public static List<PGNode> Pathfind(PGMap mMap, PGNode mStart, PGNode mEnd)
{
mMap.ClearNodes();
mMap.GetTile(mStart.X, mStart.Y).Value = 0;
mMap.GetTile(mEnd.X, mEnd.Y).Value = 0;
List<PGNode> openNodes = new List<PGNode>();
List<PGNode> closedNodes = new List<PGNode>();
List<PGNode> solutionNodes = new List<PGNode>();
mStart.G = 0;
mStart.H = GetManhattanHeuristic(mStart, mEnd);
solutionNodes.Add(mStart);
solutionNodes.Add(mEnd);
openNodes.Add(mStart); // 1) Add the starting
square (or node) to the open list.
while (openNodes.Count > 0) // 2) Repeat the following:
{
openNodes.Sort((p1, p2) => p1.F.CompareTo(p2.F));
PGNode current = openNodes[0]; // a) We refer to this as the current
square.)
if (current == mEnd)
{
while (current != null)
{
solutionNodes.Add(current);
current = current.Parent;
}
return solutionNodes;
}
openNodes.Remove(current);
closedNodes.Add(current); // b) Switch it to the closed list.
List<PGNode> neighborNodes = current.GetNeighborNodes();
double cost = 0;
bool isCostBetter = false;
for (int i = 0; i < neighborNodes.Count; i++)
{
PGNode neighbor = neighborNodes[i];
cost = current.G + 10;
isCostBetter = false;
if (neighbor.Passable == false || closedNodes.Contains(neighbor))
continue; // If it is not walkable or if it is on the closed list, ignore it.
if (openNodes.Contains(neighbor) == false)
{
openNodes.Add(neighbor); // If it isn’t on the open list, add it to the open list.
isCostBetter = true;
}
else if (cost < neighbor.G)
{
isCostBetter = true;
}
if (isCostBetter)
{
neighbor.Parent = current; // Make the current
square the parent of this
square.
neighbor.G = cost;
neighbor.H = GetManhattanHeuristic(current, neighbor);
}
}
}
return null;
}
Here's the heuristic I'm using:
private static double GetManhattanHeuristic(PGNode mStart, PGNode mEnd)
{
return Math.Abs(mStart.X - mEnd.X) + Math.Abs(mStart.Y - mEnd.Y);
}
What am I doing wrong? It's an entire day I keep looking at the same code.
