コード例 #1
0
ファイル: NBSQueue.cs プロジェクト: Treff/NBS
 public NBSQueue(double epsilon)
 {
     ForwardQueue  = new BDOpenClosed <TState>();
     BackwardQueue = new BDOpenClosed <TState>();
     _epsilon      = epsilon;
 }
コード例 #2
0
ファイル: NBS.cs プロジェクト: Treff/NBS
        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;
                }
            }
        }