/// <summary>
/// Setup the relevant data structures for a run (possibly under CBS).
/// </summary>
/// <param name="problemInstance"></param>
/// <param name="minTimeStep"></param>
/// <param name="runner"></param>
/// <param name="CAT"></param>
/// <param name="constraints"></param>
/// <param name="positiveConstraints"></param>
/// <param name="minCost"></param>
/// <param name="maxCost"></param>
/// <param name="mdd">FIXME: Not taken into account, just added to comply with ICbsSolver</param>
public virtual void Setup(ProblemInstance problemInstance, int minTimeStep, Run runner,
ConflictAvoidanceTable CAT = null,
ISet <CbsConstraint> constraints = null, ISet <CbsConstraint> positiveConstraints = null,
int minCost = -1, int maxCost = int.MaxValue, MDD mdd = null)
{
this.minConflictsNotAvoided = int.MaxValue;
this.survivedPruningHL = 0;
this.goalTestSkipped = 0;
this.generatedHL = 1;
this.expandedHL = 1;
this.generatedLL = 0;
this.expandedLL = 0;
this.totalCost = Constants.TIMEOUT_COST;
this.problem = problemInstance;
this.runner = runner;
closedList = new HashSet <CostTreeNode>();
openList = new Queue <CostTreeNode>();
int[] costs = new int[problem.GetNumOfAgents()];
for (int i = 0; i < problem.GetNumOfAgents(); i++)
{
costs[i] = Math.Max(problem.GetSingleAgentOptimalCost(problem.agents[i]), minTimeStep); // TODO: Use the time of the latest constraint on each agent!
}
openList.Enqueue(new CostTreeNode(costs)); // The root
this.initialEstimate = openList.Peek().costs.Sum(); // TODO: Support other cost functions
// Store parameters used by the Independence Detection algorithm
this.maxCost = maxCost;
this.minCost = minCost;
this.CAT = CAT;
}
public override void Setup(ProblemInstance problemInstance, int minTimeStep, Run runner,
ConflictAvoidanceTable CAT, ISet <CbsConstraint> constraints, ISet <CbsConstraint> positiveConstraints,
int minCost, int maxCost, MDD mdd)
{
base.Setup(problemInstance, minTimeStep, runner, CAT, constraints, positiveConstraints, minCost, maxCost, mdd);
this.generatedAndDiscarded = 0;
this.expandedFullStates = 0;
}
public override void Setup(ProblemInstance problemInstance, int minDepth, Run runner,
ConflictAvoidanceTable CAT = null,
ISet <CbsConstraint> constraints = null, ISet <CbsConstraint> positiveConstraints = null,
int minCost = -1, int maxCost = int.MaxValue, MDD mdd = null)
{
base.Setup(problemInstance, minDepth, runner, CAT, constraints, positiveConstraints,
minCost, maxCost, mdd);
this.expandedFullStates = 0;
}
public override void Setup(ProblemInstance problemInstance, int minTimeStep, Run runner,
ConflictAvoidanceTable CAT = null,
ISet <CbsConstraint> constraints = null, ISet <CbsConstraint> positiveConstraints = null,
int minCost = -1, int maxCost = int.MaxValue, MDD mdd = null)
{
edgesMatrix = new int[problemInstance.agents.Length, problemInstance.GetMaxX() * problemInstance.GetMaxY() + problemInstance.GetMaxY(), Move.NUM_NON_DIAG_MOVES];
edgesMatrixCounter = 0;
base.Setup(problemInstance, minTimeStep, runner, CAT, constraints, positiveConstraints, minCost, maxCost, mdd);
}
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)
{
coexistingNodesFromOtherMdds = new HashSet <MDDNode> [numOfAgents];
for (int i = 0; i < numOfAgents; i++)
{
coexistingNodesFromOtherMdds[i] = new HashSet <MDDNode>(5); // Each level is small
}
}
}
public MDD(MDD other, CbsConstraint newConstraint)
: this(other)
{
List <MDDNode> toDelete = new List <MDDNode>();
foreach (MDDNode mddNode in this.levels[newConstraint.time])
{
if (newConstraint.move.Equals(mddNode.move))
{
toDelete.Add(mddNode);
}
}
Trace.Assert(toDelete.Count > 0);
foreach (MDDNode mddNode in toDelete)
{
mddNode.delete();
}
}
public MDD(MDD other, ISet <TimedMove> reserved) : this(other)
{
ISet <int> times_of_reservations = reserved.Select(move => move.time).ToHashSet();
var toDelete = new List <MDDNode>();
foreach (int time in times_of_reservations)
{
foreach (MDDNode mddNode in this.levels[time])
{
if (mddNode.move.IsColliding(reserved))
{
toDelete.Add(mddNode);
}
}
foreach (MDDNode mddNode in toDelete)
{
mddNode.delete();
}
toDelete.Clear();
}
}
public override SinglePlan[] Solve(ConflictAvoidanceTable 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
{
int matchCounterIncrement;
(conflictStatus, matchCounterIncrement) = allMDDs[i].SyncMDDs(allMDDs[j], this.syncSize == 3);
this.matchCounter += matchCounterIncrement;
if (conflictStatus == MDD.PruningDone.EVERYTHING)
{
return(null);
}
else if (conflictStatus == MDD.PruningDone.SOME)
{
changed[i] = currentIteration;
Converging = true;
}
(conflictStatus, matchCounterIncrement) = allMDDs[i].SyncMDDs(allMDDs[j], this.syncSize == 3);
this.matchCounter += matchCounterIncrement;
if (conflictStatus == MDD.PruningDone.EVERYTHING)
{
return(null);
}
else if (conflictStatus == MDD.PruningDone.SOME)
{
changed[i] = currentIteration;
Converging = true;
}
}
}
}
}
this.solver.survivedPruningHL++;
if (allMDDs[0].levels == null)
{
return(null);
}
A_Star_MDDs findSolution = new A_Star_MDDs(allMDDs, runner, CAT);
SinglePlan[] ans = findSolution.Solve();
generated = findSolution.generated;
expanded = findSolution.expanded;
conflictsNotAvoided = findSolution.conflictCount;
return(ans);
}
public override SinglePlan[] Solve(ConflictAvoidanceTable CAT)
{
A_Star_MDDs findSolution;
SinglePlan[] subCheck;
MDD[] match;
MddMatchAndPrune matcher = new MddMatchAndPrune(runner, this);
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 A_Star_MDDs(match, runner, 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 A_Star_MDDs(match, runner, 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 A_Star_MDDs(match, runner, CAT);
subCheck = findSolution.Solve();
if (subCheck == null || subCheck[0] == null)
{
return(null);
}
}
}
}
}
}
this.solver.survivedPruningHL++;
if (allMDDs[0].levels == null)
{
return(null);
}
findSolution = new A_Star_MDDs(allMDDs, runner, CAT);
SinglePlan[] ans = findSolution.Solve();
generated = findSolution.generated;
expanded = findSolution.expanded;
conflictsNotAvoided = findSolution.conflictCount;
conflictCounts = findSolution.GetExternalConflictCounts();
conflictTimes = findSolution.GetConflictTimes();
return(ans);
}
/// <summary>
/// Match and prune MDD according to another MDD.
/// </summary>
/// <param name="other"></param>
/// <param name="checkTriples">If true, use the "3E" method.</param>
/// <returns>How much pruning was done, and by how much the matchCounter should be incremented</returns>
public (PruningDone, int) SyncMDDs(MDD other, bool checkTriples)
{
int matchCounter = 0;
PruningDone pruningDone = PruningDone.NOTHING;
if (this.levels == null || other.levels == null) // Either of the MDDs was already completely pruned already
{
return(PruningDone.EVERYTHING, matchCounter);
}
// Cheaply find the coexisting nodes on level zero - all nodes coexist because agent starting points never collide
var coexistingNodesForLevelZero = new HashSet <MDDNode>()
{
other.levels[0].First.Value
};
levels[0].First.Value.SetCoexistingNodes(coexistingNodesForLevelZero, other.mddNum);
for (int i = 1; i < levels.Length; i++)
{
LinkedListNode <MDDNode> linkedListNode = levels[i].First;
while (linkedListNode != null && linkedListNode.List != null)
{
var node = linkedListNode.Value;
linkedListNode = linkedListNode.Next; // Must be before any potential deletions!
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> parentLinkedListNode = node.parents.First;
while (parentLinkedListNode != null)
{
var parent = parentLinkedListNode.Value;
bool validParent = false;
foreach (MDDNode parentCoexistingNode in parent.coexistingNodesFromOtherMdds[other.mddNum])
{
foreach (MDDNode childOfParentCoexistingNode in parentCoexistingNode.children)
{
if (node.move.IsColliding(childOfParentCoexistingNode.move) == false)
{
if (checkTriples == false ||
node.IsCoexistingWithOtherMDDs(childOfParentCoexistingNode, other.mddNum)) // The "3" part
{
validParent = true;
if (coexistingForNode.Add(childOfParentCoexistingNode))
{
matchCounter++;
}
}
}
else
{
}
}
}
parentLinkedListNode = parentLinkedListNode.Next; // Must be before any potential deletions!
if (!validParent)
{
node.removeParent(parent); // And continue up the levels if necessary
pruningDone = PruningDone.SOME;
}
}
node.SetCoexistingNodes(coexistingForNode, other.mddNum);
if (node.getCoexistingNodesCount(other.mddNum) == 0)
{
node.delete();
pruningDone = PruningDone.SOME;
}
}
if (levels == null || levels[0].Count == 0)
{
return(PruningDone.EVERYTHING, matchCounter);
}
}
return(pruningDone, matchCounter);
}
public MDD(MDD other)
{
this.problem = other.problem;
this.mddNum = other.mddNum;
this.agentNum = other.agentNum;
this.numOfAgents = other.numOfAgents;
this.cost = other.cost;
this.supportPruning = other.supportPruning;
if (other.levels == null)
{
this.levels = null;
return;
}
this.levels = new LinkedList <MDDNode> [other.levels.Length];
int numOfLevels = other.levels.Length - 1; // Level zero not counted
for (int i = 0; i < levels.Length; i++)
{
levels[i] = new LinkedList <MDDNode>();
}
Dictionary <MDDNode, MDDNode> originals = new Dictionary <MDDNode, MDDNode>();
Dictionary <MDDNode, MDDNode> copies = new Dictionary <MDDNode, MDDNode>();
MDDNode copiedRoot = new MDDNode(new TimedMove(other.levels[0].First.Value.move),
numOfAgents, this, supportPruning); // Root
LinkedListNode <MDDNode> llNode = new LinkedListNode <MDDNode>(copiedRoot);
copiedRoot.setMyNode(llNode);
llNode.Value.startOrGoal = true;
levels[0].AddFirst(llNode);
originals.Add(copiedRoot, other.levels[0].First.Value);
copies.Add(other.levels[0].First.Value, copiedRoot);
for (int i = 0; i < numOfLevels; i++) // For each level, populate the _next_ level
{
foreach (MDDNode copiedNode in levels[i])
{
foreach (MDDNode originalChildNode in originals[copiedNode].children)
{
MDDNode copiedChild;
if (copies.ContainsKey(originalChildNode) == false)
{
copiedChild = new MDDNode(originalChildNode.move, numOfLevels, this, supportPruning);
originals.Add(copiedChild, originalChildNode);
copies.Add(originalChildNode, copiedChild);
llNode = new LinkedListNode <MDDNode>(copiedChild);
copiedChild.setMyNode(llNode);
levels[i + 1].AddLast(llNode);
}
else
{
copiedChild = copies[originalChildNode];
}
copiedNode.addChild(copiedChild); // forward edge
copiedChild.addParent(copiedNode); // backward edge
}
}
}
originals.Clear();
copies.Clear();
foreach (MDDNode goal in levels[numOfLevels]) // The goal may be reached in more than one direction
{
goal.startOrGoal = true;
}
}