public static List <int> AStar <T>(ioGraph <T> _graph, int _start, int _end, Func <int, double> _fHeuristic)
{
//var startRecord = new NodeRecord(_start, -1, 0, _fHeuristic(_start));
var startRecord = new NodeRecord <T>(_start, null, 0, _fHeuristic(_start));
var openList = new Dictionary <int, NodeRecord <T> >();
openList.Add(_start, startRecord);
var closedList = new Dictionary <int, NodeRecord <T> >();
int curNode = -1;
while (openList.Count > 0)
{
//Find smallest element in open list (est total cost)
curNode = FindSmallest(openList);
//Exit if goal found
if (curNode == _end)
{
closedList.Add(curNode, openList[curNode]);
break;
}
var curEdges = _graph.GetConnectionsFast(curNode);
foreach (ioGraph <T> .ioGraphEdge edge in curEdges)
{
//Set end node and add cost
var endNode = edge.ToNode;
var endNodeCost = openList[curNode].CostSoFar + edge.Weight;
NodeRecord <T> endNodeRecord;
double endNodeHeuristic;
if (openList.ContainsKey(endNode))
{
if (openList[endNode].CostSoFar <= endNodeCost)
{
continue;
}
openList.Remove(endNode);
}
if (closedList.ContainsKey(endNode))
{
if (closedList[endNode].CostSoFar <= endNodeCost)
{
continue;
}
closedList.Remove(endNode);
}
endNodeRecord = new NodeRecord <T>();
endNodeRecord.FromNode = curNode;
endNodeHeuristic = _fHeuristic(endNode);
endNodeRecord.CostSoFar = endNodeCost;
endNodeRecord.Connection = edge;
endNodeRecord.EstTotalCost = endNodeCost + endNodeHeuristic;
openList.Add(endNode, endNodeRecord);
/*if(closedList.ContainsKey(endNode))
* {
*
* if (closedList[endNode].CostSoFar <= endNodeCost)
* continue;
*
* endNodeRecord = closedList[endNode];
* endNodeHeuristic = endNodeRecord.EstTotalCost - endNodeRecord.CostSoFar;
* closedList.Remove(endNode);
* }
* if(openList.ContainsKey(endNode))
* {
* if (openList[endNode].CostSoFar <= endNodeCost)
* continue;
*
* endNodeRecord = openList[endNode];
* endNodeHeuristic = endNodeRecord.EstTotalCost - openList[endNode].CostSoFar;
* openList.Remove(endNode);
*
* }
* else
* {
* endNodeRecord = new NodeRecord();
* endNodeRecord.FromNode = curNode;
* endNodeHeuristic = _fHeuristic(endNode);
* }
*
* endNodeRecord.CostSoFar = endNodeCost;
* endNodeRecord.Connection = edge;
* endNodeRecord.EstTotalCost = endNodeCost + endNodeHeuristic;
*
* if (!openList.ContainsKey(endNode)) openList.Add(endNode, endNodeRecord);*/
}
closedList.Add(curNode, openList[curNode]);
openList.Remove(curNode);
}
if (curNode != _end)
{
String dbgMsg = "End Node Null -- No path found?";
Msg.LogDebug(TAG_DEBUG, dbgMsg, MsgPriLvl.HIGH);
return(null);
}
else
{
var path = new List <int>();
while (curNode != _start)
{
path.Add(curNode);
Msg.LogDebug(TAG_DEBUG, "Added to path: " + _graph.GetNode(curNode), MsgPriLvl.LOW);
curNode = closedList[curNode].FromNode; //Seed 705 crashes here with invalid key
}
return(path);
}
}