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
return aStarCore( start, goal, out shortestPath, ref SPT );
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;
}
