Пример #1
0
        private static void AStar(ref ExtendedDictionary <DS, Metric> G,
                                  ExtendedDictionary <DS, Metric> H, DS Sstart, DS Sgoal,
                                  ExtendedDictionary <DS, DS> tree, ref PriorityQueue <Metric, DS> OPEN,
                                  ref HashSet <DS> CLOSED, int lookahead, Operator <SS, DS>[] Ops, SS ss,
                                  Func <DS, DS, Metric> h0, ref uint Expansions, StateVisualizer <SS, DS> sv)
        {
            G = new ExtendedDictionary <DS, Metric>(k => Metric.Infinity);
            G.Set(Sstart, Metric.Zero);
            OPEN   = new PriorityQueue <Metric, DS>( );
            CLOSED = new HashSet <DS>( );
            OPEN.Enqueue(H.Lookup(Sstart), Sstart);

            int expansions = 0;

            while (!OPEN.IsEmpty( ) && G.Lookup(Sgoal) > OPEN.TopKey( ) &&
                   expansions < lookahead)
            {
#if OpenGl
                if (sv != null)
                {
                    HashSet <DS> hs = new HashSet <DS>( );
                    OPEN.AddToHashSet(hs);
                    sv.VisualizeAlg(ss, ss.InitialDynamicState, new OpenGLStateVisualizer.OpenGlVisulizationData( )
                    {
                        List      = hs as HashSet <GenericGridWorldDynamicState>,
                        HAdjusted = H,
                    });
                }
#endif
                expansions++;
                DS s = OPEN.Dequeue( );
                CLOSED.Add(s);
                Expansions++;
                foreach (DS sp in s.Expand(Ops, ss, false))
                {
                    Metric cost = G.Lookup(s) + c(s, sp, h0);
                    if (G.Lookup(sp) > cost)
                    {
                        G.Set(sp, cost);
                        tree.Set(sp, s);
                        if (OPEN.Contains(sp))
                        {
                            OPEN.Remove(sp);
                        }
                        OPEN.Enqueue(G.Lookup(sp) + H.Lookup(sp), sp);
                    }
                }
            }
        }
Пример #2
0
        public static Path <SS, DS> AStarLookAheadNotSliced(DS Start, SS ss,
                                                            int ComputationLimit, StateVisualizer <SS, DS> sv,
                                                            ref uint Expansions, ref uint Generations, bool Static,
                                                            Operator <SS, DS>[] Actions, Dictionary <string, Metric> HAdjusted,
                                                            Heuristic <SS, DS> Heuristic, Goal <SS, DS> Goal, Metric Weight, bool RTAStar)
        {
            if (ComputationLimit <= 0)
            {
                throw new ArgumentException("Computation Limit Invalid");
            }

            DS Cur = null;

            Start.ResetPath( );
            int IterationExpansions             = 0;
            PriorityQueue <Metric, DS> OpenList = new PriorityQueue <Metric, DS>( );
            HashSet <DS> ClosedList             = new HashSet <DS>( );

            OpenList.Enqueue(h(Start, HAdjusted, Heuristic, Goal, Actions, ss), Start);
            while (true)
            {
                if (OpenList.IsEmpty( ))
                {
                    Cur = null;
                    break;
                }
#if OpenGl
                if (sv != null)
                {
                    sv.VisualizeAlg(ss, ss.InitialDynamicState, new OpenGLStateVisualizer.OpenGlVisulizationData( )
                    {
                        OL        = OpenList as PriorityQueue <Metric, GenericGridWorldDynamicState>,
                        HAdjusted = HAdjusted,
                    });
                }
#endif
                Cur = OpenList.Dequeue( );
                ClosedList.Add(Cur);
                if (Goal.IsSatisfiedBy(ss, Cur))
                {
                    break;
                }
                else
                {
                    if (++IterationExpansions > ComputationLimit)
                    {
                        break;
                    }
                    Expansions++;
                    foreach (DS e in Cur.Expand(Actions, ss, Static))
                    {
                        if (!ClosedList.Contains(e) && !OpenList.Contains(e))
                        {
                            OpenList.Enqueue(Weight * h(e, HAdjusted, Heuristic, Goal, Actions, ss)
                                             + gComputer.G(ss, e, Goal), e);
                        }
                        Generations++;
                    }
                }
            }
            Metric            Best       = null;
            Operator <SS, DS> BestOp     = null;
            Metric            SecondBest = null;
            if (Cur != null)
            {
                Best = Weight * h(Cur, HAdjusted, Heuristic, Goal, Actions, ss)
                       + gComputer.G(ss, Cur, Goal);
                BestOp = Cur.Path( ).Actions.First.Value;

                if (!OpenList.IsEmpty( ))
                {
                    Cur        = OpenList.Dequeue( );
                    SecondBest = Weight * h(Cur, HAdjusted, Heuristic, Goal, Actions, ss)
                                 + gComputer.G(ss, Cur, Goal);
                }
            }

            if (RTAStar && SecondBest != null)
            {
                if (h(Start, HAdjusted, Heuristic, Goal, Actions, ss) < SecondBest)
                {
                    h(Start, SecondBest, HAdjusted);
                }
            }
            else if ((RTAStar && SecondBest == null) || Best == null)
            {
                h(Start, new Metric(double.PositiveInfinity), HAdjusted);
            }
            else if (Best != null)
            {
                if (h(Start, HAdjusted, Heuristic, Goal, Actions, ss) < Best)
                {
                    h(Start, Best, HAdjusted);
                }
            }
            if (BestOp == null)
            {
                return(null);
            }
            else
            {
                return(Operator <SS, DS> .MakePath(BestOp));
            }
        }
