private static void Dijkstra(ref ExtendedDictionary <DS, Metric> G, ExtendedDictionary <DS, Metric> H, DS Sstart, DS Sgoal, ref ExtendedDictionary <DS, DS> tree, ref PriorityQueue <Metric, DS> OPEN, ref HashSet <DS> CLOSED, Operator <SS, DS>[] Ops, SS ss, Func <DS, DS, Metric> h0) { foreach (DS s in CLOSED) { H.Set(s, Metric.Infinity); } OPEN = OPEN.Reorder(s => H.Lookup(s)); while (CLOSED.Count != 0 && !OPEN.IsEmpty( )) { DS s = OPEN.Dequeue( ); if (CLOSED.Contains(s)) { CLOSED.Remove(s); } foreach (DS sp in s.Expand(Ops, ss, false)) { Metric cost = c(sp, s, h0) + H.Lookup(s); if (CLOSED.Contains(sp) && H.Lookup(sp) > cost) { H.Set(sp, cost); if (!OPEN.Contains(sp)) { OPEN.Enqueue(cost, sp); } } } } }
/// <summary> /// The compute method which uses greedy search. /// </summary> /// <param name="StaticState">A static state to work on.</param> /// <param name="DynamicState">A dynamic state to work on.</param> /// <param name="Goal">The goal state(s) to search for.</param> /// <returns>Computes a path.</returns> public override IEnumerable <Path <SS, DS> > Compute(SS StaticState, DS DynamicState, Goal <SS, DS> Goal, Operator <SS, DS>[] Actions) { ExtendedDictionary <DS, DS> tree = new ExtendedDictionary <DS, DS>(x => null); ExtendedDictionary <DS, Metric> G = new ExtendedDictionary <DS, Metric>(k => Metric.Infinity); G.Set(DynamicState, Metric.Zero); uint IterationExpansions = 0; PriorityQueue <Metric, DS> OpenList = new PriorityQueue <Metric, DS>( ); HashSet <DS> ClosedList = new HashSet <DS>( ); OpenList.Enqueue(Metric.Zero, DynamicState); while (true) { if (OpenList.IsEmpty( )) { throw new PathNotFoundException("Path Not Found"); } DS Cur = OpenList.Dequeue( ); ClosedList.Add(Cur); if (Goal.IsSatisfiedBy(StaticState, Cur)) { yield return(Algorithm <SS, DS> .ExtractPath(StaticState.InitialDynamicState, Cur, tree, Actions, StaticState)); yield break; } else { this.Expansions++; IterationExpansions++; if (IterationExpansions > ComputationLimit) { yield return(Algorithm <SS, DS> .ExtractPath(StaticState.InitialDynamicState, Cur, tree, Actions, StaticState)); IterationExpansions = 1; } foreach (DS e in Cur.Expand(Actions, StaticState, true)) { if (!ClosedList.Contains(e)) { Metric cost = G.Lookup(Cur) + e.LastOperator.Cost(Cur); if (G.Lookup(e) > cost) { G.Set(e, cost); tree.Set(e, Cur); OpenList.Enqueue(cost, e); } } this.Generations++; } } } }
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); } } } }
/// <summary> /// The compute method which uses greedy search. /// </summary> /// <param name="StaticState">A static state to work on.</param> /// <param name="DynamicState">A dynamic state to work on.</param> /// <param name="Goal">The goal state(s) to search for.</param> /// <returns>Computes a path.</returns> public override IEnumerable <Path <SS, DS> > Compute(SS StaticState, DS DynamicState, Goal <SS, DS> Goal, Operator <SS, DS>[] Actions) { HashSet <DS> DeadEndList = new HashSet <DS>( ); InStateGoal <SS, DS> FakeGoal = new InStateGoal <SS, DS>(DynamicState); DS GoalState = Transformer.Transform(Goal, DynamicState); uint IterationExpansions = 0; ExtendedDictionary <DS, DS> tree = new ExtendedDictionary <DS, DS>(x => null); ExtendedDictionary <DS, Metric> G = new ExtendedDictionary <DS, Metric>(k => Metric.Infinity); G.Set(DynamicState, Metric.Zero); PriorityQueue <Metric, DS> OpenList = new PriorityQueue <Metric, DS>( ); HashSet <DS> ClosedList = new HashSet <DS>( ); OpenList.Enqueue(Metric.Zero, GoalState); while (true) { if (OpenList.IsEmpty( )) { throw new PathNotFoundException("Path Not Found"); } var Cur = OpenList.DequeueNode( ); ClosedList.Add(Cur.Second); if (FakeGoal.IsSatisfiedBy(StaticState, Cur.Second)) { LinkedList <Operator <SS, DS> > Operators = new LinkedList <Operator <SS, DS> >( ); foreach (var a in Algorithm <SS, DS> .ExtractPath(StaticState.InitialDynamicState, Cur.Second, tree, Actions, StaticState).Actions) { Operators.AddFirst(new ReverseOperator <SS, DS>(a)); } yield return(new Path <SS, DS>((DS)null) { Actions = Operators }); yield break; } else { this.Expansions++; if (++IterationExpansions >= ComputationLimit) { IEnumerable <DS> dss = DynamicState.Expand(Actions, StaticState, true).Where( ds => !DeadEndList.Contains(ds)); if (dss.All(ds => HeuristicSS.H(StaticState, ds, Cur.Second, Actions) > HeuristicSS.H(StaticState, DynamicState, Cur.Second, Actions)) ) { DeadEndList.Add(DynamicState); } if (!dss.Any( )) { dss = DynamicState.Expand(Actions, StaticState, true); } DS s = dss.ArgMin( ds => HeuristicSS.H(StaticState, ds, Cur.Second, Actions)); var p = Algorithm <SS, DS> .ExtractPath(StaticState.InitialDynamicState, Cur.Second, tree, Actions, StaticState); yield return(p); DynamicState = p.Actions.First( ).PerformOn(DynamicState, StaticState).First( ); if (OpenList.Contains(DynamicState)) { DS st; do { st = OpenList.Dequeue( ); } while (!st.Equals(DynamicState)); //TODO add method for this. LinkedList <Operator <SS, DS> > Operators = new LinkedList <Operator <SS, DS> >( ); foreach (var a in Algorithm <SS, DS> .ExtractPath(StaticState.InitialDynamicState, st, tree, Actions, StaticState).Actions) { Operators.AddFirst(new ReverseOperator <SS, DS>(a)); } yield return(new Path <SS, DS>((DS)null) { Actions = Operators }); yield break; } if (ClosedList.Contains(DynamicState)) { LinkedList <Operator <SS, DS> > Operators = new LinkedList <Operator <SS, DS> >( ); var pgot = Algorithm <SS, DS> .ExtractPath(StaticState.InitialDynamicState, ClosedList.First(ds => ds.Equals( DynamicState)), tree, Actions, StaticState); foreach (var a in pgot.Actions) { Operators.AddFirst(new ReverseOperator <SS, DS>(a)); } yield return(new Path <SS, DS>((DS)null) { Actions = Operators }); yield break; } FakeGoal = new InStateGoal <SS, DS>(DynamicState); IterationExpansions = 0; OpenList.Enqueue(Cur.First, Cur.Second); if (Sort) { /*PriorityQueue<Metric, DS> sortedOpen = * new PriorityQueue<Metric, DS>( ); * DS x; * while ( ( x = OpenList.Dequeue( ) ) != null ) { * sortedOpen.Enqueue( G.Lookup( Cur.Second ) + e.LastOperator.Cost( Cur.Second ), x ); * } * OpenList = sortedOpen; */ } } else { foreach (DS e in Cur.Second.Expand(Actions, StaticState, true )) { if (!ClosedList.Contains(e) && !OpenList.Contains(e)) { Metric cost = G.Lookup(Cur.Second) + e.LastOperator.Cost(Cur.Second); if (G.Lookup(e) > cost) { G.Set(e, cost); tree.Set(e, Cur.Second); OpenList.Enqueue(cost, e); } } this.Generations++; } } } } }
/// <summary> /// The compute method which uses greedy search. /// </summary> /// <param name="StaticState">A static state to work on.</param> /// <param name="DynamicState">A dynamic state to work on.</param> /// <param name="Goal">The goal state(s) to search for.</param> /// <returns>Computes a path.</returns> public override Path <SS, DS> ComputeComplete(SS StaticState, DS DynamicState, Goal <SS, DS> Goal, Operator <SS, DS>[] Actions) { PriorityQueue <Metric, DS> OpenList = new PriorityQueue <Metric, DS>( ); ExtendedDictionary <DS, DS> tree = new ExtendedDictionary <DS, DS>(x => null); ExtendedDictionary <DS, Metric> G = new ExtendedDictionary <DS, Metric>(k => Metric.Infinity); G.Set(DynamicState, Metric.Zero); HashSet <DS> ClosedList = new HashSet <DS>( ); OpenList.Enqueue(Metric.Zero, DynamicState); while (true) { #if OpenGl if (sv != null) { this.sv.VisualizeAlg(StaticState, StaticState.InitialDynamicState, new OpenGLStateVisualizer.OpenGlVisulizationData( ) { OL = OpenList as PriorityQueue <Metric, GenericGridWorldDynamicState>, }); } #endif if (OpenList.IsEmpty( )) { throw new PathNotFoundException("Path Not Found"); } DS Cur = OpenList.Dequeue( ); if (UseClosedList) { ClosedList.Add(Cur); } if (Goal.IsSatisfiedBy(StaticState, Cur)) { return(Algorithm <SS, DS> .ExtractPath(StaticState.InitialDynamicState, Cur, tree, Actions, StaticState)); } else { this.Expansions++; foreach (DS e in Cur.Expand(Actions, StaticState, true)) { e.BookKeeping = new ParentBookKeeping <SS, DS>( ); if (!UseClosedList) { (e.BookKeeping as ParentBookKeeping <SS, DS>).Parent = Cur; } if ((UseClosedList && !ClosedList.Contains(e) && !OpenList.Contains(e)) || (!UseClosedList && !CheckReversePathForDuplicates(e))) { Metric cost = G.Lookup(Cur) + e.LastOperator.Cost(Cur); if (G.Lookup(e) > cost) { G.Set(e, cost); tree.Set(e, Cur); OpenList.Enqueue(cost, e); } } this.Generations++; } } } }