/// <summary>
///
/// </summary>
/// <param name="mddNum"></param>
/// <param name="agentNum"></param>
/// <param name="start_pos"></param>
/// <param name="cost">The MDD must be of this cost</param>
/// <param name="numOfLevels">
/// The MDD must be of this number of levels, not counting level zero.
/// If higher than cost, the extra levels will be WAITs at the goal.
/// </param>
/// <param name="numOfAgents"></param>
/// <param name="instance"></param>
/// <param name="ignoreConstraints"></param>
public MDD(int mddNum, int agentNum, Move start_pos, int cost, int numOfLevels, int numOfAgents, ProblemInstance instance, bool ignoreConstraints = false, bool supportPruning = true)
{
this.problem = instance;
this.mddNum = mddNum;
this.agentNum = agentNum;
this.cost = cost;
this.levels = new LinkedList <MDDNode> [numOfLevels + 1];
this.supportPruning = supportPruning;
if (ignoreConstraints == false && instance.parameters.ContainsKey(CBS_LocalConflicts.CONSTRAINTS) &&
((HashSet_U <CbsConstraint>)instance.parameters[CBS_LocalConflicts.CONSTRAINTS]).Count != 0)
{
this.queryConstraint = new CbsConstraint();
this.queryConstraint.queryInstance = true;
this.constraints = (HashSet_U <CbsConstraint>)instance.parameters[CBS_LocalConflicts.CONSTRAINTS];
}
if (ignoreConstraints == false && instance.parameters.ContainsKey(CBS_LocalConflicts.MUST_CONSTRAINTS) &&
((HashSet_U <CbsConstraint>)instance.parameters[CBS_LocalConflicts.MUST_CONSTRAINTS]).Count != 0)
{
// TODO: Code dup with ClassicAStar's constructor
var musts = (HashSet_U <CbsConstraint>)instance.parameters[CBS_LocalConflicts.MUST_CONSTRAINTS];
this.mustConstraints = new Dictionary <int, TimedMove> [musts.Max <CbsConstraint>(con => con.GetTimeStep()) + 1]; // To have index MAX, array needs MAX + 1 places.
foreach (CbsConstraint con in musts)
{
int timeStep = con.GetTimeStep();
if (this.mustConstraints[timeStep] == null)
{
this.mustConstraints[timeStep] = new Dictionary <int, TimedMove>();
}
this.mustConstraints[timeStep][con.agentNum] = con.move;
}
}
var closedList = new Dictionary <MDDNode, MDDNode>();
var toDelete = new List <MDDNode>();
for (int i = 0; i <= numOfLevels; i++)
{
levels[i] = new LinkedList <MDDNode>();
}
MDDNode root = new MDDNode(new TimedMove(start_pos, 0), numOfAgents, this, supportPruning); // Root
LinkedListNode <MDDNode> llNode = new LinkedListNode <MDDNode>(root);
root.setMyNode(llNode);
llNode.Value.startOrGoal = true;
levels[0].AddFirst(llNode);
for (int i = 0; i < numOfLevels; i++) // For each level, populate the _next_ level
{
int heuristicBound = cost - i - 1; // We want g+h <= cost, so h <= cost-g. -1 because it's the bound of the _children_.
if (heuristicBound < 0)
{
heuristicBound = 0;
}
// Go over each MDDNode in this level
foreach (MDDNode currentMddNode in levels[i]) // Since we're not deleting nodes in this method, we can use the simpler iteration method :)
{
List <MDDNode> children = this.GetAllChildren(currentMddNode, heuristicBound, numOfAgents);
if (children.Count == 0) // Heuristic wasn't perfect because of constraints, illegal moves or other reasons
{
toDelete.Add(currentMddNode);
}
foreach (MDDNode child in children)
{
MDDNode toAdd = child; // The compiler won't let me assign to the foreach variable...
if (closedList.ContainsKey(child))
{
toAdd = closedList[child];
}
else
{
closedList.Add(toAdd, toAdd);
llNode = new LinkedListNode <MDDNode>(toAdd);
toAdd.setMyNode(llNode);
levels[i + 1].AddLast(toAdd);
}
currentMddNode.addChild(toAdd); // forward edge
toAdd.addParent(currentMddNode); // backward edge
}
}
closedList.Clear();
}
foreach (MDDNode goal in levels[numOfLevels]) // The goal may be reached in more than one direction
{
goal.startOrGoal = true;
}
foreach (MDDNode remove in toDelete)
{
remove.delete();
}
// Make sure the goal was reached - imperfect heuristics, constraints or illegal moves can cause this to be false.
if (levels[numOfLevels].Count == 0 || levels[0].First.Value.isDeleted == true) //if no possible route mark levels as null
{
levels = null;
}
}
public MDD(int mddNum, int agentNum, Move start_pos, int cost, int maxCostOnLevel, int numOfAgents, ProblemInstance instance)
{
//if (agentNum == 2 && maxCostOnLevel == 4)
// Console.Write("ff");
this.problem = instance;
this.mddNum = mddNum;
this.agentNum = agentNum;
levels = new LinkedList <MDDNode> [maxCostOnLevel + 1];
Hashtable closedList = new Hashtable();
LinkedList <MDDNode> children;
LinkedList <MDDNode> toDelete = null;
for (int i = 0; i <= maxCostOnLevel; i++)
{
levels[i] = new LinkedList <MDDNode>();
}
MDDNode toAdd = new MDDNode(new TimedMove(start_pos, 0), numOfAgents, this);
LinkedListNode <MDDNode> llNode = new LinkedListNode <MDDNode>(toAdd);
toAdd.setMyNode(llNode);
llNode.Value.startOrGoal = true;
levels[0].AddFirst(llNode);
for (int i = 0; i < maxCostOnLevel; i++)
{
int heuristicBound = cost - i - 1;
if (heuristicBound < 0)
{
heuristicBound = 0;
}
LinkedListNode <MDDNode> currentMddNode = levels[i].First;
while (currentMddNode != null)
{
LinkedListNode <MDDNode> child;
children = this.GetAllChildren(currentMddNode.Value, heuristicBound, numOfAgents, i);
child = children.First;
if (child == null)
{
if (toDelete == null)
{
toDelete = new LinkedList <MDDNode>();
}
toDelete.AddFirst(currentMddNode.Value);
}
while (child != null)
{
toAdd = child.Value;
if (closedList.Contains(toAdd))
{
toAdd = (MDDNode)closedList[toAdd];
}
else
{
closedList.Add(toAdd, toAdd);
llNode = new LinkedListNode <MDDNode>(toAdd);
toAdd.setMyNode(llNode);
levels[i + 1].AddLast(llNode);
}
currentMddNode.Value.addChild(toAdd);
toAdd.addParent(currentMddNode.Value);
child = child.Next;
}
currentMddNode = currentMddNode.Next;
}
closedList.Clear();
}
if (levels[maxCostOnLevel].Count != 0)
{
levels[maxCostOnLevel].First.Value.startOrGoal = true;
}
if (toDelete != null)
{
foreach (MDDNode remove in toDelete)
{
remove.delete();
}
}
if (levels[maxCostOnLevel].Count == 0 || levels[0].First.Value.isDeleted == true) //if no possible route mark levels as null
{
levels = null;
}
}