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));
}
}
