private void singleAgentShortestPath(int agentId, out int[] shortestPathLengths, out Move[] optimalMoves, out AgentState agentStartState, out int start) { // Run a single source shortest path algorithm from the _goal_ of the agent shortestPathLengths = new int[this.numLocations]; optimalMoves = new Move[this.numLocations]; for (int i = 0; i < numLocations; i++) { shortestPathLengths[i] = -1; } var openlist = new Queue <AgentState>(); // Create initial state agentStartState = this.agents[agentId]; var agent = agentStartState.agent; var goalState = new AgentState(agent.Goal.x, agent.Goal.y, -1, -1, agentId); int goalIndex = this.GetCardinality(goalState.lastMove); shortestPathLengths[goalIndex] = 0; optimalMoves[goalIndex] = new Move(goalState.lastMove); openlist.Enqueue(goalState); while (openlist.Count > 0) { AgentState state = openlist.Dequeue(); // Generate child states foreach (TimedMove aMove in state.lastMove.GetNextMoves()) { if (IsValid(aMove)) { int entry = cardinality[aMove.x, aMove.y]; // If move will generate a new or better state - add it to the queue if ((shortestPathLengths[entry] == -1) || (shortestPathLengths[entry] > state.g + 1)) { var childState = new AgentState(state); childState.MoveTo(aMove); shortestPathLengths[entry] = childState.g; optimalMoves[entry] = new Move(aMove.GetOppositeMove()); openlist.Enqueue(childState); } } } } start = this.GetCardinality(agentStartState.lastMove); if (shortestPathLengths[start] == -1) { throw new Exception($"Unsolvable instance! Agent {agentId} cannot reach its goal"); // Note instances can still be unsolvable if this isn't reached. E.g. this corridor: // s1-g2-g1-s2 } }
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++; } } } }
/// <summary> /// Computes the shortest path to the goal for a given agent from every location in the grid. /// Current implementation is a simple breadth-first search from every location in the graph. /// </summary> /// <param name="state">Agent's goal state</param> /// <returns>Tuple with shortestPathLengths and optimalMoves </returns> public Tuple <int[], Move[]> AllShortestPathsTo(AgentState state) { var openlist = new Queue <AgentState>(); var shortestPathLengths = new int[this.numLocations]; var optimalMoves = new Move[this.numLocations]; for (int i = 0; i < numLocations; i++) { shortestPathLengths[i] = -1; } openlist.Enqueue(state); int goalIndex = this.GetCardinality(state.lastMove); shortestPathLengths[goalIndex] = 0; optimalMoves[goalIndex] = new Move(state.lastMove); while (openlist.Count > 0) { AgentState nextState = openlist.Dequeue(); // Generate child states foreach (TimedMove aMove in nextState.lastMove.GetNextMoves()) { if (IsValid(aMove)) { int entry = cardinality[aMove.x, aMove.y]; // If move will generate a new or better state - add it to the queue if ((shortestPathLengths[entry] == -1) || (shortestPathLengths[entry] > nextState.g + 1)) { var childState = new AgentState(nextState); childState.MoveTo(aMove); shortestPathLengths[entry] = childState.g; optimalMoves[entry] = new Move(aMove.GetOppositeMove()); openlist.Enqueue(childState); } } } } return(Tuple.Create <int[], Move[]>(shortestPathLengths, optimalMoves)); }
/// <summary> /// Compute the shortest path to the goal of every agent in the problem instance, from every location in the grid. /// Current implementation is a simple breadth-first search from every location in the graph. /// </summary> public void ComputeSingleAgentShortestPaths() { Debug.WriteLine("Computing the single agent shortest path for all agents..."); Stopwatch watch = Stopwatch.StartNew(); double startTime = watch.Elapsed.TotalMilliseconds; //return; // Add for generator this.singleAgentOptimalCosts = new int[this.GetNumOfAgents()][]; this.singleAgentOptimalMoves = new Move[this.GetNumOfAgents()][]; for (int agentId = 0; agentId < this.GetNumOfAgents(); agentId++) { // Run a single source shortest path algorithm from the _goal_ of the agent var shortestPathLengths = new int[this.numLocations]; var optimalMoves = new Move[this.numLocations]; for (int i = 0; i < numLocations; i++) { shortestPathLengths[i] = -1; } var openlist = new Queue <AgentState>(); // Create initial state var agentStartState = this.agents[agentId]; var agent = agentStartState.agent; var goalState = new AgentState(agent.Goal.x, agent.Goal.y, -1, -1, agentId); int goalIndex = this.GetCardinality(goalState.lastMove); shortestPathLengths[goalIndex] = 0; optimalMoves[goalIndex] = new Move(goalState.lastMove); openlist.Enqueue(goalState); while (openlist.Count > 0) { AgentState state = openlist.Dequeue(); // Generate child states foreach (TimedMove aMove in state.lastMove.GetNextMoves()) { if (IsValid(aMove)) { int entry = cardinality[aMove.x, aMove.y]; // If move will generate a new or better state - add it to the queue if ((shortestPathLengths[entry] == -1) || (shortestPathLengths[entry] > state.g + 1)) { var childState = new AgentState(state); childState.MoveTo(aMove); shortestPathLengths[entry] = childState.g; optimalMoves[entry] = new Move(aMove.GetOppositeMove()); openlist.Enqueue(childState); } } } } int start = this.GetCardinality(agentStartState.lastMove); if (shortestPathLengths[start] == -1) { throw new Exception($"Unsolvable instance! Agent {agentId} cannot reach its goal"); // Note instances can still be unsolvable if this isn't reached. E.g. this corridor: // s1-g2-g1-s2 } this.singleAgentOptimalCosts[agentId] = shortestPathLengths; this.singleAgentOptimalMoves[agentId] = optimalMoves; } double endTime = watch.Elapsed.TotalMilliseconds; this.shortestPathComputeTime = endTime - startTime; }