public bool TerminateOnG() { if (ForwardQueue.OpenReadySize() > 0 && BackwardQueue.OpenReadySize() > 0) { return(FPUtil.Equal(_lowerBound, ForwardQueue.PeekAt(StateLocation.OpenReady).G + BackwardQueue.PeekAt(StateLocation.OpenReady).G + _epsilon)); } return(false); }
public static bool CompareOpenWaiting(BDOpenClosedData <TState> i1, BDOpenClosedData <TState> i2) { double f1 = i1.G + i1.H; double f2 = i2.G + i2.H; if (FPUtil.Equal(f1, f2)) { return(!FPUtil.Greater(i1.G, i2.G)); } return(FPUtil.Greater(f1, f2)); // low f over high }
public bool ExpandAPair(List <TState> thePath) { bool result = _queue.GetNextPair(out int nForward, out int nBackward); // if failed, see if we have optimal path (but return) if (result == false) { if (_currentCost == double.MaxValue) { thePath.Clear(); return(true); } ExtractFromMiddle(thePath); return(true); } // if success, see if nodes are the same (return path) if (_queue.ForwardQueue.Lookup(nForward).Data.Equals(_queue.BackwardQueue.Lookup(nBackward).Data)) { if (_queue.TerminateOnG()) { Console.WriteLine("NBS: Lower Bound on C* from g+g (gsum)\n"); } ExtractFromMiddle(thePath); return(true); } if (!FPUtil.Less(_queue.GetLowerBound(), _currentCost)) { ExtractFromMiddle(thePath); return(true); } double lowerBound = _queue.GetLowerBound(); if (!_counts.ContainsKey(lowerBound)) { _counts[lowerBound] = 0; } _counts[lowerBound] += 2; Expand(nForward, _queue.ForwardQueue, _queue.BackwardQueue, _forwardHeuristic, _goal); Expand(nBackward, _queue.BackwardQueue, _queue.ForwardQueue, _backwardHeuristic, _start); return(false); }
public bool GetNextPair(out int nextForward, out int nextBackward) { // move items with f < lowerBound to ready nextForward = nextBackward = -1; while (ForwardQueue.OpenWaitingSize() != 0 && FPUtil.Less( ForwardQueue.PeekAt(StateLocation.OpenWaiting).G + ForwardQueue.PeekAt(StateLocation.OpenWaiting).H, _lowerBound)) { ForwardQueue.PutToReady(); } while (BackwardQueue.OpenWaitingSize() != 0 && FPUtil.Less( BackwardQueue.PeekAt(StateLocation.OpenWaiting).G + BackwardQueue.PeekAt(StateLocation.OpenWaiting).H, _lowerBound)) { BackwardQueue.PutToReady(); } while (true) { if (ForwardQueue.OpenSize() == 0) { return(false); } if (BackwardQueue.OpenSize() == 0) { return(false); } if ((ForwardQueue.OpenReadySize() != 0) && (BackwardQueue.OpenReadySize() != 0) && (!FPUtil.Greater( ForwardQueue.PeekAt(StateLocation.OpenReady).G + BackwardQueue.PeekAt(StateLocation.OpenReady).G + _epsilon, _lowerBound))) { nextForward = ForwardQueue.Peek(StateLocation.OpenReady); nextBackward = BackwardQueue.Peek(StateLocation.OpenReady); return(true); } bool changed = false; if (BackwardQueue.OpenWaitingSize() != 0) { BDOpenClosedData <TState> i4 = BackwardQueue.PeekAt(StateLocation.OpenWaiting); if (!FPUtil.Greater(i4.G + i4.H, _lowerBound)) { changed = true; BackwardQueue.PutToReady(); } } if (ForwardQueue.OpenWaitingSize() != 0) { BDOpenClosedData <TState> i3 = ForwardQueue.PeekAt(StateLocation.OpenWaiting); if (!FPUtil.Greater(i3.G + i3.H, _lowerBound)) { changed = true; ForwardQueue.PutToReady(); } } if (!changed) { _lowerBound = double.MaxValue; if (ForwardQueue.OpenWaitingSize() != 0) { BDOpenClosedData <TState> i5 = ForwardQueue.PeekAt(StateLocation.OpenWaiting); _lowerBound = Math.Min(_lowerBound, i5.G + i5.H); } if (BackwardQueue.OpenWaitingSize() != 0) { BDOpenClosedData <TState> i6 = BackwardQueue.PeekAt(StateLocation.OpenWaiting); _lowerBound = Math.Min(_lowerBound, i6.G + i6.H); } if ((ForwardQueue.OpenReadySize() != 0) && (BackwardQueue.OpenReadySize() != 0)) { _lowerBound = Math.Min(_lowerBound, ForwardQueue.PeekAt(StateLocation.OpenReady).G + BackwardQueue.PeekAt(StateLocation.OpenReady).G + _epsilon); } } } }
private void Expand(int nextID, BDOpenClosed <TState> current, BDOpenClosed <TState> opposite, HCost <TState> heuristic, TState target) { current.Close(); //this can happen when we expand a single node instead of a pair if (FPUtil.GreaterEq(current.Lookup(nextID).G + current.Lookup(nextID).H, _currentCost)) { return; } _nodesExpanded++; IEnumerable <TState> neighbors = _getSuccessors(current.Lookup(nextID).Data); foreach (TState succ in neighbors) { _nodesTouched++; StateLocation loc = current.Lookup(_getStateHash(succ), out int childID); // screening double edgeCost = _gCost(current.Lookup(nextID).Data, succ); if (FPUtil.GreaterEq(current.Lookup(nextID).G + edgeCost, _currentCost)) { continue; } switch (loc) { case StateLocation.Closed: // ignore break; case StateLocation.OpenReady: // update cost if needed case StateLocation.OpenWaiting: { if (FPUtil.Less(current.Lookup(nextID).G + edgeCost, current.Lookup(childID).G)) { double oldGCost = current.Lookup(childID).G; current.Lookup(childID).ParentID = nextID; current.Lookup(childID).G = current.Lookup(nextID).G + edgeCost; current.KeyChanged(childID); StateLocation loc2 = opposite.Lookup(_getStateHash(succ), out int reverseLoc); if (loc2 == StateLocation.OpenReady || loc2 == StateLocation.OpenWaiting) { if (FPUtil.Less(current.Lookup(nextID).G + edgeCost + opposite.Lookup(reverseLoc).G, _currentCost)) { _currentCost = current.Lookup(nextID).G + edgeCost + opposite.Lookup(reverseLoc).G; _middleNode = succ; } } else if (loc == StateLocation.Closed) { current.Remove(childID); } } } break; case StateLocation.Unseen: { StateLocation locReverse = opposite.Lookup(_getStateHash(succ), out int reverseLoc); if (locReverse != StateLocation.Closed) { double newNodeF = current.Lookup(nextID).G + edgeCost + heuristic(succ, target); if (FPUtil.Less(newNodeF, _currentCost)) { if (FPUtil.Less(newNodeF, _queue.GetLowerBound())) { current.AddOpenNode(succ, _getStateHash(succ), current.Lookup(nextID).G + edgeCost, heuristic(succ, target), nextID, StateLocation.OpenReady); } else { current.AddOpenNode(succ, _getStateHash(succ), current.Lookup(nextID).G + edgeCost, heuristic(succ, target), nextID, StateLocation.OpenWaiting); } if (locReverse == StateLocation.OpenReady || locReverse == StateLocation.OpenWaiting) { if (FPUtil.Less(current.Lookup(nextID).G + edgeCost + opposite.Lookup(reverseLoc).G, _currentCost)) { _currentCost = current.Lookup(nextID).G + edgeCost + opposite.Lookup(reverseLoc).G; _middleNode = succ; } } } } } break; } } }