public EncapsulatedSearchNode(
T position,
SearchNodeBase <T> parent,
Func <T, T, double> getScoreBetween,
Func <T, double> getHeuristicScore)
: base(position, parent,
parent == null ? 0.0 : getScoreBetween.Invoke(parent.Position, position))
{
_h = getHeuristicScore.Invoke(position);
}
protected SearchNodeBase(T position, SearchNodeBase <T> parent, double scoreFromParent)
{
Position = position;
Parent = parent;
G = scoreFromParent;
if (parent != null)
{
G += parent.G;
}
}
public int CompareTo(SearchNodeBase <T> other)
{
var ret = F.CompareTo(other.F);
if (ret != 0)
{
return(ret);
}
// wikipedia says that you should return itens in LIFO fashion for improved A* performance.
// we could increment some node creation seed, but after some thought,
// I think falling back to an inverse G comparision should be almost equivalent; favor the larger G
return(other.G.CompareTo(G));
}
/// <summary>
/// Returns the best path. It runs two searches on separate threads. One search starts from each end.
/// TPosition will be used as a dictionary key.
/// </summary>
public static List <TPosition> BidirectionalFindMinimalPath <TPosition>(TPosition startingPosition, TPosition endingPosition, Func <TPosition, IEnumerable <TPosition> > getNeighbors,
Func <TPosition, TPosition, double> getScoreBetween, Func <TPosition, bool, double> getHeuristicScore, out double distance, out bool success, CancellationToken token = new CancellationToken())
where TPosition : class // needed for thread-safe assignment
{
var dictionary = new ConcurrentDictionary <TPosition, EncapsulatedSearchNode <TPosition> >();
Dictionary <TPosition, RandomMeldablePriorityTree <EncapsulatedSearchNode <TPosition> > > lookup1 = null, lookup2 = null;
Func <Dictionary <TPosition, RandomMeldablePriorityTree <EncapsulatedSearchNode <TPosition> > >, EncapsulatedSearchNode <TPosition>, bool> thread1Done =
(lookup, best) =>
{
if (!dictionary.TryAdd(best.Position, best) || token.IsCancellationRequested)
{
lookup1 = lookup;
return(true);
}
return(false);
};
Func <Dictionary <TPosition, RandomMeldablePriorityTree <EncapsulatedSearchNode <TPosition> > >, EncapsulatedSearchNode <TPosition>, bool> thread2Done =
(lookup, best) =>
{
if (!dictionary.TryAdd(best.Position, best) || token.IsCancellationRequested)
{
lookup2 = lookup;
return(true);
}
return(false);
};
var startingNode = new EncapsulatedSearchNode <TPosition>(startingPosition, null, (p1, p2) => 0.0, p => getHeuristicScore.Invoke(p, false));
var endingNode = new EncapsulatedSearchNode <TPosition>(endingPosition, null, (p1, p2) => 0.0, p => getHeuristicScore.Invoke(p, true));
var task1 = Task.Run(() => FindMinimalPath(startingNode, thread1Done, node =>
getNeighbors.Invoke(node.Position).Select(neighbor => new EncapsulatedSearchNode <TPosition>(neighbor, node, getScoreBetween, p => getHeuristicScore.Invoke(p, false)))));
var task2 = Task.Run(() => FindMinimalPath(endingNode, thread2Done, node =>
getNeighbors.Invoke(node.Position).Select(neighbor => new EncapsulatedSearchNode <TPosition>(neighbor, node, getScoreBetween, p => getHeuristicScore.Invoke(p, true)))));
var path1 = task1.Result;
var path2 = task2.Result;
var ret = new List <TPosition>();
if (lookup1 == null || lookup2 == null)
{
distance = double.PositiveInfinity;
success = false;
return(ret);
}
LastExpansionCount = lookup1.Count + lookup2.Count;
var overlap = lookup1.Keys.Intersect(lookup2.Keys);
var kvps = overlap.ToDictionary(o => o, o => GetG(lookup1, dictionary, o) + GetG(lookup2, dictionary, o));
var first = kvps.OrderBy(kvp => kvp.Value).Select(kvp => kvp.Key).FirstOrDefault();
if (first == null)
{
distance = double.PositiveInfinity;
success = false;
return(ret);
}
SearchNodeBase <TPosition> node1 = GetNode(lookup1, dictionary, first);
SearchNodeBase <TPosition> node2 = GetNode(lookup2, dictionary, first);
distance = node1.G + node2.G;
while (node1 != null)
{
ret.Add(node1.Position);
node1 = node1.Parent;
}
ret.Reverse();
ret.RemoveAt(ret.Count - 1); // don't double-add the overlap
while (node2 != null)
{
ret.Add(node2.Position);
node2 = node2.Parent;
}
var comparer = EqualityComparer <TPosition> .Default;
success = comparer.Equals(ret.First(), startingPosition) && comparer.Equals(ret.Last(), endingPosition);
return(ret);
}
