public override IEnumerable <Path <SS, DS> > Compute(SS StaticState, DS DynamicState,
Goal <SS, DS> Goal, Operator <SS, DS>[] Actions)
{
bool solutionFound = false;
bool solutionFoundAndTraced = false;
bool doneTrace = true;
DS loc = DynamicState;
Path <SS, DS> pathNew = null;
Path <SS, DS> pathFollow = null;
Path <SS, DS> pathNewBuild = null;
Path <SS, DS> pathTaken = new Path <SS, DS>(DynamicState);
Alg.SetComputationLimit(Convert.ToInt32(ComputationLimit * r));
var searchStep = Alg.Compute(StaticState, DynamicState, Goal, Actions
).GetEnumerator( );
Alg.SetComputationLimit(Math.Min(Convert.ToInt32(ComputationLimit * r),
(ComputationLimit - Convert.ToInt32(ComputationLimit * r)) * c));
while (!Goal.IsSatisfiedBy(StaticState, loc))
{
if (!solutionFound)
{
solutionFound = !searchStep.MoveNext( );
Alg.SetComputationLimit(Convert.ToInt32(ComputationLimit * r));
}
if (!solutionFoundAndTraced)
{
if (doneTrace)
{
pathNew = searchStep.Current;
}
doneTrace = traceBack(ref pathNew, ref pathNewBuild, loc, (ComputationLimit -
Convert.ToInt32(ComputationLimit * r)) * c, DynamicState);
if (doneTrace && pathNewBuild.ActionCount != 0)
{
pathFollow = pathNewBuild;
if (Goal.IsSatisfiedBy(StaticState, pathFollow.States.Last.Value))
{
solutionFoundAndTraced = true;
}
}
}
var locO = loc;
Operator <SS, DS> op;
if (pathFollow == null)
{
yield return(null);
}
else
{
if (pathFollow.StateHashTable.Contains(loc))
{
if (pathFollow.Actions.Count != 0 &&
pathFollow.Actions.First.Value.IsValidOn(loc, StaticState, false))
{
var result = pathFollow.popFront( );
loc = pathFollow.States.First.Value;
op = result.Second;
pathTaken.Push(op, locO, loc);
}
else
{
op = new ReverseOperator <SS, DS>(pathTaken.Actions.Last.Value);
loc = op.PerformOn(locO, StaticState).First( );
pathTaken.Push(op, locO, loc);
}
}
else
{
if (!loc.Equals(DynamicState))
{
pathTaken.StateHashTable.Remove(pathTaken.States.Last.Value);
pathTaken.States.RemoveLast( );
op = new ReverseOperator <SS, DS>(pathTaken.Actions.Last.Value);
pathTaken.Actions.RemoveLast( );
loc = pathTaken.States.Last.Value;
}
else
{
op = new ReverseOperator <SS, DS>(pathTaken.Actions.Last.Value);
loc = op.PerformOn(locO, StaticState).First( );
pathTaken.Push(op, locO, loc);
}
}
if (!op.IsValidOn(locO, StaticState, false))
{
System.Console.Error.WriteLine("ERROR");
}
var p = new Path <SS, DS>((DS)null);
p.Push(op, loc, null);
yield return(p);
}
}
this.Expansions += this.Alg.Expansions;
this.Generations += this.Alg.Generations;
yield break;
}
public override IEnumerable <Path <SS, DS> > Compute(SS StaticState, DS DynamicState,
Goal <SS, DS> Goal, Operator <SS, DS>[] Actions)
{
bool solutionFound = false;
DS loc = DynamicState;
Operator <SS, DS> opLast = null;
PriorityQueue <Metric, DS> OpenList = new PriorityQueue <Metric, DS>( );
HashSet <DS> ClosedList = new HashSet <DS>( );
OpenList.Enqueue(Metric.Zero, DynamicState);
LinkedList <DS> pathFollow = null;
Path <SS, DS> actionPathFollow = null;
Path <SS, DS> pathTaken = new Path <SS, DS>((DS)null);
while (!Goal.IsSatisfiedBy(StaticState, loc))
{
if (!solutionFound)
{
/********************/
int IterationExpansions = 0;
while (true)
{
if (OpenList.IsEmpty( ))
{
throw new PathNotFoundException("Path Not Found");
}
DS Cur = OpenList.Dequeue( );
ClosedList.Add(Cur);
if (Goal.IsSatisfiedBy(StaticState, Cur))
{
OpenList.Enqueue(BestExpresion(StaticState, Cur, Goal, Actions), Cur);
solutionFound = true;
break;
}
else
{
this.Expansions++;
foreach (DS e in Cur.Expand(Actions, StaticState, false))
{
e.BookKeeping = new ParentBookKeeping <SS, DS>( );
(e.BookKeeping as ParentBookKeeping <SS, DS>).Parent = Cur;
if (!ClosedList.Contains(e))
{
OpenList.Enqueue(BestExpresion(StaticState, e, Goal, Actions), e);
}
this.Generations++;
}
if (++IterationExpansions >= ComputationLimit)
{
solutionFound = false;
break;
}
}
} /********************/
}
pathFollow = new LinkedList <DS>( );
var pN = OpenList.DequeueNode( );
OpenList.Enqueue(pN.First, pN.Second);
DS p = pN.Second;
var path = p.Path( );
actionPathFollow = new Path <SS, DS>((DS)null);
pathFollow.AddFirst(p);
var i = path.Actions.Reverse( ).GetEnumerator( );
while ((p.BookKeeping as ParentBookKeeping <SS, DS>) != null && (p = (p.BookKeeping as
ParentBookKeeping <SS, DS>).Parent) != null && (!p.Equals(DynamicState) || loc.Equals(DynamicState)))
{
pathFollow.AddFirst(p);
if (i.MoveNext( ))
{
actionPathFollow.Actions.AddFirst(i.Current);
}
if (p.Equals(loc))
{
break;
}
}
Operator <SS, DS> action;
if (pathFollow.Contains(loc))
{
action = actionPathFollow.Actions.First.Value;
if (!action.IsValidOn(loc, StaticState, false))
{
System.Console.Error.WriteLine("ERROR");
}
pathFollow.RemoveFirst( );
actionPathFollow.Actions.RemoveFirst( );
pathTaken.Push(action, loc, null);
}
else
{
if (!loc.Equals(DynamicState))
{
action = new ReverseOperator <SS, DS>(pathTaken.Actions.Last.Value);
pathTaken.Actions.RemoveLast( );
}
else
{
action = new ReverseOperator <SS, DS>(opLast);
pathTaken.Push(action, loc, null);
}
}
if (!action.IsValidOn(loc, StaticState, false))
{
System.Console.Error.WriteLine("ERROR");
}
loc = action.PerformOn(loc, StaticState).First( );
opLast = action;
yield return(new Path <SS, DS>(action));
}
yield break;
}
public override IEnumerable <Path <SS, DS> > Compute(SS StaticState,
DS DynamicState, Goal <SS, DS> Goal, Operator <SS, DS>[] Actions)
{
int Ne;
int remainingNe;
int Nt;
int remainingNt;
bool pathFollowIsValid = true;
Metric pathFollowCost = Metric.Zero;
Metric pathToTraceCost = Metric.Zero;
Path <SS, DS> pathToTrace = null;
Path <SS, DS> pathFollow = null;
Path <SS, DS> pathNew = null;
Path <SS, DS> pathTraced = null;
Path <SS, DS> pathTaken = new Path <SS, DS>(DynamicState);
bool doneTrace = true;
DS loc = DynamicState;
DS loc_last = loc;
Operator <SS, DS> op = null;
Operator <SS, DS> op_last = null;
this.StaticState = StaticState;
this.Sstart = DynamicState;
this.Sgoal = Transformer.Transform(Goal, Sstart);
this.Actions = Actions;
Ne = Convert.ToInt32(ComputationLimit * r);
if (Ne <= 0)
{
Ne = 1;
}
Nt = (ComputationLimit - Convert.ToInt32(ComputationLimit * r)) * c;
if (Nt <= 0)
{
Nt = 1;
}
var searchStep = Alg.Compute(
StaticState, DynamicState, Goal, Actions).GetEnumerator( );
Alg.SetComputationLimit(Math.Min(Ne, Nt));
while (!Goal.IsSatisfiedBy(StaticState, loc))
{
remainingNe = Alg.GetComputationLimit( );
searchStep.MoveNext( );
remainingNe -= Alg.GetComputationDone( );
if (searchStep.Current != null)
{
pathNew = searchStep.Current;
}
if (doneTrace && pathNew != null && (!pathFollowIsValid ||
(pathToTrace == null || !pathNew.Equals(pathToTrace)) &&
(pathToTrace == null || pathNew.Cost > pathToTraceCost)))
{
pathToTrace = pathNew;
pathToTraceCost = pathToTrace.Cost;
doneTrace = false;
}
remainingNt = Nt;
if (!doneTrace)
{
doneTrace = traceBack(ref pathToTrace, ref pathTraced, loc, Nt,
ref remainingNt, DynamicState, StaticState);
}
if (doneTrace && (!pathFollowIsValid || pathFollow == null ||
pathToTraceCost > pathFollowCost ||
(pathFollow.Actions.Count != 0 &&
!pathFollow.peekFront( ).Second.IsValidOn(
loc, StaticState, true))))
{
pathFollow = pathTraced;
pathFollowCost = pathToTraceCost;
pathToTrace = null;
pathFollowIsValid = true;
}
// shortcut detection
if (sd == ShortcutDetection.AStar &&
(remainingNe + (remainingNt / c)) > 0 &&
(!pathFollow.StateHashTable.Contains(loc)))
{
Path <SS, DS> ret = null;
#if DEBUG
System.Console.Error.WriteLine("expansions = {0}",
remainingNe + (remainingNt / c));
#endif
ret = AStarSearch(pathTraced, pathTaken, loc,
remainingNe + (remainingNt / c));
if (ret != null)
{
pathFollow = ret;
pathFollowIsValid = true;
}
}
if (pathFollow != null && pathFollow.StateHashTable.Contains(loc))
{
while (pathFollow.Actions.Count != 0 &&
!pathFollow.peekFront( ).First.Equals(loc))
{
pathFollow.popFront( );
}
}
if (pathFollow.Actions.Count == 0 || (pathFollow.Actions.Count != 0 &&
pathFollow.peekFront( ).First.Equals(loc) &&
!pathFollow.peekFront( ).Second.IsValidOn(
loc, StaticState, true)))
{
pathFollowIsValid = false;
}
// Movement Choice
if (pathFollowIsValid &&
pathFollow.Actions.Count != 0 &&
pathFollow.peekFront( ).First.Equals(loc) &&
pathFollow.peekFront( ).Second.IsValidOn(
loc, StaticState, true))
{
op = pathFollow.popFront( ).Second;
loc = op.PerformOn(loc_last, StaticState).First( );
pathTaken.Push(op, loc_last, loc);
}
else if (!loc.Equals(DynamicState))
{
op = new ReverseOperator <SS, DS>(pathTaken.popBack( ).Second);
loc = op.PerformOn(loc_last, StaticState).First( );
}
else
{
if (pathTraced != null && pathTraced.Cost < pathToTraceCost)
{
pathTraced = null;
pathToTrace = null;
doneTrace = true;
}
if (op_last == null)
{
op = null;
}
else
{
op = new ReverseOperator <SS, DS>(op_last);
loc = op.PerformOn(loc_last, StaticState).First( );
pathTaken.Push(op, loc_last, loc);
}
}
if (op == null)
{
Alg.SetComputationLimit(Ne);
yield return(null);
}
else
{
#if DEBUG
System.Console.Error.WriteLine(" loc = " + loc);
System.Console.Error.WriteLine(" op = " + op);
if (!op.IsValidOn(loc_last, StaticState, true) || loc.IsInvalid(StaticState))
{
System.Console.Error.WriteLine("ERROR");
}
#endif
loc_last = loc;
op_last = op;
var p = new Path <SS, DS>((DS)null);
p.Push(op, loc, null);
Alg.SetComputationLimit(Ne);
yield return(p);
}
#if DEBUG
if (!loc.Equals(StaticState.InitialDynamicState))
{
System.Console.Error.WriteLine("loc = " + loc.ToString()
+ " true = " + StaticState.InitialDynamicState.ToString());
System.Console.Error.WriteLine("");
}
#endif
}
this.Expansions += this.Alg.Expansions;
this.Generations += this.Alg.Generations;
yield break;
}
