public IList <TNode> ShortestPath(IDirectedGraph <TNode, TEdge> graph, TNode source, TNode goal)
{
if (source == null || goal == null)
{
return(null);
}
var visited = new HashSet <string>();
var queue = new KeyValueHeap <string, int>(_comparer);
var cameFrom = new Dictionary <string, string>();
var gScore = new Dictionary <string, int>();
var fScore = new Dictionary <string, int>();
queue.Insert(source.Id, 0);
gScore[source.Id] = 0;
fScore[source.Id] = gScore[source.Id] + Heuristic.Evaluate(source, goal);
while (!queue.Empty)
{
var current = graph[queue.RemoveRootKey()];
if (current.Id == goal.Id)
{
return(ReconstructPath(graph, cameFrom, current.Id));
}
visited.Add(current.Id);
foreach (var neighbor in graph.Outgoing(current).Where(neighbor => !visited.Contains(neighbor.Id)))
{
var score = gScore[current.Id] + graph[current, neighbor].Weight;
if (!queue.ContainsKey(neighbor.Id) || score < gScore[neighbor.Id])
{
cameFrom[neighbor.Id] = current.Id;
gScore[neighbor.Id] = score;
var tmpFScore = score + Heuristic.Evaluate(neighbor, goal);
if (queue.ContainsKey(neighbor.Id))
{
if (tmpFScore < fScore [neighbor.Id])
{
fScore[neighbor.Id] = tmpFScore;
queue.SetValue(neighbor.Id, tmpFScore);
}
}
else
{
fScore[neighbor.Id] = tmpFScore;
queue.Insert(neighbor.Id, fScore[neighbor.Id]);
}
}
}
}
return(null);
}
private void FactFlow(ILogicNode startNode)
{
if (startNode is ILiteralNode && _graph.OutDegree(startNode) == 1)
{
foreach (var node in _graph.Outgoing(startNode).Where(node => !node.Fact))
{
if (node is ILiteralNode)
{
var negNode = _graph.GetNode(Negate(node.Id));
if (negNode.Fact)
{
throw new Exception("KB is inconsistent, " + node.Id + " and " + negNode.Id + " are both facts!");
}
}
node.Fact = true;
FactFlow(node);
}
}
foreach (var node in _graph.Incoming(startNode).Where(node => !node.Fact))
{
if (node is IClauseNode)
{
if (_graph.Outgoing(node).All(dependency => dependency.Fact))
{
node.Fact = true;
FactFlow(node);
}
}
else if (node is ILiteralNode && (_graph.OutDegree(node) == 1 || node.Id.Contains("|")))
{
var negNode = _graph.GetNode(Negate(node.Id));
if (negNode.Fact)
{
throw new Exception("KB is inconsistent, " + node.Id + " and " + negNode.Id + " are both facts!");
}
node.Fact = true;
FactFlow(node);
}
}
}
