private bool singleAgentAStar(AgentState agent)
{
AgentState.EquivalenceOverDifferentTimes = false;
BinaryHeap <AgentState> openList = new BinaryHeap <AgentState>(); // TODO: Safe to use OpenList here instead?
HashSet <AgentState> closedList = new HashSet <AgentState>();
agent.h = this.problem.GetSingleAgentOptimalCost(agent);
openList.Add(agent);
AgentState node;
this.initialEstimate += agent.h;
TimedMove queryTimedMove = new TimedMove();
while (openList.Count > 0)
{
if (this.runner.ElapsedMilliseconds() > Constants.MAX_TIME)
{
return(false);
}
node = openList.Remove();
if (node.h == 0)
{
bool valid = true;
for (int i = node.lastMove.time; i <= maxPathCostSoFar; i++)
{
queryTimedMove.setup(node.lastMove.x, node.lastMove.y, Move.Direction.NO_DIRECTION, i);
if (reservationTable.Contains(queryTimedMove))
{
valid = false;
}
}
if (valid)
{
this.paths[agent.agent.agentNum] = new SinglePlan(node);
reservePath(node);
totalcost += node.lastMove.time;
parked.Add(new Move(node.lastMove.x, node.lastMove.y, Move.Direction.NO_DIRECTION), node.lastMove.time);
return(true);
}
}
expandNode(node, openList, closedList);
expanded++;
}
return(false);
}
private void expandNode(AgentState node, BinaryHeap <AgentState> openList, HashSet <AgentState> closedList)
{
foreach (TimedMove move in node.lastMove.GetNextMoves())
{
if (this.isValidMove(move))
{
AgentState child = new AgentState(node);
child.prev = node;
child.MoveTo(move);
if (closedList.Contains(child) == false)
{
closedList.Add(child);
child.h = this.problem.GetSingleAgentOptimalCost(child);
openList.Add(child);
generated++;
}
}
}
}
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;
}
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();
}
}