Пример #3
0
        public static Path <SS, DS> BreadthFirstLookAhead(DS Start, SS ss,
                                                          int ComputationLimit, StateVisualizer <SS, DS> sv,
                                                          ref uint Expansions, ref uint Generations, bool Static,
                                                          Operator <SS, DS>[] Actions, Dictionary <string, Metric> HAdjusted,
                                                          Heuristic <SS, DS> Heuristic, Goal <SS, DS> Goal, Metric Weight, bool RTAStar, Random Gen)
        {
            Start.ResetPath( );

            if (ComputationLimit <= 0)
            {
                throw new ArgumentException("Computation Limit Invalid");
            }
            DS[] Neighbors = Start.Expand(Actions, ss, Static).ToArray( );
            Expansions++;
            Metric            Best       = null;
            Operator <SS, DS> BestOp     = null;
            Metric            SecondBest = null;

            foreach (DS Neighbor in Neighbors)
            {
                Metric            GNeigh  = Neighbor.Path( ).Cost;
                Operator <SS, DS> OpNeigh = Neighbor.Path( ).Actions.First.Value;
                Metric            HNeigh  = null;
                if (HAdjusted == null || !HAdjusted.TryGetValue(Neighbor.ToString( ), out HNeigh))
                {
                    Neighbor.ResetPath( );
                    Metric       Alpha      = new Metric(double.PositiveInfinity);
                    Stack <DS>   ToVisit    = new Stack <DS>( );
                    Stack <int>  Depths     = new Stack <int>( );
                    HashSet <DS> ClosedList = new HashSet <DS>( );
                    ToVisit.Push(Neighbor);
                    Depths.Push(0);
                    ClosedList.Add(Start);
                    while (ToVisit.Count > 0)
                    {
#if OpenGl
                        if (sv != null)
                        {
                            sv.VisualizeAlg(ss, ss.InitialDynamicState, new OpenGLStateVisualizer.OpenGlVisulizationData( )
                            {
                                List      = new HashSet <DS>(ToVisit) as HashSet <GenericGridWorldDynamicState>,
                                HAdjusted = HAdjusted,
                            });
                        }
#endif
                        DS  Top   = ToVisit.Pop( );
                        int Depth = Depths.Pop( );
                        ClosedList.Add(Top);
                        Metric TVal = gComputer.G(ss, Top, Goal) + Weight *
                                      h(Top, HAdjusted, Heuristic, Goal, Actions, ss);
                        if (TVal >= Alpha)
                        {
                            continue;
                        }
                        if (HNeigh == null || HNeigh > TVal)
                        {
                            HNeigh = TVal;
                        }
                        if (ComputationLimit == Depth)
                        {
                            Alpha = Metric.Min(Alpha, TVal);
                        }
                        else
                        {
                            Expansions++;
                            foreach (DS n in Top.Expand(Actions, ss, Static))
                            {
                                Generations++;
                                if (!ClosedList.Contains(n) && !ToVisit.Contains(n))
                                {
                                    ToVisit.Push(n);
                                    Depths.Push(Depth + 1);
                                }
                            }
                        }
                    }
                }
                if (HNeigh != null)
                {
                    Metric FNeigh = GNeigh + HNeigh;
                    if (Best == null || Best > FNeigh)
                    {
                        BestOp = OpNeigh;
                        Best   = FNeigh;
                    }
                    else if (SecondBest == null || SecondBest > FNeigh)
                    {
                        SecondBest = FNeigh;
                    }
                }
            }
            if (RTAStar && SecondBest != null)
            {
                if (h(Start, HAdjusted, Heuristic, Goal, Actions, ss) < SecondBest)
                {
                    h(Start, SecondBest, HAdjusted);
                }
            }
            else if ((RTAStar && SecondBest == null) || Best == null)
            {
                h(Start, new Metric(double.PositiveInfinity), HAdjusted);
            }
            else if (Best != null)
            {
                if (h(Start, HAdjusted, Heuristic, Goal, Actions, ss) < Best)
                {
                    h(Start, Best, HAdjusted);
                }
            }
            if (BestOp == null)
            {
                return(null);
            }
            else
            {
                return(Operator <SS, DS> .MakePath(BestOp));
            }
        }
