# Latitude/Longitude graph optimised A* Algorithm

Can anyone point me in the direction of a C# or pseudocode implementation of A* optimised for relatively sparsely connected graphs (average < 3 edges per node)?

The implementation below that I have currently runs in 100-200ms for a graph of ~3500 nodes. I need this to be as low as absolutely possible. According to the Visual Studios CPU profiling, the bit taking the longest is searching for the lowest cost node.

Current Implementation

public List<NodeRow> BuildAStar(string start, string end)
{
ILookup<string, DijkstraNodeData> nodes = AllNodes.ToLookup(n => n.Key, n => new DijkstraNodeData(n.FirstOrDefault()));

if (start == end)
{
return new List<NodeRow>();
}

var startNode = nodes[start].FirstOrDefault();
var endNode = nodes[end].FirstOrDefault();
var endCorridor = FindNextCorridorNode(endNode.Node);
startNode.Closed = false;
startNode.Cost = 0;

while (!endNode.Closed)
{
// Find Lowest Cost.
float smallest = float.PositiveInfinity;
string smallestId = nodes.FirstOrDefault().Key;

foreach (var node in nodes)
{

if (!node.First().Closed)
{
if (node.First().Cost < smallest)
{
smallest = node.First().Cost;
smallestId = node.Key;
}

}
}
if(smallest == float.PositiveInfinity) { return new List<NodeRow>(); }
// Close this location.
nodes[smallestId].First().Closed = true;

// Calculate new costs.
var currentNode = nodes[smallestId].First();
foreach (var node in currentNode.Node.Edges)
{
float currentCost = currentNode.Cost + node.Weight;

DijkstraNodeData currentConnection;
if (node.NodeId1 != currentNode.Node.NodeId)
{
currentConnection = nodes[node.NodeId1].First();
}
else
{
currentConnection = nodes[node.NodeId2].First();
}

double heuristicCost;
if (currentConnection.Node.Type.ToLower() == "c")
{
heuristicCost = SharedFunctions.GetDistanceFromLatLonInMeters((double)currentConnection.Node.Latitude, (double)currentConnection.Node.Longitude, (double)endCorridor.Latitude, (double)endCorridor.Longitude);
}
else
{
heuristicCost = 10000;
}

if (currentCost < currentConnection.Cost + heuristicCost)
{
currentConnection.Cost = currentCost + (float)heuristicCost;
}
}
}

List<NodeRow> pathStartToEnd = new List<NodeRow>();

bool done = false;
string nextClosed = end;
startNode.InPath = true;
while (!done)
{
if (nextClosed.Length > 0)
{
var thisNode = nodes[nextClosed].First();
thisNode.InPath = true;
if (nextClosed == start) { done = true; }
}
}
pathStartToEnd.Reverse();
return pathStartToEnd;
}

private NodeRow FindNextCorridorNode(NodeRow node)
{
if (node.Type.ToLower() == "c")
{
return node;
}
foreach (NodeEdgeRow edge in node.Edges)
{
if (edge.Node2.Type.ToLower() == "c")
{
return edge.Node2;
}
}
foreach (NodeEdgeRow edge in node.Edges)
{
FindNextCorridorNode(edge.Node2);
}
return null;
}

public class DijkstraNodeData
{
public NodeRow Node;
public bool Closed;
public float Cost = float.PositiveInfinity;
public bool InPath = false;

public DijkstraNodeData(NodeRow node)
{
Node = node;
}
}


## Performance

I won't say much about the algorithm, but the profiler has pointed you in the right direction: what you need is a Priority Queue.

A Priority Queue will enable you to keep track of the 'open' nodes in such a way that you can determine the 'cheapest' in sub-linear time. Unfortunately, C#/.NET doesn't provide a priority Queue, so you'll have to find one. Blue Raja will probably show up at some point and point you at this offering, and you can see an outline of an A* implementation using it near the end of my answer to another A* themed question.

Get it working with this better data structure before worrying about anything else with regard to performance.

I'll add that your 'closed/!closed' system needs rethinking. You explicitly set the start node open, but none of the others. It's much easier just to keep track of explored nodes by putting them in a HashSet.

## BuildAStar(string, string)

This is a biiiig method. Consider breaking it down into smaller, more manageable, methods. I would try to pull the code which 'expands' the current node out, and I'd definitely move all the pathStartToEnd stuff into a different method. I'd also move the call to GetDistanceFromLatLonInMeters into its own method, since getting the cost between nodes is perfectly self-contained concept independent of using said cost for path-finding.

The return type could be more general: there is no reason to expose the fact you are using a List internally (e.g. you could just as well use a Stack to accumulate the path). Consider changing the return type to IEnumerable<NodeRow> or IReadOnlyList<NodeRow> or IList<NodeRow>. This allows you to change the internal implementation without having to adhere to an unnecessarily strict API, and you can more easily change from an abstract type to a concrete type in the future, than to change from a concrete type to a more abstract type.

