/// <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; } }