How can I make my A* algorithm faster?

This is a slightly specialised A* algorithm, basically it allows you to search to and from disabled nodes. It's for a game and we the entities spawn in houses and go into them. The rest of the search behaves normally, it doesn't traverse any disabled nodes in between the start and end points.

NavData is a class which has the collections of nodes and edges in as well as some other things listed below:

(also edges and nodes are simple. edges have two integers - connect from node and connected to node. They also have a value for their weight. Nodes have an int index, a vector3 position and an enabled bool)

```
private List nodes; //just a list of all nodes in the level, unsorted
private Dictionary> edges;//edges[fromNode][toNode]
private List> nodeAdjacencies; //maintains a list of each node's connected nodes which can be used to index into 'nodes'
private List simpleEdges; //this keeps all the edges in one big list for easy traversal

private bool aStarCore( int start, int goal, out List shortestPath, out Dictionary SPT )
{
SPT = new Dictionary(); //[toNode] = Edge
NavData ND = NavData.GetSingleton;
Vector3 targetCoords = ND.Nodes[goal].Position; // for calculating heuristic
shortestPath = new List();

SortedList> openList = new SortedList>();//lump nodes with same cost together, doesn't matter which one we grab
List searchedNodes = new List(); // could definitely make this more efficient (find a better collection for lookup)

// push the start node onto the open list
openList.Add( Vector3.Distance( ND.Nodes[start].Position, targetCoords ), new List() );
openList[ Vector3.Distance( ND.Nodes[start].Position, targetCoords ) ].Add( start );
int openListCount = 1;
searchedNodes.Add(start);

// while there are still nodes on the open list
while ( openListCount > 0 )
{
// look at the next lowest cost

There are a few things you could do here.

A common optimization is to break ties towards smaller h-values. See here. This will cause your pathfinder to try the most direct routes first.

You may also want to look into using a hierarchical search algorithm if you're running this many different times on the same map, with different start/end points.

Finally, a minor optimization you could do is use an actual PriorityQueue instead of a SortedList. If you're going to run this on a system with a crappy garbage collector (XBox 360), make sure to use one which doesn't require any extra allocations.

StackExchange Code Review Q#14753, answer score: 2