public PathRoute Diamater()
{
PathRoute diameterPath = null;
var nodes = this._nodes.Values;
foreach (var source in nodes)
{
foreach (var target in nodes)
{
if (source != target)
{
var pathRoute = Graph.ShortestPathHeuristic(source, target);
if (pathRoute.Status == EPathStatus.Found)
{
if (diameterPath == null || pathRoute.Distance > diameterPath.Distance)
{
diameterPath = pathRoute;
}
}
}
}
}
return(diameterPath);
}
public static PathRoute ShortestPathHeuristic(Node source, Node target, Func <Edge, double> edgeWeightCalculation, Func <Node, Node, double> distanceHeuristic)
{
// f(n) = g(n) + h(n)
if (source == null || target == null || edgeWeightCalculation == null || distanceHeuristic == null)
{
throw new ArgumentNullException("No parameter can be null");
}
else if (source == target || source.Id == target.Id)
{
throw new ArgumentException("Source and target must be different");
}
IList <Node> border = new List <Node>();
IDictionary <long, double> weightToNode = new Dictionary <long, double>();
IDictionary <long, double> totalCostForNode = new Dictionary <long, double>();
IDictionary <long, Edge> parents = new Dictionary <long, Edge>();
int quantityOfExpansions = 0;
IList <long> searched = new List <long>();
border.Add(source);
weightToNode.Add(source.Id, 0);
totalCostForNode.Add(source.Id, distanceHeuristic(source, target));
Node current = null;
while (border.Count != 0)
{
current = border[0];
border.RemoveAt(0);
quantityOfExpansions++;
if (current == target)
{
break;
}
var currentWeight = weightToNode[current.Id];
IList <Node> childrens = null;
childrens = current.NeighborsOut();
foreach (Node children in childrens)
{
var bestRouteToChildren = Node.ShortestPathBetweenNeihbors(current, children, edgeWeightCalculation);
if (bestRouteToChildren.Status == EPathStatus.NotFound)
{
throw new Exception("Expanded node lost reference to children");
}
Edge edge = bestRouteToChildren.Edges[0];
var weight = bestRouteToChildren.Distance;
double weightToChildren = currentWeight + weight;
double costToTarget = distanceHeuristic(children, target);
double costFunction = weightToChildren + costToTarget;
if (weightToNode.ContainsKey(children.Id))
{
if (weightToNode[children.Id] <= weightToChildren)
{
continue;
}
}
else
{
border.Remove(children);
}
weightToNode[children.Id] = weightToChildren;
totalCostForNode[children.Id] = costFunction;
parents[children.Id] = edge;
var index = border.FindIndex(node => totalCostForNode[node.Id] > costFunction);
border.Insert(index >= 0 ? index : border.Count, children);
}
}
if (current == target)
{
IList <Edge> edges = new List <Edge>();
EPathStatus status = EPathStatus.Found;
var edge = parents[target.Id];
edges.Add(edge);
Node parent = edge.Source;
while (parent != source)
{
edge = parents[parent.Id];
edges.Add(edge);
if (edge == null || edge.Source == null)
{
status = EPathStatus.FailOnRouteBuilding;
break;
}
parent = edge.Source;
}
var pathRoute = new PathRoute(status, source, target, edges.Reverse().ToArray(), weightToNode[target.Id]);
pathRoute.QuantityOfExpansions = quantityOfExpansions;
return(pathRoute);
}
return(new PathRoute(EPathStatus.NotFound, source, target));
}