More importantly, you return new List<NodeRow>() both when the route is zero-length and when there is no route. These results must absolutely be distinct. A quick-and-dirty solution would be to return null when there is no route, which will at least make unsuspecting code crash violently when it's assumptions are violated (which is good).

I really don't like your nodes variable. It's explicitly an ILookup, and you spend the rest of the method working around the fact that you can't be sure whether nodes exists, and assuming that none of them have the same name. Your code depends on there being only one. You can resolve much of this weirdness by switching to ToDictionary, which enforces the rule that there can only be one element to each key, throwing if a duplicate appears (which is good). You're also inconsistent between First() and FirstOrDefault(). Since you are never checking for nulls, these should all be First(), so that the program crashes as soon as possible. The other option is to actually check for the nulls, and throw a meaningful error. For example:

IDictionary<string, DijkstraNodeData> nodes = AllNodes.ToLookup(n => n.Key, n => new DijkstraNodeData(n.FirstOrDefault()));

// snip

DijkstraNodeData startNode;
if (!nodes.TryGetValue(start, out startNode))
{
throw new ArgumentException("No node exists with the start key", nameof(start));
}


Why do you record smallestId = node.Key; rather than currentNode = node? The only time you use smallestId is when you are looking up the node in the table, and that will just add overhead and makes the code more fragile.

Finally, where did the number 10000 come from? This is a magic number: such important numbers deserve good names. Naming constants makes the code clear and easier to maintain (e.g. if the 'constant' changes, you need only change it in one place, which means you can't miss any instances, or accidently change any other magic numbers which happened to be the same values but are logically distinct.

## FindNextCorridorNode(NodeRow)

The following loop returns nothing, so it's just doing work for the fun of it:

foreach (NodeEdgeRow edge in node.Edges)
{
FindNextCorridorNode(edge.Node2);
}


Not knowing the topology of your graph, it's not unthinkable that this could lead to an infinite loop.

What is "c"? I'm guessing it stands for 'corridor', but it's not clear. Give it a meaningful name, or change the type of NodeRow.Type to something with an IsCorridor() method. The ToLower calls do not inspire much confidence either.

The only code provided which uses this method assumes that it returns non-null. This means that when it does return null, it will wait quietly until the calling code falls over this null, and whoever has to debug it will wonder where this null came from. The return null should probably be a throw with a helpful error message.

## class DijkstraNodeData

I'd make Node readonly, since it makes no sense ever to change it.

I don't like that Link is initialised to "". This looks like a valid value (on account of not being null), which means that (if you did any null checks) it could slip through, and cause problems far away from the source.

Should Closed be initialised to true?

## Style

It's a non-issue for a consumer, but I'd personally appreciate some more consistency with empty lines.

This was a little surprising:

if(smallest == float.PositiveInfinity) { return new List<NodeRow>();


All on one line, no space after the if: rather out of place. And another one:

if (nextClosed == start) { done = true; }


I'd rewrite the while as:

while (nextClosed.Length > 0)
{
var thisNode = nodes[nextClosed].First();
thisNode.InPath = true;

if (nextClosed == start)
{
break;
}
}


Your names are all formatted nicely, though some could be better and more consistent. The two that bother me most are smallest and smallestId, which from the rest of the code feel like they should be smallestCost and smallest.

• Thanks for the feedback! I'll look at implementing the improvements you suggested. This is far from production code and is a quick test to compare performance against the current method of pathfinding.
– Jack
Sep 4, 2018 at 7:58
• Do you have an example of A* implementation using a priority queue like the FastPriorityQueue in the library you mentioned. I can't find any full examples through googling and have spent most of the day trying get it working with what examples I could find. Farthest I got was getting it to not crash but the route it finds goes through most of the nodes in the graph - which is clearly wrong.
– Jack
Sep 4, 2018 at 15:05
• @Jack there is some completely untested code I wrote in this other answer (untested; at a glance it looks like the enqueue/update are the wrong way round) based on SimplePriorityQueue. IIRC, FastPriorityQueue requires that your node types inherit a particular class, but otherwise I don't recall it being too different. Sep 4, 2018 at 16:17
• I tried building it off that example code, but it returns routes that are definitely not the shortest, they are 800-1000 nodes long when they should be more like 20-100. This is the code i have currently
– Jack
Sep 6, 2018 at 8:14
• @Jack this is really beyond StackOverflow even, and without knowing more about your data-structures and topology I can't do much better, but here is an example (based on your code) with a single passing test case. The big issues are that I have no idea what a corridor is, and your heuristic makes no sense if anything is not a corridor, and you are. Every line which has anything to do with a corridor should be suspect. Please ask any questions on gist, not here. Sep 6, 2018 at 10:24