How can I make my A* algorithm faster?

Question

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



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

    SortedList<float, List<int>> openList = new SortedList<float, List<int>>();//lump nodes with same cost together, doesn't matter which one we grab
    List<int>                    searchedNodes = new List<int>(); // 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<int>() );
    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 node
        int source = openList[ openList.Keys[0] ][0];//first node of first list

        if (openList[ openList.Keys[0]].Count == 1)
            openList.Remove( openList.Keys[0] );
        else
            openList[ openList.Keys[0] ].RemoveAt(0);

        openListCount--;
        //Debug.Log( "source: " + source );

        // only allow the code to look at enabled nodes, ( unless it's the start 
        // node as I assume we'll want the agents to emerge from occupied tiles )
        if ( ND.Nodes[source].Enabled == true || source == start )
        {
            for ( int i = 0; i < ND.NodeAdjacencies[source].Count; i++ )
            {   
                //Debug.Log("adjacency count: " + nodeAdjacencies[source].Count);
                int target = ND.NodeAdjacencies[source][i];

                if ( !searchedNodes.Contains( target ) )
                {   
                    SPT.Add( ND.Edges[source][target].ToNode, ND.Edges[source][target] );

                    //does the key(cost) already exist?
                    float costToNode = ND.Edges[source][target].Weight + Vector3.Distance(ND.Nodes[target].Position, targetCoords);
                    if ( openList.ContainsKey(costToNode) )
                    {
                        openList[costToNode].Add(target);
                    }
                    else
                    {
                        openList.Add( costToNode, new List<int>() );
                        openList[costToNode].Add(target);
                    }
                    searchedNodes.Add( target );
                    openListCount++;

                    if ( target == goal ) 
                    { 
                        //calculate shortest path from the SPT
                        int counter = target;
                        while ( counter != start )
                        {
                            shortestPath.Add(SPT[counter]);
                            counter = SPT[counter].FromNode;
                        }
                        return true; 
                    }
                }
            }
        }
    }
    shortestPath = null;
    return false;
}

Answer 1

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<float, List<int>>. 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.