Пример #4
0
        public static Path <SS, DS> AStarLookAhead(DS Start, SS ss,
                                                   int ComputationLimit, StateVisualizer <SS, DS> sv,
                                                   ref uint Expansions, ref uint Generations, bool Static,
                                                   Operator <SS, DS>[] Actions, Dictionary <string, Metric> HAdjusted,
                                                   Heuristic <SS, DS> Heuristic, Goal <SS, DS> Goal, Metric Weight, bool RTAStar)
        {
            if (ComputationLimit <= 0)
            {
                throw new ArgumentException("Computation Limit Invalid");
            }

            DS[] Neighbors           = Start.Expand(Actions, ss, Static).ToArray( );
            int  IterationExpansions = 1;

            Expansions++;

            Dictionary <DS, Metric>             GNeighs = new Dictionary <DS, Metric>( );
            Dictionary <DS, Metric>             HNeighs = new Dictionary <DS, Metric>( );
            Dictionary <DS, Operator <SS, DS> > OpNeighs
                = new Dictionary <DS, Operator <SS, DS> >( );

            LinkedList <DS> LookAheadNeighbors = new LinkedList <DS>( );

            foreach (DS Neighbor in Neighbors)
            {
                GNeighs.Add(Neighbor, Neighbor.Path( ).Cost);
                OpNeighs.Add(Neighbor, Neighbor.Path( ).Actions.First.Value);
                Neighbor.ResetPath( );
                Metric HNeigh = null;
                if (HAdjusted != null && HAdjusted.TryGetValue(Neighbor.ToString( ), out HNeigh))
                {
                    HNeighs.Add(Neighbor, HNeigh);
                }
                else
                {
                    LookAheadNeighbors.AddFirst(Neighbor);
                }
            }

            Dictionary <DS, PriorityQueue <Metric, DS> > OpenLists =
                new Dictionary <DS, PriorityQueue <Metric, DS> >( );

#if OpenGl
            HashSet <DS> OpenStates = new HashSet <DS>( );
#endif
            HashSet <DS> ClosedList = new HashSet <DS>( );
            ClosedList.Add(Start);
            foreach (DS Neighbor in LookAheadNeighbors)
            {
                var ol = new PriorityQueue <Metric, DS>( );
                ol.Enqueue(h(Neighbor, HAdjusted, Heuristic, Goal, Actions, ss), Neighbor);
                OpenLists.Add(Neighbor, ol);
#if OpenGl
                OpenStates.Add(Neighbor);
#endif
            }

            while (true)
            {
                LinkedList <DS> OpenNeighbors = new LinkedList <DS>(LookAheadNeighbors);
                foreach (DS Neighbor in LookAheadNeighbors)
                {
                    PriorityQueue <Metric, DS> OpenList;
                    OpenLists.TryGetValue(Neighbor, out OpenList);
#if OpenGl
                    if (sv != null)
                    {
                        sv.VisualizeAlg(ss, ss.InitialDynamicState, new OpenGLStateVisualizer.OpenGlVisulizationData( )
                        {
                            List      = OpenStates as HashSet <GenericGridWorldDynamicState>,
                            HAdjusted = HAdjusted,
                        });
                    }
#endif
                    if (OpenList.IsEmpty( ))
                    {
                        HNeighs.Add(Neighbor, new Metric(double.PositiveInfinity));
                        OpenNeighbors.Remove(Neighbor);
                        continue;
                    }
                    DS Cur = OpenList.Dequeue( );
#if OpenGl
                    OpenStates.Remove(Cur);
#endif

                    ClosedList.Add(Cur);
                    if (Goal.IsSatisfiedBy(ss, Cur))
                    {
                        HNeighs.Add(Neighbor, Cur.Path( ).Cost);
                        OpenNeighbors.Remove(Neighbor);
                        continue;
                    }
                    else
                    {
                        if (++IterationExpansions > ComputationLimit)
                        {
                            HNeighs.Add(Neighbor, Cur.Path( ).Cost
                                        + Weight * h(Cur, HAdjusted, Heuristic, Goal, Actions, ss));
                            OpenNeighbors.Remove(Neighbor);
                            break;
                        }
                        Expansions++;
                        foreach (DS e in Cur.Expand(Actions, ss, Static))
                        {
                            if (!ClosedList.Contains(e) && !OpenList.Contains(e))
                            {
                                OpenList.Enqueue(Weight * h(e, HAdjusted, Heuristic, Goal, Actions, ss)
                                                 + gComputer.G(ss, e, Goal), e);
#if OpenGl
                                OpenStates.Add(e);
#endif
                            }
                            Generations++;
                        }
                    }
                }
                LookAheadNeighbors = OpenNeighbors;
                if (IterationExpansions > ComputationLimit || LookAheadNeighbors.Count == 0)
                {
                    break;
                }
            }
            foreach (DS Neighbor in LookAheadNeighbors)
            {
                PriorityQueue <Metric, DS> OpenList;
                OpenLists.TryGetValue(Neighbor, out OpenList);
                if (OpenList.IsEmpty( ))
                {
                    HNeighs.Add(Neighbor, new Metric(double.PositiveInfinity));
                }
                else
                {
                    DS Cur = OpenList.Dequeue( );
                    HNeighs.Add(Neighbor, Cur.Path( ).Cost
                                + Weight * h(Cur, HAdjusted, Heuristic, Goal, Actions, ss));
                }
            }

            Metric            Best       = null;
            Operator <SS, DS> BestOp     = null;
            Metric            SecondBest = null;

            foreach (DS Neighbor in Neighbors)
            {
                Metric            HNeigh  = null;
                Metric            GNeigh  = null;
                Operator <SS, DS> OpNeigh = null;
                HNeighs.TryGetValue(Neighbor, out HNeigh);
                GNeighs.TryGetValue(Neighbor, out GNeigh);
                OpNeighs.TryGetValue(Neighbor, out OpNeigh);
                if (HNeigh != null)
                {
                    Metric FNeigh = GNeigh + HNeigh;
                    if (Best == null || Best > FNeigh)
                    {
                        BestOp = OpNeigh;
                        Best   = FNeigh;
                    }
                    else if (SecondBest == null || SecondBest > FNeigh)
                    {
                        SecondBest = FNeigh;
                    }
                }
            }
            if (RTAStar && SecondBest != null)
            {
                if (h(Start, HAdjusted, Heuristic, Goal, Actions, ss) < SecondBest)
                {
                    h(Start, SecondBest, HAdjusted);
                }
            }
            else if ((RTAStar && SecondBest == null) || Best == null)
            {
                h(Start, new Metric(double.PositiveInfinity), HAdjusted);
            }
            else if (Best != null)
            {
                if (h(Start, HAdjusted, Heuristic, Goal, Actions, ss) < Best)
                {
                    h(Start, Best, HAdjusted);
                }
            }
            if (BestOp == null)
            {
                return(null);
            }
            else
            {
                return(Operator <SS, DS> .MakePath(BestOp));
            }
        }