public MDDNode(TimedMove move, int numOfAgents, MDD father)
{
this.move = move;
this.father = father;
setUntil = 0;
children = new LinkedList <MDDNode>();
parents = new LinkedList <MDDNode>();
coexistLinkedList = new LinkedList <MDDNode> [numOfAgents];
for (int i = 0; i < numOfAgents; i++)
{
coexistLinkedList[i] = new LinkedList <MDDNode>();
}
}
public MDDNode(TimedMove move, int numOfAgents, MDD mdd, bool supportPruning = true)
{
this.move = move;
this.mdd = mdd;
children = new LinkedList <MDDNode>();
parents = new LinkedList <MDDNode>();
if (supportPruning)
{
coexistLinkedList = new LinkedList <MDDNode> [numOfAgents];
for (int i = 0; i < numOfAgents; i++)
{
coexistLinkedList[i] = new LinkedList <MDDNode>();
}
}
}
public virtual void setup(CostTreeNode costNode)
{
AgentState agent;
maxCost = 0;
this.startingPos = problem.m_vAgents;
this.costs = costNode.costs;
maxCost = costNode.costs.Max();
for (int i = 0; i < startingPos.Length; i++)
{
agent = startingPos[i];
allMDDs[i] = new MDD(i, agent.agent.agentNum, agent.lastMove, costNode.costs[i], maxCost, startingPos.Length, problem);
}
generated = 0;
matchCounter = 0;
}
public CostTreeNodeSolver(ProblemInstance problem, CostTreeNode costNode, Run runner) //make sure agent numbers are in the correct order
{
this.runner = runner;
AgentState agent;
maxCost = 0;
allMDDs = new MDD[problem.GetNumOfAgents()];
this.startingPos = problem.m_vAgents;
this.costs = costNode.costs;
this.problem = problem;
maxCost = costNode.costs.Max();
for (int i = 0; i < startingPos.Length; i++)
{
agent = startingPos[i];
allMDDs[i] = new MDD(i, agent.agent.agentNum, agent.lastMove, costNode.costs[i], maxCost, startingPos.Length, problem);
}
matchCounter = 0;
}
/// <summary>
/// Match and prune MDD according to another MDD.
/// </summary>
/// <param name="other"></param>
/// <param name="checkTriples">If true, use the "3E" method.</param>
public PruningDone SyncMDDs(MDD other, bool checkTriples)
{
PruningDone ans = PruningDone.NOTHING;
if (this.levels == null || other.levels == null) // Either of the MDDs was already completely pruned already
{
return(PruningDone.EVERYTHING);
}
// Cheaply find the coexisting nodes on level zero - all nodes coexist because agent starting points never collide
LinkedList <MDDNode> coexistingNodesForLevelZero = new LinkedList <MDDNode>();
coexistingNodesForLevelZero.AddFirst(other.levels[0].First.Value);
levels[0].First.Value.setCoexist(coexistingNodesForLevelZero, other.mddNum);
for (int i = 1; i < levels.Length; i++)
{
LinkedListNode <MDDNode> toSetCoexisting = levels[i].First;
while (toSetCoexisting != null && toSetCoexisting.List != null) // FIXME: Can .First return null ever?
{
if (toSetCoexisting.Value.isDeleted) // Previous level marked this MDDNode for deletion. Delete it and continue to the next.
{
LinkedListNode <MDDNode> tempToSetCoexisting = toSetCoexisting;
toSetCoexisting = toSetCoexisting.Next;
levels[i].Remove(tempToSetCoexisting);
continue;
}
var coexistingForNode = new HashSet <MDDNode>();
// Go over all the node's parents and test their coexisting nodes' children for coexistance with this node
LinkedListNode <MDDNode> parent = toSetCoexisting.Value.parents.First;
while (parent != null)
{
bool validParent = false;
foreach (MDDNode coexist in parent.Value.coexistLinkedList[other.mddNum])
{
foreach (MDDNode child in coexist.children)
{
if (toSetCoexisting.Value.move.IsColliding(child.move) == false)
{
if (checkTriples == false ||
toSetCoexisting.Value.isCoexistingWithOtherMDDs(child, other.mddNum)) // The "3" part
{
validParent = true;
if (coexistingForNode.Contains(child) == false)
{
CostTreeNodeSolver.matchCounter++;
coexistingForNode.Add(child);
}
}
}
}
}
MDDNode parentDeletionCandidate = parent.Value;
parent = parent.Next;
if (!validParent)
{
toSetCoexisting.Value.removeParent(parentDeletionCandidate); // And continue up the levels if necessary
ans = PruningDone.SOME;
}
}
toSetCoexisting.Value.setCoexist(new LinkedList <MDDNode>(coexistingForNode), other.mddNum);
LinkedListNode <MDDNode> tempToSetCoexisting1 = toSetCoexisting;
toSetCoexisting = toSetCoexisting.Next;
if (tempToSetCoexisting1.Value.getCoexistCount(other.mddNum) == 0)
{
tempToSetCoexisting1.Value.delete();
ans = PruningDone.SOME;
}
}
if (levels[0].First.Value.children.Count == 0)
{
return(PruningDone.EVERYTHING);
}
}
return(ans);
}
public int sync2GDDs(MDD other)
{
int ans = 1;
if (this.levels == null || other.levels == null)
{
return(0);
}
bool validParent;
LinkedListNode <MDDNode> parent;
MDDNode parentToDelete;
LinkedListNode <MDDNode> toSetCoexisting;
LinkedListNode <MDDNode> tempToSetCoexisting;
LinkedList <MDDNode> coexitingForNodeLL = new LinkedList <MDDNode>();
HashSet <MDDNode> coexitingForNodeHS = new HashSet <MDDNode>();
coexitingForNodeLL.AddFirst(other.levels[0].First.Value);
levels[0].First.Value.setCoexist(coexitingForNodeLL, other.mddNum);
for (int i = 1; i < levels.Length; i++)
{
toSetCoexisting = levels[i].First;
while (toSetCoexisting != null && toSetCoexisting.List != null)
{
if (toSetCoexisting.Value.isDeleted)
{
tempToSetCoexisting = toSetCoexisting;
toSetCoexisting = toSetCoexisting.Next;
levels[i].Remove(tempToSetCoexisting);
continue;
}
coexitingForNodeLL = new LinkedList <MDDNode>();
coexitingForNodeHS.Clear();
parent = toSetCoexisting.Value.parents.First;
while (parent != null)
{
validParent = false;
foreach (MDDNode coexist in parent.Value.coexistLinkedList[other.mddNum])
{
foreach (MDDNode child in coexist.children)
{
if (!toSetCoexisting.Value.Equals(child))
{
if (!parent.Value.EqualsSwitch(child) || !toSetCoexisting.Value.EqualsSwitch(coexist))
{
validParent = true;
if (!coexitingForNodeHS.Contains(child))
{
CostTreeNodeSolver.matchCounter++;
coexitingForNodeHS.Add(child);
coexitingForNodeLL.AddFirst(child);
}
}
}
}
}
parentToDelete = parent.Value;
parent = parent.Next;
if (!validParent)
{
toSetCoexisting.Value.removeParent(parentToDelete);
ans = 2;
}
}
toSetCoexisting.Value.setCoexist(coexitingForNodeLL, other.mddNum);
tempToSetCoexisting = toSetCoexisting;
toSetCoexisting = toSetCoexisting.Next;
if (tempToSetCoexisting.Value.getCoexistCount(other.mddNum) == 0)
{
tempToSetCoexisting.Value.delete();
ans = 2;
}
}
if (levels[0].First.Value.children.Count == 0)
{
return(0);
}
}
return(ans);
}
public override LinkedList <Move>[] Solve(HashSet <TimedMove> conflictTable, HashSet_U <TimedMove> CBS_CAT)
{
MDD[] match = new MDD[2];
bool Converging = true;
int[] changed = new int[allMDDs.Length];
int currentIteration = 0;
int conflictStatus = 1;
while (Converging)
{
currentIteration++;
Converging = false;
for (int i = allMDDs.Length - 1; i >= 0; i--)
{
for (int j = i + 1; j < allMDDs.Length; j++)
{
if (changed[i] >= currentIteration - 1 || changed[j] >= currentIteration - 1)//if at least one of the two MDDs was changed during the last iteration
{
if (syncSize == 2)
{
conflictStatus = allMDDs[i].sync2GDDs(allMDDs[j]);
}
else if (syncSize == 3)
{
conflictStatus = allMDDs[i].sync3GDDs(allMDDs[j], j);
}
if (conflictStatus == 0)
{
return(null);
}
else if (conflictStatus == 2)
{
changed[i] = currentIteration;
Converging = true;
}
if (syncSize == 2)
{
conflictStatus = allMDDs[i].sync2GDDs(allMDDs[j]);
}
else if (syncSize == 3)
{
conflictStatus = allMDDs[i].sync3GDDs(allMDDs[j], j);
}
if (conflictStatus == 0)
{
return(null);
}
else if (conflictStatus == 2)
{
changed[i] = currentIteration;
Converging = true;
}
}
}
}
}
CostTreeSearchSolver.passed++;
if (allMDDs[0].levels == null)
{
return(null);
}
AStarMDD findSolution = new AStarMDD(allMDDs, runner, conflictTable, CBS_CAT);
LinkedList <Move>[] ans = findSolution.Solve();
generated = findSolution.generated;
expanded = findSolution.expanded;
caViolations = findSolution.conflictAvoidanceViolations;
return(ans);
}
public override LinkedList <Move>[] Solve(HashSet <TimedMove> conflictTable, HashSet_U <TimedMove> CBS_CAT)
{
AStarMDD findSolution;
LinkedList <Move>[] subCheck;
MDD[] match;
MddMatchAndPrune matcher = new MddMatchAndPrune(runner);
foreach (MDD checkValid in allMDDs)
{
if (checkValid.levels == null)
{
return(null);
}
}
if (maxGroupChecked >= 2)
{
match = new MDD[2];
for (int i = allMDDs.Length - 1; i >= 0; i--)
{
for (int j = i + 1; j < allMDDs.Length; j++)
{
match[0] = allMDDs[i];
match[1] = allMDDs[j];
//matcher.initialize(match);
//if (matcher.pruneMDDs() == false)
findSolution = new AStarMDD(match, runner, conflictTable, CBS_CAT);
subCheck = findSolution.Solve();
if (subCheck == null || subCheck[0] == null)
{
return(null);
}
}
}
}
if (maxGroupChecked >= 3)
{
match = new MDD[3];
for (int i = allMDDs.Length - 2; i >= 0; i--)
{
for (int j = i + 1; j < allMDDs.Length - 1; j++)
{
for (int t = j + 1; t < allMDDs.Length; t++)
{
match[0] = allMDDs[i];
match[1] = allMDDs[j];
match[2] = allMDDs[t];
//matcher.initialize(match);
//if (matcher.pruneMDDs() == false)
findSolution = new AStarMDD(match, runner, conflictTable, CBS_CAT);
subCheck = findSolution.Solve();
if (subCheck == null || subCheck[0] == null)
{
return(null);
}
}
}
}
}
if (maxGroupChecked >= 4)
{
match = new MDD[4];
for (int i = allMDDs.Length - 3; i >= 0; i--)
{
for (int j = i + 1; j < allMDDs.Length - 2; j++)
{
for (int t = j + 1; t < allMDDs.Length - 1; t++)
{
for (int m = t + 1; m < allMDDs.Length; m++)
{
match[0] = allMDDs[i];
match[1] = allMDDs[j];
match[2] = allMDDs[t];
match[3] = allMDDs[m];
//matcher.initialize(match);
//if (matcher.pruneMDDs() == false)
findSolution = new AStarMDD(match, runner, conflictTable, CBS_CAT);
subCheck = findSolution.Solve();
if (subCheck == null || subCheck[0] == null)
{
return(null);
}
}
}
}
}
}
CostTreeSearchSolver.passed++;
if (allMDDs[0].levels == null)
{
return(null);
}
findSolution = new AStarMDD(allMDDs, runner, conflictTable, CBS_CAT);
LinkedList <Move>[] ans = findSolution.Solve();
generated = findSolution.generated;
expanded = findSolution.expanded;
caViolations = findSolution.conflictAvoidanceViolations;
return(ans);
}
public override SinglePlan[] Solve(Dictionary <TimedMove, List <int> > conflictTable, Dictionary <TimedMove, List <int> > CBS_CAT)
{
MDD[] match = new MDD[2];
bool Converging = true;
int[] changed = new int[allMDDs.Length];
int currentIteration = 0;
MDD.PruningDone conflictStatus = MDD.PruningDone.NOTHING;
while (Converging)
{
currentIteration++;
Converging = false;
for (int i = allMDDs.Length - 1; i >= 0; i--)
{
for (int j = i + 1; j < allMDDs.Length; j++)
{
if (changed[i] >= currentIteration - 1 || changed[j] >= currentIteration - 1) // If at least one of the two MDDs was changed during the last iteration
{
conflictStatus = allMDDs[i].SyncMDDs(allMDDs[j], this.syncSize == 3);
if (conflictStatus == MDD.PruningDone.EVERYTHING)
{
return(null);
}
else if (conflictStatus == MDD.PruningDone.SOME)
{
changed[i] = currentIteration;
Converging = true;
}
conflictStatus = allMDDs[i].SyncMDDs(allMDDs[j], this.syncSize == 3);
if (conflictStatus == MDD.PruningDone.EVERYTHING)
{
return(null);
}
else if (conflictStatus == MDD.PruningDone.SOME)
{
changed[i] = currentIteration;
Converging = true;
}
}
}
}
}
CostTreeSearchSolver.passed++;
if (allMDDs[0].levels == null)
{
return(null);
}
AStarMDD findSolution = new AStarMDD(allMDDs, runner, conflictTable, CBS_CAT);
SinglePlan[] ans = findSolution.Solve();
generated = findSolution.generated;
expanded = findSolution.expanded;
caViolations = findSolution.conflictAvoidanceViolations;
return(ans);
}