Пример #1
0
 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);
 }
Пример #2
0
        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
        }
Пример #3
0
Файл: NBS.cs Проект: Treff/NBS
        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);
        }
Пример #4
0
        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);
                    }
                }
            }
        }
Пример #5
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;
                }
            }
        }