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