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