public void Expand(ExpandedNode currentNode)
{
while (true)
{
MDDStep child = currentNode.GetNextChild();
if (child == null)
{
break;
}
if (IsLegalMove(child))
{
child.conflicts = currentNode.parent.conflicts;
child.SetConflicts(ID_CAT, CBS_CAT);
if (this.closedList.ContainsKey(child) == true)
{
MDDStep inClosedList = this.closedList[child];
if (inClosedList.conflicts > child.conflicts)
{
closedList.Remove(inClosedList);
openList.Remove(inClosedList);
}
}
if (this.closedList.ContainsKey(child) == false)
{
this.openList.Add(child);
this.closedList.Add(child, child);
generated++;
}
}
}
}
private SinglePlan[] GetAnswer(MDDStep finish)
{
// TODO: Move the construction of the SinglePlans to a static method in SinglePlan
var routes = new LinkedList <Move> [problem.Length];
for (int i = 0; i < routes.Length; i++)
{
routes[i] = new LinkedList <Move>();
}
MDDStep current = finish;
while (current != null)
{
for (int i = 0; i < problem.Length; i++)
{
routes[i].AddFirst(new Move(current.allSteps[i].move));
}
current = current.prevStep;
}
var ans = new SinglePlan[problem.Length];
for (int i = 0; i < ans.Length; i++)
{
ans[i] = new SinglePlan(routes[i], i);
}
return(ans);
}
private LinkedList <Move>[] GetAnswer(MDDStep finish)
{
if (finish == null)
{
return(new LinkedList <Move> [1]);
}
LinkedList <Move>[] ans = new LinkedList <Move> [problem.Length];
Move.Direction direction;
for (int i = 0; i < ans.Length; i++)
{
ans[i] = new LinkedList <Move>();
}
MDDStep current = finish;
while (current.prevStep != null)
{
for (int i = 0; i < problem.Length; i++)
{
direction = Move.getDirection(current.allSteps[i].getX(), current.allSteps[i].getY(), current.prevStep.allSteps[i].getX(), current.prevStep.allSteps[i].getY());
ans[i].AddFirst(new Move(current.allSteps[i].getX(), current.allSteps[i].getY(), direction));
}
current = current.prevStep;
}
for (int i = 0; i < problem.Length; i++)
{
ans[i].AddFirst(new Move(current.allSteps[i].getX(), current.allSteps[i].getY(), 0));
}
return(ans);
}
private bool GoalTest(MDDStep toCheck)
{
if (toCheck.GetDepth() == problem[0].levels.Length - 1)
{
return(true);
}
return(false);
}
public MDDStep(MDDStep cpy)
{
this.allSteps = new MDDNode[cpy.allSteps.Length];
for (int i = 0; i < allSteps.Length; i++)
{
this.allSteps[i] = cpy.allSteps[i];
}
this.prevStep = cpy.prevStep;
}
public void Setup(MDDStep parent)
{
this.parent = parent;
this.chosenChild = new int[parent.allSteps.Length];
foreach (MDDNode node in parent.allSteps)
{
if (node.children.Count == 0)
{
this.chosenChild[0] = -1;
break;
}
}
}
private bool IsLegalMove(MDDStep to)
{
if (to == null)
{
return(false);
}
if (to.prevStep == null)
{
return(true);
}
for (int i = 0; i < problem.Length; i++)
{
for (int j = i + 1; j < to.allSteps.Length; j++)
{
if (CheckIfLegal(to.allSteps[i], to.allSteps[j]) == false)
{
return(false);
}
}
}
return(true);
}
public AStarMDD(MDD[] problem, Run runner, Dictionary <TimedMove, List <int> > conflicts, Dictionary <TimedMove, List <int> > CBS_CAT)
{
this.expanded = 0;
this.generated = 0;
MDDStep root;
this.problem = problem;
this.runner = runner;
this.ID_CAT = conflicts;
this.CBS_CAT = CBS_CAT;
this.closedList = new Dictionary <MDDStep, MDDStep>();
this.openList = new BinaryHeap();
MDDNode[] sRoot = new MDDNode[problem.Length];
for (int i = 0; i < problem.Length; i++)
{
sRoot[i] = problem[i].levels[0].First.Value;
sRoot[i].legal = true;
}
root = new MDDStep(sRoot, null);
openList.Add(root);
closedList.Add(root, root); // There will never be a hit. This is only done for consistancy
conflictAvoidanceViolations = 0;
}
public AStarMDD(MDD[] problem, Run runner, HashSet <TimedMove> conflicts, HashSet_U <TimedMove> CBS_CAT)
{
this.expanded = 0;
this.generated = 0;
MDDStep root;
this.problem = problem;
this.runner = runner;
this.ID_CAT = conflicts;
this.CBS_CAT = CBS_CAT;
this.closedList = new Dictionary <MDDStep, MDDStep>();
this.openList = new BinaryHeap();
MDDNode[] sRoot = new MDDNode[problem.Length];
for (int i = 0; i < problem.Length; i++)
{
sRoot[i] = problem[i].levels[0].First.Value;
sRoot[i].legal = true;
}
root = new MDDStep(sRoot, null);
openList.Add(root);
// Not adding it automatically to the closed list here?
conflictAvoidanceViolations = 0;
}
public int CompareTo(IBinaryHeapItem other)
{
MDDStep that = (MDDStep)other;
if (this.conflicts < that.conflicts)
{
return(-1);
}
if (this.conflicts > that.conflicts)
{
return(1);
}
if (this.GetDepth() > that.GetDepth())
{
return(-1);
}
if (this.GetDepth() < that.GetDepth())
{
return(1);
}
return(0);
}
/// <summary>
/// Only compares the steps.
/// </summary>
/// <param name="obj"></param>
/// <returns></returns>
public override bool Equals(object obj)
{
MDDStep comp = (MDDStep)obj;
return(this.allSteps.SequenceEqual <MDDNode>(comp.allSteps));
}
public MDDStep(MDDNode[] allSteps, MDDStep prevStep)
{
this.allSteps = allSteps;
this.prevStep = prevStep;
}
