private State <T>[] EmptyQueueIntoArray() { int size = OpenListSize; State <T>[] array = new State <T> [size]; for (int i = 0; i < size; ++i) { array[i] = OpenList.Dequeue(); } return(array); }
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)); } }