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