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