private SinglePlan[] GetAnswer(A_Star_MDDs_Node 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>();
}
A_Star_MDDs_Node current = finish;
while (current != null)
{
for (int i = 0; i < problem.Length; i++)
{
routes[i].AddFirst(new Move(current.allSteps[i].move));
}
current = current.prev;
}
var ans = new SinglePlan[problem.Length];
for (int i = 0; i < ans.Length; i++)
{
ans[i] = new SinglePlan(routes[i], i);
}
return(ans);
}
private bool GoalTest(A_Star_MDDs_Node toCheck)
{
if (toCheck.GetDepth() == problem[0].levels.Length - 1)
{
return(true);
}
return(false);
}
/// <summary>
/// Only compares the steps.
/// </summary>
/// <param name="obj"></param>
/// <returns></returns>
public override bool Equals(object obj)
{
if (obj == null)
{
return(false);
}
A_Star_MDDs_Node comp = (A_Star_MDDs_Node)obj;
return(this.allSteps.SequenceEqual <MDDNode>(comp.allSteps));
}
public A_Star_MDDs_Node(MDDNode[] allSteps, A_Star_MDDs_Node prevStep)
{
this.allSteps = allSteps;
this.prev = prevStep;
this.currentMoves = null; // All non-intermediate nodes have currentMoves == null
this.conflictCount = 0;
// Initialize conflict tracking data structures
this.conflictCounts = new Dictionary <int, int>();
this.conflictTimes = new Dictionary <int, List <int> >();
}
public void Setup(A_Star_MDDs_Node a_star_mdd_node)
{
this.a_star_mdd_node = a_star_mdd_node;
this.chosenChild = new int[a_star_mdd_node.allSteps.Length];
foreach (MDDNode mddNode in a_star_mdd_node.allSteps)
{
if (mddNode.children.Count == 0)
{
this.chosenChild[0] = -1;
break;
}
}
}
protected List <A_Star_MDDs_Node> ExpandOneAgent(List <A_Star_MDDs_Node> intermediateNodes, int mddIndex)
{
var generated = new List <A_Star_MDDs_Node>();
// Expand the mdd node
foreach (A_Star_MDDs_Node node in intermediateNodes)
{
// Try all the children of this MDD node
foreach ((int childIndex, MDDNode childMddNode) in node.allSteps[mddIndex].children.Enumerate())
{
if (node.currentMoves != null && childMddNode.move.IsColliding(node.currentMoves)) // Can happen. We only prune partially, we don't build the full k-agent MDD.
{
continue;
}
var childNode = new A_Star_MDDs_Node(node, mddIndex != node.allSteps.Length - 1);
childNode.allSteps[mddIndex] = childMddNode;
// Update target conflict count and prune nodes that can't get to the target conflict count
childNode.UpdateRemainingDeltaConflictCount(mddIndex, childIndex);
if (childNode.remainingDeltaConflictCount == ushort.MaxValue || // Last move was bad - not sure this can happen here
(childNode.hasChildrenForCurrentDeltaConflictCount(mddIndex + 1) == false)) // No children that can reach the target
{
continue;
}
if (mddIndex < node.allSteps.Length - 1) // More MDD nodes need to choose a child
{
childNode.currentMoves.Add(childMddNode.move);
}
else // Moved the last agent
{
childNode.currentMoves = null; // To reduce memory load and lookup times
}
// Set the node's prev to its real parent, skipping over the intermediate nodes.
if (mddIndex != 0)
{
childNode.prev = node.prev;
}
else
{
childNode.prev = node;
}
generated.Add(childNode);
}
}
return(generated);
}
/// <summary>
/// Copy constructor
/// </summary>
public A_Star_MDDs_Node(A_Star_MDDs_Node cpy, bool createIntermediate)
{
this.allSteps = new MDDNode[cpy.allSteps.Length];
for (int i = 0; i < allSteps.Length; i++)
{
this.allSteps[i] = cpy.allSteps[i];
}
this.prev = cpy.prev;
if (cpy.currentMoves != null)
{
// cpy is an intermediate node
if (createIntermediate)
{
this.currentMoves = new HashSet <TimedMove>(cpy.currentMoves);
}
else
{
this.currentMoves = cpy.currentMoves; // We're not going to add anything currentMoves
}
}
else
{
// cpy is a concrete node
this.currentMoves = new HashSet <TimedMove>(capacity: cpy.allSteps.Length);
}
// The conflictTimes and conflictCounts are only copied later if necessary.
alreadyExpanded = false; // Creating a new unexpanded node from cpy
// For intermediate nodes created during expansion (fully expanded nodes have these fields recalculated when they're expanded)
targetDeltaConflictCount = cpy.targetDeltaConflictCount; // Just to ease debugging
remainingDeltaConflictCount = cpy.remainingDeltaConflictCount;
singleAgentDeltaConflictCounts = cpy.singleAgentDeltaConflictCounts; // For the UpdateRemainingDeltaConflictCount call on temporary nodes.
// Notice that after an agent is moved its row won't be up-to-date.
conflictCountLookup = cpy.conflictCountLookup; // For the hasChildrenForCurrentDeltaConflictCount call on temporary nodes.
// Notice that after an agent is moved, all rows up to and including the one of the agent that moved
// won't be up-to-date.
maxDeltaConflictCount = cpy.maxDeltaConflictCount; // Not necessarily achievable after some of the agents moved.
// The above is OK because we won't be using data for agents that already moved.
}
public A_Star_MDDs(MDD[] problem, Run runner, ConflictAvoidanceTable CAT)
{
this.expanded = 0;
this.generated = 0;
A_Star_MDDs_Node root;
this.problem = problem;
this.runner = runner;
this.CAT = CAT;
this.closedList = new Dictionary <A_Star_MDDs_Node, A_Star_MDDs_Node>();
this.openList = new BinaryHeap <A_Star_MDDs_Node>();
MDDNode[] sRoot = new MDDNode[problem.Length];
for (int i = 0; i < problem.Length; i++)
{
sRoot[i] = problem[i].levels[0].First.Value;
}
root = new A_Star_MDDs_Node(sRoot, null);
openList.Add(root);
closedList.Add(root, root); // There will never be a hit. This is only done for consistancy
conflictCount = 0;
}
private bool IsLegalMove(A_Star_MDDs_Node to)
{
if (to == null)
{
return(false);
}
if (to.prev == 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);
}
/// <summary>
/// Prefers fewer conflicts. If the number of conflicts is the same, prefers more depth.
/// </summary>
/// <param name="other"></param>
/// <returns></returns>
public int CompareTo(IBinaryHeapItem other)
{
A_Star_MDDs_Node that = (A_Star_MDDs_Node)other;
if (this.conflictCount + this.targetDeltaConflictCount < that.conflictCount + that.targetDeltaConflictCount)
{
return(-1);
}
if (this.conflictCount + this.targetDeltaConflictCount > that.conflictCount + that.targetDeltaConflictCount)
{
return(1);
}
if (this.GetDepth() > that.GetDepth())
{
return(-1);
}
if (this.GetDepth() < that.GetDepth())
{
return(1);
}
return(0);
}
public void Expand(A_Star_MDDs_Expander currentNode)
{
while (true)
{
A_Star_MDDs_Node child = currentNode.GetNextChild();
if (child == null)
{
break;
}
if (IsLegalMove(child))
{
child.conflictCount = child.prev.conflictCount;
child.UpdateConflicts(CAT);
bool was_closed = this.closedList.ContainsKey(child);
if (was_closed)
{
A_Star_MDDs_Node inClosedList = this.closedList[child];
if (inClosedList.conflictCount > child.conflictCount)
{
closedList.Remove(inClosedList);
openList.Remove(inClosedList);
was_closed = false;
}
}
if (!was_closed)
{
this.openList.Add(child);
this.closedList.Add(child, child);
generated++;
}
}
}
}
public void Expand(A_Star_MDDs_Node node)
{
if (node.IsAlreadyExpanded() == false)
{
node.calcSingleAgentDeltaConflictCounts(this.CAT);
node.alreadyExpanded = true;
node.targetDeltaConflictCount = 0;
node.remainingDeltaConflictCount = node.targetDeltaConflictCount; // Just for the following hasChildrenForCurrentDeltaConflictCount call.
while (node.hasMoreChildren() && node.hasChildrenForCurrentDeltaConflictCount() == false) // DeltaConflictCount==0 may not be possible if all agents have obstacles between their location and the goal
{
node.targetDeltaConflictCount++;
node.remainingDeltaConflictCount = node.targetDeltaConflictCount;
}
if (node.hasMoreChildren() == false) // Node has no possible children at all
{
node.ClearExpansionData();
return;
}
}
var intermediateNodes = new List <A_Star_MDDs_Node>()
{
node
};
for (int mddIndex = 0; mddIndex < this.problem.Length && intermediateNodes.Count != 0; ++mddIndex)
{
if (runner.ElapsedMilliseconds() > Constants.MAX_TIME)
{
return;
}
intermediateNodes = ExpandOneAgent(intermediateNodes, mddIndex);
}
var finalGeneratedNodes = intermediateNodes;
foreach (var child in finalGeneratedNodes)
{
child.conflictCount = node.conflictCount + node.targetDeltaConflictCount;
// Accumulating the conflicts count from parent to child
// We're counting conflicts along the entire path, so the parent's conflicts count is added to the child's:
child.conflictCounts = new Dictionary <int, int>(child.prev.conflictCounts);
child.conflictTimes = new Dictionary <int, List <int> >();
foreach (var kvp in child.prev.conflictTimes)
{
child.conflictTimes[kvp.Key] = new List <int>(kvp.Value);
}
child.IncrementConflictCounts(this.CAT); // We're counting conflicts along the entire path, so the parent's conflicts count
// is added to the child's.
bool was_closed = this.closedList.ContainsKey(child);
if (was_closed)
{
A_Star_MDDs_Node inClosedList = this.closedList[child];
if (inClosedList.conflictCount > child.conflictCount)
{
closedList.Remove(inClosedList);
openList.Remove(inClosedList);
was_closed = false;
}
}
if (!was_closed)
{
this.openList.Add(child);
this.closedList.Add(child, child);
generated++;
}
}
// Prepare the node for the next partial expansion:
if (node.IsAlreadyExpanded() == false)
{
// Node was cleared during expansion.
// It's unnecessary and unsafe to continue to prepare it for the next partial expansion.
return;
}
node.targetDeltaConflictCount++; // This delta F was exhausted
node.remainingDeltaConflictCount = node.targetDeltaConflictCount;
while (node.hasMoreChildren() && node.hasChildrenForCurrentDeltaConflictCount() == false)
{
node.targetDeltaConflictCount++;
node.remainingDeltaConflictCount = node.targetDeltaConflictCount; // Just for the following hasChildrenForCurrentDeltaF call.
}
if (node.hasMoreChildren() && node.hasChildrenForCurrentDeltaConflictCount())
{
// Re-insert node into open list
openList.Add(node);
}
else
{
node.ClearExpansionData();
}
}
