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); }