public NBSQueue(double epsilon)
{
ForwardQueue = new BDOpenClosed <TState>();
BackwardQueue = new BDOpenClosed <TState>();
_epsilon = 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;
}
}
}