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(); } }