private void SearchStart(Graph <T> graph, T src, T dst)
{
if (Callback == null)
{
throw new Exception("callback not defined");
}
_graph = graph;
_dst = dst;
// Clear out vars
_state = States.Searching;
_openList.Clear();
_closedList.Clear();
_costs.Clear();
// Add starting node to open list and add costs
_openList.Add(src);
_parentCost = new NodeCosts();
_parentCost.G = 0;
_parentCost.H = Callback.ApproximateDistance(src, dst);
_parentCost.F = _parentCost.H;
_costs[src] = _parentCost;
_focusedNode = src;
}
private void CalculateNeighborCosts(IEnumerable <T> neighbours)
{
foreach (var node in neighbours)
{
if (_closedList.Contains(node))
{
continue;
}
NodeCosts cost;
var g = _parentCost.G + Callback.ApproximateDistance(_focusedNode, node) + _graph.Costs[node];
if (_openList.Contains(node))
{
cost = _costs[node];
if (g < cost.G)
{
cost.G = g;
}
}
else
{
cost = new NodeCosts
{
G = g,
H = Callback.ApproximateDistance(node, _dst)
};
_costs[node] = cost;
_openList.Add(node);
}
cost.Parent = _focusedNode;
cost.F = cost.G + cost.H;
}
}