private void FindSmallestNodeData(
HashSet <TNode> keys,
Dictionary <TNode, DynamicGraphNodeData <TNode, TCost, TEdge> > lookUp,
out TNode smallestNode,
out DynamicGraphNodeData <TNode, TCost, TEdge> smallestEdge
)
{
Contract.Requires(keys != null);
Contract.Requires(keys.Count > 0);
Contract.Requires(lookUp != null);
Contract.Requires(lookUp.Count > 0);
Contract.Requires(lookUp.Count >= keys.Count);
Contract.Requires(Contract.ForAll(lookUp.Values, x => x != null));
Contract.Requires(Contract.ForAll(keys, key => key != null && lookUp.ContainsKey(key)));
Contract.Ensures(Contract.ValueAtReturn(out smallestNode) != null);
Contract.Ensures(Contract.ValueAtReturn(out smallestEdge) != null);
using (var enumerator = keys.GetEnumerator()) {
if (!enumerator.MoveNext())
{
throw new ArgumentException("keys is empty", "keys");
}
var currentNode = enumerator.Current;
//var smallest = new KeyValuePair<TNode, DynamicGraphNodeData<TNode, TCost, TEdge>>(currentNode, lookUp[currentNode]);
smallestNode = currentNode;
smallestEdge = lookUp[currentNode];
Contract.Assume(smallestEdge != null);
while (enumerator.MoveNext())
{
currentNode = enumerator.Current;
var currentData = lookUp[currentNode];
Contract.Assume(currentData != null);
if (CostComparer.Compare(smallestEdge.Cost, currentData.Cost) > 0)
{
smallestNode = currentNode;
smallestEdge = currentData;
}
}
}
if (null == smallestNode)
{
throw new ArgumentException("null keys are not allowed", "keys");
}
if (null == smallestEdge)
{
throw new ArgumentException("null edge contained in edge look-up", "lookUp");
}
}
/// <summary>
/// Finds a graph path from the <paramref name="start"/> node to the <paramref name="target"/> node.
/// </summary>
/// <param name="start">The node to begin the path search from.</param>
/// <param name="target">The target node of the search.</param>
/// <returns>The shortest path from the start node to the target node if one exists.</returns>
/// <exception cref="System.ArgumentException">Thrown if a node or edge encountered within the graph is <c>null</c>.</exception>
public ReadOnlyCollection <DynamicGraphNodeData <TNode, TCost, TEdge> > FindPath(TNode start, TNode target)
{
if (null == start)
{
throw new ArgumentNullException("start");
}
if (null == target)
{
throw new ArgumentNullException("target");
}
Contract.EndContractBlock();
// initialize the look-ups
var nodeDataCache = new Dictionary <TNode, DynamicGraphNodeData <TNode, TCost, TEdge> >(NodeComparer)
{
{ start, new DynamicGraphNodeData <TNode, TCost, TEdge>(start, default(TCost), default(TEdge)) }
};
var visitRequired = new HashSet <TNode>(NodeComparer)
{
start
}; // NOTE: in order for a node to be in this collection it must have a corresponding key in the pathData dictionary.
DynamicGraphNodeData <TNode, TCost, TEdge> nodeData;
TNode currentNode;
var bestCompleteCost = default(TCost);
var completeRouteFound = false;
// generate the dynamic path information and find the shortest path
while (visitRequired.Count != 0)
{
DynamicGraphNodeData <TNode, TCost, TEdge> currentNodeData;
Contract.Assume(nodeDataCache.Count >= visitRequired.Count);
Contract.Assume(Contract.ForAll(visitRequired, k => k != null && nodeDataCache.ContainsKey(k)));
FindSmallestNodeData(visitRequired, nodeDataCache, out currentNode, out currentNodeData);
visitRequired.Remove(currentNode);
// logic to see if we can short out of checking this node due to it being too long
if (completeRouteFound)
{
if (CostComparer.Compare(bestCompleteCost, currentNodeData.Cost) <= 0)
{
continue; // this path is larger than or equal to the best found complete path
}
if (NodeComparer.Equals(currentNode, target))
{
bestCompleteCost = currentNodeData.Cost;
}
}
else if (NodeComparer.Equals(currentNode, target))
{
bestCompleteCost = currentNodeData.Cost;
completeRouteFound = true;
}
foreach (var neighborInfo in GetNeighborInfo(currentNode, currentNodeData.Cost))
{
if (ReferenceEquals(null, neighborInfo) || ReferenceEquals(null, neighborInfo.Node))
{
continue;
}
if (nodeDataCache.TryGetValue(neighborInfo.Node, out nodeData))
{
Contract.Assume(nodeData != null);
if (CostComparer.Compare(neighborInfo.Cost, nodeData.Cost) < 0)
{
nodeData.Node = currentNode;
nodeData.Cost = neighborInfo.Cost;
nodeData.Edge = neighborInfo.Edge;
visitRequired.Add(neighborInfo.Node);
}
}
else
{
nodeDataCache.Add(
neighborInfo.Node,
new DynamicGraphNodeData <TNode, TCost, TEdge>(currentNode, neighborInfo.Cost, neighborInfo.Edge));
visitRequired.Add(neighborInfo.Node);
}
}
}
// build the final result path
if (nodeDataCache.TryGetValue(target, out nodeData))
{
Contract.Assume(nodeData != null);
var pathResult = new List <DynamicGraphNodeData <TNode, TCost, TEdge> > {
new DynamicGraphNodeData <TNode, TCost, TEdge>(target, nodeData.Cost, nodeData.Edge)
};
currentNode = nodeData.Node;
if (null == currentNode)
{
return(null);
}
while (nodeDataCache.TryGetValue(currentNode, out nodeData))
{
Contract.Assume(nodeData != null);
pathResult.Add(new DynamicGraphNodeData <TNode, TCost, TEdge>(currentNode, nodeData.Cost, nodeData.Edge));
if (Equals(currentNode, start))
{
break;
}
currentNode = nodeData.Node;
if (null == currentNode)
{
return(null);
}
}
pathResult.Reverse();
return(new ReadOnlyCollection <DynamicGraphNodeData <TNode, TCost, TEdge> >(pathResult));
}
return(null); // no path was found
}