# 2D A* Path-Finder

I have re-coded my entire path-finding class, it used to follow Dijkstra's Algorithm, while also being super inefficient, using Lists and a lot of other bad things that I had/have no idea about.

However, I've spent the last 2 days re-writing the entire thing, this time following A*, finally delving deeper into the subject, understand heuristics and more etc..

I came up with this (I won't port the entire class here since I think it maybe not necessary):

public virtual Path Find(Point source, Point goal, Func<Point, Point, float> heuristic, bool cutCorners = true, bool memorizePath = false)
{
if (CutCorners != cutCorners) { CutCorners = cutCorners; if (precalculatedNeighbours) PrecalculateNeighbours(); }
if (Heuristic != heuristic) Heuristic = heuristic;
if ((source == goal) || !InBounds(source) || !InBounds(goal)) return null;
else
{
if (memorizePath && memoryDictionary.ContainsKey(source) && memoryDictionary[source].ContainsKey(goal)) return memoryDictionary[source][goal].Clone();
var open = new HashSet<Point>(); var closed = new HashSet<Point>();
while (!open.Contains(goal))
{
Point currentPoint = open.LowestFScore(source, Nodes);
Node currentNode = Nodes[currentPoint.X, currentPoint.Y];
open.Remove(currentPoint);
if (closed.Contains(currentPoint)) continue;
List<Point> neighbours = Neighbours(currentPoint);
foreach (Point neighbourPoint in neighbours)
{
if (closed.Contains(neighbourPoint)) continue;
Node neighbourNode = Nodes[neighbourPoint.X, neighbourPoint.Y];
float gScore = (currentNode.GScore + Node.CostBetween(currentPoint, neighbourPoint, neighbourNode.Cost));
else if (gScore >= neighbourNode.GScore) continue;
neighbourNode.Parent = currentPoint;
neighbourNode.GScore = gScore;
neighbourNode.FScore = (gScore + heuristic(neighbourPoint, goal));
}
if (open.Count == 0) return null;
}
return ConstructPath(source, goal, memorizePath);
}
}

public class Node
{
internal Point Parent;
internal float GScore, FScore;
internal List<Point> Neighbours;

internal bool Walkable;
internal float Cost;

public Node(bool walkable = true, float cost = 1) { Walkable = walkable; Cost = cost; }

internal static float CostBetween(Point point, Point other, float cost) { if ((point.X == other.X) || (point.Y == other.Y)) return cost; else return (cost + 1); }
}

public virtual List<Point> Neighbours(Point point, bool overwrite = false)
{
if ((Nodes[point.X, point.Y].Neighbours != null) && !overwrite) return Nodes[point.X, point.Y].Neighbours;
if (!CutCorners)
{
if (((point.Y - 1) >= 0) && Nodes[point.X, (point.Y - 1)].Walkable) adjacentPoints.Add(new Point(point.X, (point.Y - 1)));
if (((point.Y + 1) < Height) && Nodes[point.X, (point.Y + 1)].Walkable) adjacentPoints.Add(new Point(point.X, (point.Y + 1)));
if (((point.X - 1) >= 0) && Nodes[(point.X - 1), point.Y].Walkable) adjacentPoints.Add(new Point((point.X - 1), point.Y));
if (((point.X + 1) < Width) && Nodes[(point.X + 1), point.Y].Walkable) adjacentPoints.Add(new Point((point.X + 1), point.Y));
}
else
{
if (((point.Y - 1) >= 0) && Nodes[point.X, (point.Y - 1)].Walkable)
{
if (((point.X - 1) >= 0) && Nodes[(point.X - 1), (point.Y - 1)].Walkable &&
Nodes[(point.X - 1), point.Y].Walkable && Nodes[point.X, (point.Y - 1)].Walkable)
if (((point.X + 1) < Width) && Nodes[(point.X + 1), (point.Y - 1)].Walkable &&
Nodes[point.X, (point.Y - 1)].Walkable && Nodes[(point.X + 1), point.Y].Walkable)
}
if (((point.Y + 1) < Height) && Nodes[point.X, (point.Y + 1)].Walkable)
{
if (((point.X - 1) >= 0) && Nodes[(point.X - 1), (point.Y + 1)].Walkable &&
Nodes[(point.X - 1), point.Y].Walkable && Nodes[point.X, (point.Y + 1)].Walkable)
if (((point.X + 1) < Width) && Nodes[(point.X + 1), (point.Y + 1)].Walkable &&
Nodes[point.X, (point.Y + 1)].Walkable && Nodes[(point.X + 1), point.Y].Walkable)
}
if (((point.X - 1) >= 0) && Nodes[(point.X - 1), point.Y].Walkable) adjacentPoints.Add(new Point((point.X - 1), point.Y));
if (((point.X + 1) < Width) && Nodes[(point.X + 1), point.Y].Walkable) adjacentPoints.Add(new Point((point.X + 1), point.Y));
}
}

public virtual Path ConstructPath(Point source, Point goal, bool memorizePath)
{
Path path = new Path();
Node currentNode = Nodes[goal.X, goal.Y];
while (!path.Contains(source)) { path.Add(currentNode.Parent); currentNode = Nodes[currentNode.Parent.X, currentNode.Parent.Y]; }
path.Reverse();
path.RemoveAt(0);
if (memorizePath)
{
if (memoryDictionary.ContainsKey(source)) { if (!memoryDictionary[source].ContainsKey(goal)) memoryDictionary[source].Add(goal, path.Clone()); }
Path reversedPath = path.Clone();
reversedPath.Reverse();
reversedPath.RemoveAt(0);
if (memoryDictionary.ContainsKey(goal)) { if (!memoryDictionary[goal].ContainsKey(source)) memoryDictionary[goal].Add(source, reversedPath); }
}
return path;
}

public class Path : List<Point>
{
public Path() : base() { }
public Path(int capacity) : base(capacity) { }

public Path Clone() { Path clone = new Path(Count); clone.AddRange(this); return clone; }
}

public static class Heuristics
{
public static float Manhattan(Point source, Point goal) { return ((Math.Abs(source.X - goal.X) + Math.Abs(source.Y - goal.Y))); }
public static float Euclidean(Point source, Point goal) { return (float)(Math.Sqrt(Math.Pow((source.X - goal.X), 2) + Math.Pow((source.Y - goal.Y), 2))); }
public static float EuclideanNoSQR(Point source, Point goal) { return (float)((Math.Pow((source.X - goal.X), 2) + Math.Pow((source.Y - goal.Y), 2))); }
public static float MaxDXDY(Point source, Point goal) { return ((Math.Max(Math.Abs(source.X - goal.X), Math.Abs(source.Y - goal.Y)))); }
public static float Diagonal(Point source, Point goal)
{
Point dxy = new Point(Math.Abs(goal.X - source.X), Math.Abs(goal.Y - source.Y));
int Orthogonal = Math.Abs(dxy.X - dxy.Y);
int Diagonal = Math.Abs(((dxy.X + dxy.Y) - Orthogonal) / 2);
return (Diagonal + Orthogonal + dxy.X + dxy.Y);
}
}


It's more than 100 times the speed of my old path-finder right now (my old took an average of 14,000 ticks, this being around 400 for pretty much the same paths), how? I don't know, I guess my old one was just that bad.

However, while I'm really happy with my new path-finder, and still obsessed with improving my engine/old code, I'd like to ask whether anyone can take a look at that, and let me know of any improvements they could think of, specifically I've been researching a lot and heard about using things like a "Priority Queue" for the open list, opposed to using this:

open.LowestFCost(source, Nodes);


using my code:

internal static Point LowestFScore(this HashSet<Point> points, Point source, Pathfinder.Node[,] nodes)
{
Point chosenNode = source;
float lowestFScore = float.MaxValue;
foreach (Point point in points)
{
Pathfinder.Node node = nodes[point.X, point.Y];
if (node.FScore < lowestFScore) { lowestFScore = node.FScore; chosenNode = point; }
}
return chosenNode;
}


My comments are mostly stylistic, since the code seems pretty optimal.

You have an unusual approach to newlines, especially with if statements. Personally, I find this pretty unreadable

if (CutCorners != cutCorners) { CutCorners = cutCorners; if (precalculatedNeighbours) PrecalculateNeighbours(); }


I would at least expand it to

if (CutCorners != cutCorners) {
CutCorners = cutCorners;
if (precalculatedNeighbours)
PrecalculateNeighbours();
}


Similarly with your if (pred) continue and if (pred) return patterns, I would want those split onto two lines.

Your Node class has its members with a low visibility, which is good, but I'm not sure the class itself has to be public. It at least warrants a closer look.

Consider replacing

if (Heuristic != heuristic) Heuristic = heuristic;


with

Heuristic = heuristic;

• You're right, I just do that to keep my code super short and readable. – Dean Reynolds Feb 3 '16 at 1:44

A* isn't done until you select the goal.

    while (true)
{
Point currentPoint = open.LowestFScore(source, Nodes);
if(currentPoint == goal)
return ConstructPath(source, goal, memorizePath);
//...


Consider if you have an tile with 2 possible paths to the goal one direct one which takes 100* stepCost or another way which has more steps each with just 1* stepCost. Your code will select the inefficient path every time.

When using a queue you will need a way to find the node and tell the queue to update its internal structure.

However if you accept the memory cost then you can simply add the new neighbour with better FScore. Then when the one with the worse FScore comes up you will already have pushed it into the closed set and the if (closed.Contains(currentPoint)) continue; will catch that.

• I'm sorry, but I re-read this about 5 times, and still misunderstand all of it, from what I can gather.. The whole path-finder I have coded works 100%, I posted here in-case anyone has any ideas for improvement. – Dean Reynolds Dec 6 '15 at 14:55