private void UpdateBestCost
(
MAM_AgentState currentNode
)
{
Move move = new Move(currentNode.lastMove);
OGAM_Run low_level = new OGAM_Run(this.GetProblemInstance(), move);
long nodeCost = low_level.solve(this.costFunction);
//Console.WriteLine(low_level.getPlan(false));
if (nodeCost == -1)
{
throw new TimeoutException("OGAM timeout");
}
if (nodeCost < this.getBestCost())
{
if (costFunction == CostFunction.SOC)
{
this.bestSOCCost = (int)nodeCost;
}
if (costFunction == CostFunction.MakeSpan)
{
this.bestMakeSpanCost = (int)nodeCost;
}
this.bestCostLocation = currentNode.lastMove;
this.solution = low_level;
}
}
private MAM_AgentState getLowestCentralityAgent()
{
int minCentralityValue = int.MaxValue;
int minAgentIndex = 0;
for (int currentAgentIndex = 0; currentAgentIndex < this.instance.m_vAgents.Length; currentAgentIndex++)
{
int currentCentralityValue = 0;
MAM_AgentState currentAgent = this.instance.m_vAgents[currentAgentIndex];
for (int comparedAgentIndex = 0; comparedAgentIndex < this.instance.m_vAgents.Length; comparedAgentIndex++)
{
MAM_AgentState comparedAgent = this.instance.m_vAgents[comparedAgentIndex];
if (currentAgentIndex == comparedAgentIndex)
{
continue;
}
currentCentralityValue += calculateTwoAgentsManhattanDistance(currentAgent, comparedAgent);
}
if (currentCentralityValue < minCentralityValue)
{
minCentralityValue = currentCentralityValue;
minAgentIndex = currentAgentIndex;
}
}
return(this.instance.m_vAgents[minAgentIndex]);
}
private void CalculateF
(
MAM_AgentState state
)
{
if (costFunction == CostFunction.MakeSpan)
{
if (hCalculatorList[0][0].GetName() == "LP H")
{
state.f = state.g + state.h;
}
else
{
double soc = state.g + state.h;
state.f = Math.Max(state.g, soc / state.numOfAgentsInBestHeuristic);
if (state.prev != null && state.prev.f > state.f)
{
state.f = state.prev.f;
}
}
}
else if (costFunction == CostFunction.SOC)
{
state.f = state.g + state.h;
if (state.prev != null)
{
state.f = Math.Max(state.f, state.prev.f);
}
}
}
public void AddSubSetHeuristics()
{
MAM_HeuristicCalculator hCalculator = hCalculatorList[0][0];
hCalculatorList = new List <List <MAM_HeuristicCalculator> >();
foreach (MAM_AgentState agent in this.instance.m_vAgents)
{
agent.numOfAgentsInBestHeuristic = hCalculator.GetNumberOfAgents();
this.hCalculatorList.Add(new List <MAM_HeuristicCalculator>());
this.hCalculatorList[agent.agentIndex].Add(hCalculator);
if (costFunction == CostFunction.SOC || hCalculator is ZeroHCalculator)
{
continue;
}
foreach (MAM_AgentState agent2 in this.instance.m_vAgents) // set all pairs of agents (for makespan heurisic)
{
if (agent == agent2)
{
continue;
}
MAM_HeuristicCalculator newHeuristicCalculator = hCalculator.copyHeuristicCalculator();
MAM_AgentState[] agentStartStates = new MAM_AgentState[2];
agentStartStates[0] = agent;
agentStartStates[1] = agent2;
ProblemInstance subProblem = instance.CreateSubProblem(agentStartStates);
newHeuristicCalculator.init(subProblem);
this.hCalculatorList[agent.agentIndex].Add(newHeuristicCalculator);
}
}
}
private bool closed
(
MAM_AgentState child
)
{
TimedMove timedChildMove = child.lastMove;
Move childMove = new Move(timedChildMove.x, timedChildMove.y, Move.Direction.NO_DIRECTION);
if (!closedList.ContainsKey(childMove)) // Child is not in the closed list
{
closedList.Add(childMove, new Dictionary <int, Dictionary <int, MAM_AgentState> >());
Dictionary <int, MAM_AgentState> moveAgentDiffTimes = new Dictionary <int, MAM_AgentState>();
moveAgentDiffTimes.Add(timedChildMove.time, child);
closedList[childMove].Add(child.agentIndex, moveAgentDiffTimes);
return(false);
}
else if (!closedList[childMove].Keys.Contains(child.agentIndex)) // Child is not in the closed list for this agent
{
Dictionary <int, MAM_AgentState> moveAgentDiffTimes = new Dictionary <int, MAM_AgentState>();
moveAgentDiffTimes.Add(timedChildMove.time, child);
closedList[childMove].Add(child.agentIndex, moveAgentDiffTimes);
return(false);
}
else if (!closedList[childMove][child.agentIndex].Keys.Contains(timedChildMove.time)) // Child is in the closed list for this agent, but not for this time
{
Dictionary <int, MAM_AgentState> moveAgentDiffTimes = closedList[childMove][child.agentIndex];
moveAgentDiffTimes.Add(timedChildMove.time, child);
//closedList[childMove].Add(child.agentIndex, moveAgentDiffTimes);
return(false);
}
return(true);
}
public double h
(
MAM_AgentState state,
MAM_AgentState parent
)
{
if (parent == null)
{
state.h = CalculateInitialH();
}
else
{
state.h = parent.h * (instance.m_vAgents.Count() - 1);
MAM_AgentState[] startStates = instance.m_vAgents;
foreach (MAM_AgentState startState in startStates)
{
if (startState.agentIndex == state.agentIndex)
{
continue;
}
int mdParent = ManhattanDistance(parent.lastMove, startState.lastMove);
int mdChild = ManhattanDistance(state.lastMove, startState.lastMove);
state.h = state.h - mdParent + mdChild;
}
state.h = Math.Max(state.h / (instance.m_vAgents.Count() - 1), 0);
}
return(state.h);
}
/// <summary>
/// Used when AgentState objects are put in the open list priority queue - mainly in AStarForSingleAgent, I think.
/// </summary>
/// <param name="other"></param>
/// <returns></returns>
public int CompareTo
(
IBinaryHeapItem other
)
{
MAM_AgentState that = (MAM_AgentState)other;
if (this.f < that.f)
{
return(-1);
}
if (this.f > that.f)
{
return(1);
}
// Prefer larger g:
if (this.agentIndex == that.agentIndex)
{
if (this.g < that.g)
{
return(1);
}
if (this.g > that.g)
{
return(-1);
}
if (this.h > that.h)
{
return(1);
}
if (this.h < that.h)
{
return(-1);
}
}
else
{
if (this.g > that.g)
{
return(1);
}
if (this.g < that.g)
{
return(-1);
}
if (this.h < that.h)
{
return(1);
}
if (this.h > that.h)
{
return(-1);
}
}
return(0);
}
public double h
(
MAM_AgentState state,
MAM_AgentState parent
)
{
return(0);
}
/// <summary>
/// When equivalence over different times is necessary,
/// checks this.agent and last position only,
/// ignoring data that would make this state different to other equivalent states:
/// It doesn't matter from which direction the agent got to its current location.
/// It's also necessary to ignore the agents' move time - we want the same positions
/// in any time to be equivalent.
/// </summary>
/// <param name="obj"></param>
/// <returns></returns>
public override bool Equals
(
object obj
)
{
MAM_AgentState that = (MAM_AgentState)obj;
return(this.agentIndex == that.agentIndex &&
this.lastMove.x == that.lastMove.x &&
this.lastMove.y == that.lastMove.y); // Ignoring the time and the direction
}
public List <MAM_AgentState> GetChildrenStates()
{
List <MAM_AgentState> children = new List <MAM_AgentState>();
foreach (TimedMove nextMove in this.lastMove.GetNextMoves())
{
MAM_AgentState child = new MAM_AgentState(nextMove.x, nextMove.y, this.agentIndex, nextMove.time);
child.g = this.g + 1;
child.prev = this;
children.Add(child);
}
return(children);
}
private void CalculateF
(
MAM_AgentState state
)
{
if (costFunction == CostFunction.SOC)
{
state.f = state.g + state.h;
if (state.prev != null)
{
state.f = Math.Max(state.f, state.prev.f);
}
}
}
public MAM_AgentState
(
MAM_AgentState copy
)
{
this.agentIndex = copy.agentIndex;
this.lastMove = copy.lastMove;
this.g = copy.g;
this.h = copy.h;
this.f = copy.f;
this.prev = copy.prev;
this.heuristics = new List <double>(copy.heuristics);
this.numOfAgentsInBestHeuristic = copy.numOfAgentsInBestHeuristic;
}
private void CalculateH
(
MAM_AgentState state,
MAM_AgentState prev
)
{
state.h = 0;
if (hCalculatorList[0][0] is ZeroHCalculator)
{
return;
}
else if (costFunction == CostFunction.SOC)
{
state.h = hCalculatorList[0][0].h(state, prev);
}
}
private bool Expand
(
MAM_AgentState node
)
{
if (expandedNodes.Contains(node))
{
nodesExpanded--;
return(false);
}
UpdateBestCost(node);
expandedNodes.Add(node);
foreach (MAM_AgentState child in node.GetChildrenStates())
{
Generate(node, child);
}
return(true);
}
public List <List <Move> > listOfLocations; // The plan
/// <summary>
/// Reconstructs the plan by goind backwards from the goal.
/// </summary>
/// <param name="lastAgentsStates">The goal state from which to start going backwards</param>
public MAM_Plan
(
List <MAM_AgentState> lastAgentsStates
)
{
listOfLocations = new List <List <Move> >();
lastAgentsStates = lastAgentsStates.OrderBy(o => o.agentIndex).ToList();
foreach (MAM_AgentState state in lastAgentsStates)
{
List <Move> newList = new List <Move>();
MAM_AgentState lastState = state;
while (lastState != null)
{
newList.Add(lastState.lastMove);
lastState = lastState.prev;
}
newList.Reverse();
listOfLocations.Add(newList);
}
}
public virtual MAM_Plan GetPlan()
{
if (this.solution == null)
{
if (bestCostLocation == null)
{
return(null);
}
Dictionary <int, Dictionary <int, MAM_AgentState> > solutionAgentsStatesDictionaries = closedList[bestCostLocation];
Dictionary <int, MAM_AgentState> solutionAgentsStates = new Dictionary <int, MAM_AgentState>();
foreach (int agent in solutionAgentsStatesDictionaries.Keys)
{
int bestTimeForGivenAgent = solutionAgentsStatesDictionaries[agent].Keys.Min();
MAM_AgentState bestStateForGivenAgent = solutionAgentsStatesDictionaries[agent][bestTimeForGivenAgent];
solutionAgentsStates.Add(agent, bestStateForGivenAgent);
}
this.solution = new MAM_Plan(solutionAgentsStates.Values.ToList());
}
return(this.solution);
}
private double CalculateInitialH()
{
if (initialH != -1)
{
return(initialH);
}
int sumOfDistances = 0;
MAM_AgentState[] startStates = instance.m_vAgents;
for (int agentIndex1 = 0; agentIndex1 < startStates.Length; agentIndex1++)
{
for (int agentIndex2 = agentIndex1 + 1; agentIndex2 < startStates.Length; agentIndex2++)
{
MAM_AgentState agent1 = startStates[agentIndex1];
MAM_AgentState agent2 = startStates[agentIndex2];
sumOfDistances += ManhattanDistance(agent1.lastMove, agent2.lastMove);
}
}
initialH = (double)sumOfDistances / (double)(startStates.Length - 1);
return(initialH);
}
/// <summary>
///
/// </summary>
/// <param name="problemInstance"></param>
/// <param name="minDepth"></param>
/// <param name="runner"></param>
/// <param name="minCost">Not taken into account</param>
public virtual void Setup
(
ProblemInstance problemInstance,
int minDepth,
CFMAM_Run runner,
int minCost = -1,
HashSet <MMStarConstraint> constraints = null
)
{
this.instance = problemInstance;
this.runner = runner;
this.ClearPrivateStatistics();
this.totalCost = 0;
this.solutionDepth = -1;
this.milliCap = int.MaxValue;
this.goalLocation = null;
this.solution = null;
this.bestMakeSpanCost = int.MaxValue;
this.bestSOCCost = int.MaxValue;
this.bestCostLocation = null;
this.meetFlag = false;
this.success = false;
this.openList = new CFMAM_OpenList(this);
if (constraints != null)
{
this.constraints = constraints;
}
else
{
this.constraints = new HashSet <MMStarConstraint>();
}
// caculate lowest centrality
MAM_AgentState agent = getLowestCentralityAgent();
CalculateH(agent, null);
CalculateF(agent);
closed(agent);
openList.Add(agent);
}
private void Generate
(
MAM_AgentState node,
MAM_AgentState child
)
{
if (expandedNodes.Contains(child))
{
return;
}
Move childMove = child.lastMove;
if (!instance.IsValid(childMove)) // Not a valid location
{
return;
}
if (!closed(child))
{
if (child.f >= getBestCost())
{
return;
}
child.numOfAgentsInBestHeuristic = instance.m_vAgents.Length;
if (openList.Contains(child))
{
child.h = ((MAM_AgentState)openList.Get(child)).h;
child.heuristics = ((MAM_AgentState)openList.Get(child)).heuristics;
}
else
{
CalculateH(child, node);
}
CalculateF(child);
openList.Add(child);
UpdateBestCost(child);
nodesGenerated++;
}
}
private void UpdateBestCost
(
MAM_AgentState currentNode
)
{
int makeSpanCost = 0;
int SOCCost = 0;
//Move move = (Move)currentNode.lastMove;
Move move = new Move(currentNode.lastMove);
if (closedList[move].Count != instance.m_vAgents.Length)
{
return;
}
meetFlag = true;
for (int agentIndex = 0; agentIndex < instance.m_vAgents.Length; agentIndex++)
{
int currentStateTime = closedList[move][agentIndex].Keys.Min();
makeSpanCost = Math.Max(makeSpanCost, currentStateTime);
SOCCost += currentStateTime;
}
int cost = 0;
if (costFunction == CostFunction.MakeSpan)
{
cost = makeSpanCost;
}
else if (costFunction == CostFunction.SOC)
{
cost = SOCCost;
}
if (cost < getBestCost())
{
bestMakeSpanCost = makeSpanCost;
bestSOCCost = SOCCost;
bestCostLocation = move;
}
}
private void CalculateH
(
MAM_AgentState state,
MAM_AgentState prev
)
{
state.h = 0;
if (hCalculatorList[0][0] is ZeroHCalculator)
{
return;
}
if (costFunction == CostFunction.MakeSpan)
{
for (int heuristicCalculatorIndex = 0; heuristicCalculatorIndex < hCalculatorList[state.agentIndex].Count; heuristicCalculatorIndex++)
{
MAM_HeuristicCalculator heuristicCalculator = hCalculatorList[state.agentIndex][heuristicCalculatorIndex];
MAM_AgentState newState = new MAM_AgentState(state);
MAM_AgentState newPrevState = null;
if (prev != null)
{
newPrevState = new MAM_AgentState(prev);
newPrevState.h = prev.heuristics[heuristicCalculatorIndex];
}
state.heuristics.Add(heuristicCalculator.h(newState, newPrevState));
double makespan1 = (state.h + state.g) / state.numOfAgentsInBestHeuristic;
double makespan2 = (state.heuristics[heuristicCalculatorIndex] + state.g) / heuristicCalculator.GetNumberOfAgents();
if (makespan1 < makespan2)
{
state.h = state.heuristics[heuristicCalculatorIndex];
state.numOfAgentsInBestHeuristic = heuristicCalculator.GetNumberOfAgents();
}
}
}
else if (costFunction == CostFunction.SOC)
{
state.h = hCalculatorList[0][0].h(state, prev);
}
}
private bool Expand
(
MAM_AgentState node
)
{
MMStarConstraint queryConstraint = new MMStarConstraint(node.agentIndex, node.lastMove.x, node.lastMove.y, node.lastMove.direction, node.lastMove.time);
if (constraints.Contains(queryConstraint))
{
return(false);
}
if (expandedNodes.Contains(node))
{
nodesExpanded--;
return(false);
}
expandedNodes.Add(node);
foreach (MAM_AgentState child in node.GetChildrenStates())
{
Generate(node, child);
}
return(true);
}
private int solveWithCFMCBS(ProblemInstance instance)
{
int costConsistency = -1;
MAM_AgentState[] vAgents = new MAM_AgentState[instance.GetNumOfAgents()];
for (int agentIndex = 0; agentIndex < instance.GetNumOfAgents(); agentIndex++)
{
vAgents[agentIndex] = new MAM_AgentState(instance.m_vAgents[agentIndex]);
}
for (int i = 0; i < CFMCBSSolvers.Count; i++)
{
solutionCost = -1;
if (outOfTimeCounters[i] < Constants.MAX_FAIL_COUNT)
{
GC.Collect();
GC.WaitForPendingFinalizers();
this.runCFMCBS(CFMCBSSolvers[i], instance);
MAM_AgentState[] vAgents2 = new MAM_AgentState[vAgents.Count()];
for (int agentIndex = 0; agentIndex < vAgents.Count(); agentIndex++)
{
vAgents2[agentIndex] = new MAM_AgentState(vAgents[agentIndex]);
}
instance.m_vAgents = vAgents2;
solutionCost = CFMCBSSolvers[i].GetSolutionCost();
if (costConsistency == -1)
{
costConsistency = solutionCost;
}
else if (solutionCost != costConsistency)
{
throw new Exception("Inconsist Cost");
}
String plan = null;
if (CFMCBSSolvers[i].isSolved()) // Solved successfully
{
plan = CFMCBSSolvers[i].GetPlan();
if (toPrint)
{
Console.WriteLine();
Console.WriteLine(plan);
Console.WriteLine();
}
outOfTimeCounters[i] = 0;
Console.WriteLine("+SUCCESS+ (:");
}
else
{
outOfTimeCounters[i]++;
Console.WriteLine("-FAILURE- ):");
}
planningTime = elapsedTime;
WriteGivenCFMCBSProblem(instance, CFMCBSSolvers[i], plan);
}
else if (toPrint)
{
PrintNullStatistics(IMSSolvers[i]);
}
Console.WriteLine();
}
return(costConsistency);
}
public int solveWithIMS(ProblemInstance instance)
{
int costConsistency = -1;
MAM_AgentState[] vAgents = new MAM_AgentState[instance.GetNumOfAgents()];
for (int agentIndex = 0; agentIndex < instance.GetNumOfAgents(); agentIndex++)
{
vAgents[agentIndex] = new MAM_AgentState(instance.m_vAgents[agentIndex]);
}
for (int i = 0; i < IMSSolvers.Count; i++)
{
solutionCost = -1;
if (outOfTimeCounters[i] < Constants.MAX_FAIL_COUNT)
{
GC.Collect();
GC.WaitForPendingFinalizers();
preprocessingTime = 0;
if (IMSSolvers[i].GetHeuristicCalculator().GetName() == "FastMap H")
{
this.startTime = this.ElapsedMillisecondsTotal();
IMSSolvers[i].GetHeuristicCalculator().init(instance);
IMSSolvers[i].GetHeuristicCalculator().preprocessing();
preprocessingTime = this.ElapsedMilliseconds();
Console.WriteLine("Preprocessing time in milliseconds: {0}", preprocessingTime);
}
this.runIMS(IMSSolvers[i], instance);
MAM_AgentState[] vAgents2 = new MAM_AgentState[vAgents.Count()];
for (int agentIndex = 0; agentIndex < vAgents.Count(); agentIndex++)
{
vAgents2[agentIndex] = new MAM_AgentState(vAgents[agentIndex]);
}
instance.m_vAgents = vAgents2;
solutionCost = IMSSolvers[i].GetSolutionCost();
if (costConsistency == -1)
{
costConsistency = solutionCost;
}
else if (solutionCost != costConsistency)
{
throw new Exception("Inconsist Cost");
}
String plan = null;
if (IMSSolvers[i].IsSolved()) // Solved successfully
{
plan = IMSSolvers[i].GetPlan();
if (toPrint)
{
Console.WriteLine();
Console.WriteLine(plan);
Console.WriteLine();
}
outOfTimeCounters[i] = 0;
Console.WriteLine("+SUCCESS+ (:");
}
else
{
outOfTimeCounters[i]++;
Console.WriteLine("-FAILURE- ):");
}
planningTime = elapsedTime;
WriteGivenIMSProblem(instance, IMSSolvers[i], plan);
}
else if (toPrint)
{
PrintNullStatistics(IMSSolvers[i]);
}
Console.WriteLine();
}
return(costConsistency);
}
/// <summary>
/// Generates a problem instance, including a board, start and goal locations of desired number of agents
/// and desired precentage of obstacles
/// TODO: Refactor to use operators.
/// </summary>
/// <param name="gridSize"></param>
/// <param name="agentsNum"></param>
/// <param name="obstaclesNum"></param>
/// <returns></returns>
public ProblemInstance GenerateProblemInstance
(
int gridSize,
int agentsNum,
int obstaclesNum
)
{
m_mapFileName = "GRID" + gridSize + "X" + gridSize;
m_agentNum = agentsNum;
if (agentsNum + obstaclesNum > gridSize * gridSize)
{
throw new Exception("Not enough room for " + agentsNum + ", " + obstaclesNum + " and one empty space in a " + gridSize + "x" + gridSize + "map.");
}
int x;
int y;
MAM_AgentState[] aStart = new MAM_AgentState[agentsNum];
bool[][] grid = new bool[gridSize][];
bool[][] starts = new bool[gridSize][];
// Generate a random grid
for (int i = 0; i < gridSize; i++)
{
grid[i] = new bool[gridSize];
starts[i] = new bool[gridSize];
}
for (int i = 0; i < obstaclesNum; i++)
{
x = rand.Next(gridSize);
y = rand.Next(gridSize);
if (grid[x][y]) // Already an obstacle
{
i--;
}
grid[x][y] = true;
}
// Choose random start locations
for (int i = 0; i < agentsNum; i++)
{
x = rand.Next(gridSize);
y = rand.Next(gridSize);
if (starts[x][y] || grid[x][y])
{
i--;
}
else
{
starts[x][y] = true;
aStart[i] = new MAM_AgentState(x, y, i, 0);
}
}
ProblemInstance problem = new ProblemInstance();
problem = new ProblemInstance();
problem.Init(aStart, grid);
return(problem);
}
/// <summary>
/// Generates a problem instance based on a DAO map file.
/// TODO: Fix code dup with GenerateProblemInstance and Import later.
/// </summary>
/// <param name="agentsNum"></param>
/// <returns></returns>
public ProblemInstance GenerateDragonAgeProblemInstance
(
string mapFileName,
int agentsNum
)
{
m_mapFileName = mapFileName;
m_agentNum = agentsNum;
TextReader input = new StreamReader(mapFileName);
string[] lineParts;
string line;
line = input.ReadLine();
Debug.Assert(line.StartsWith("type octile"));
// Read grid dimensions
line = input.ReadLine();
lineParts = line.Split(' ');
Debug.Assert(lineParts[0].StartsWith("height"));
int maxX = int.Parse(lineParts[1]);
line = input.ReadLine();
lineParts = line.Split(' ');
Debug.Assert(lineParts[0].StartsWith("width"));
int maxY = int.Parse(lineParts[1]);
line = input.ReadLine();
Debug.Assert(line.StartsWith("map"));
bool[][] grid = new bool[maxX][];
char cell;
for (int i = 0; i < maxX; i++)
{
grid[i] = new bool[maxY];
line = input.ReadLine();
for (int j = 0; j < maxY; j++)
{
cell = line.ElementAt(j);
if (cell == '@' || cell == 'O' || cell == 'T' || cell == 'W' /* Water isn't traversable from land */)
{
grid[i][j] = true;
}
else
{
grid[i][j] = false;
}
}
}
int x;
int y;
MAM_AgentState[] aStart = new MAM_AgentState[agentsNum];
bool[][] starts = new bool[maxX][];
for (int i = 0; i < maxX; i++)
{
starts[i] = new bool[maxY];
}
// Choose random valid unclaimed goal locations
for (int i = 0; i < agentsNum; i++)
{
x = rand.Next(maxX);
y = rand.Next(maxY);
if (starts[x][y] || grid[x][y])
{
i--;
}
else
{
starts[x][y] = true;
aStart[i] = new MAM_AgentState(x, y, i, 0);
}
}
ProblemInstance problem = new ProblemInstance();
problem.parameters[ProblemInstance.GRID_NAME_KEY] = Path.GetFileNameWithoutExtension(mapFileName);
problem.Init(aStart, grid);
return(problem);
}
/// <summary>
/// Solve given instance with a list of algorithms
/// </summary>
/// <param name="instance">The instance to solve</param>
public bool SolveGivenProblem
(
ProblemInstance instance,
HashSet <MMStarConstraint> constraints = null
)
{
instanceId += 1;
bool success = true;
List <uint> agentList = Enumerable.Range(0, instance.m_vAgents.Length).Select <int, uint>(x => (uint)x).ToList <uint>(); // FIXME: Must the heuristics really receive a list of uints?
// Solve using the different algorithms
//Debug.WriteLine("Solving " + instance);
MAM_AgentState[] vAgents = new MAM_AgentState[instance.GetNumOfAgents()];
for (int agentIndex = 0; agentIndex < instance.GetNumOfAgents(); agentIndex++)
{
vAgents[agentIndex] = new MAM_AgentState(instance.m_vAgents[agentIndex]);
}
for (int i = 0; i < solvers.Count; i++)
{
solutionCost = -1;
if (outOfTimeCounters[i] < Constants.MAX_FAIL_COUNT)
{
GC.Collect();
GC.WaitForPendingFinalizers();
preprocessingTime = 0;
if (solvers[i].GetHeuristicCalculator().GetName() == "FastMap H")
{
this.startTime = this.ElapsedMillisecondsTotal();
solvers[i].GetHeuristicCalculator().init(instance);
solvers[i].GetHeuristicCalculator().preprocessing();
preprocessingTime = this.ElapsedMilliseconds();
Console.WriteLine("Preprocessing time in milliseconds: {0}", preprocessingTime);
}
this.run(solvers[i], instance, constraints);
MAM_AgentState[] vAgents2 = new MAM_AgentState[vAgents.Count()];
for (int agentIndex = 0; agentIndex < vAgents.Count(); agentIndex++)
{
vAgents2[agentIndex] = new MAM_AgentState(vAgents[agentIndex]);
}
instance.m_vAgents = vAgents2;
if (solvers[i].GetCostFunction() == CostFunction.MakeSpan)
{
solutionCost = solvers[i].GetSolutionMakeSpanCost();
}
else if (solvers[i].GetCostFunction() == CostFunction.SOC)
{
solutionCost = solvers[i].GetSolutionSOCCost();
}
MAM_Plan plan = null;
if (solvers[i].IsSolved()) // Solved successfully
{
plan = solvers[i].GetPlan();
if (toPrint)
{
Console.WriteLine();
plan.ToString();
Console.WriteLine();
}
outOfTimeCounters[i] = 0;
//Console.WriteLine("+SUCCESS+ (:");
this.plan = plan;
}
else
{
outOfTimeCounters[i]++;
Console.WriteLine("-FAILURE- ):");
}
planningTime = elapsedTime;
}
else if (toPrint)
{
PrintNullStatistics(solvers[i]);
}
}
return(true);
}
/// <summary>
/// Imports a problem instance from a given file
/// </summary>
/// <param name="fileName"></param>
/// <returns></returns>
public static ProblemInstance Import
(
string fileName
)
{
TextReader input = new StreamReader(fileName);
string[] lineParts;
string line;
int instanceId = 0;
string gridName = "Random Grid"; // The default
line = input.ReadLine();
if (line.StartsWith("Grid:") == false)
{
lineParts = line.Split(',');
instanceId = int.Parse(lineParts[0]);
if (lineParts.Length > 1)
{
gridName = lineParts[1];
}
line = input.ReadLine();
}
Debug.Assert(line.StartsWith("Grid:"));
// Read grid dimensions
line = input.ReadLine();
lineParts = line.Split(',');
int maxX = int.Parse(lineParts[0]);
int maxY = int.Parse(lineParts[1]);
bool[][] grid = new bool[maxX][];
char cell;
for (int i = 0; i < maxX; i++)
{
grid[i] = new bool[maxY];
line = input.ReadLine();
for (int j = 0; j < maxY; j++)
{
cell = line.ElementAt(j);
if (cell == '@' || cell == 'O' || cell == 'T' || cell == 'W' /* Water isn't traversable from land */)
{
grid[i][j] = true;
}
else
{
grid[i][j] = false;
}
}
}
// Next line is Agents:
line = input.ReadLine();
Debug.Assert(line.StartsWith("Agents:"));
// Read the number of agents
line = input.ReadLine();
int numOfAgents = int.Parse(line);
// Read the agents' start and goal states
MAM_AgentState[] states = new MAM_AgentState[numOfAgents];
MAM_AgentState state;
int agentNum;
int startX;
int startY;
for (int agentIndex = 0; agentIndex < numOfAgents; agentIndex++)
{
line = input.ReadLine();
lineParts = line.Split(EXPORT_DELIMITER);
agentNum = int.Parse(lineParts[0]);
startX = int.Parse(lineParts[1]);
startY = int.Parse(lineParts[2]);
state = new MAM_AgentState(startX, startY, agentIndex, 0);
states[agentIndex] = state;
}
// Generate the problem instance
ProblemInstance instance = new ProblemInstance();
instance.fileName = fileName;
instance.instanceId = instanceId;
instance.Init(states, grid);
instance.parameters[ProblemInstance.GRID_NAME_KEY] = gridName;
return(instance);
}
private int calculateTwoAgentsManhattanDistance(MAM_AgentState currentAgent, MAM_AgentState comparedAgent)
{
return(Math.Abs(currentAgent.lastMove.x - comparedAgent.lastMove.x) +
Math.Abs(currentAgent.lastMove.y - comparedAgent.lastMove.y));
}
public double h
(
MAM_AgentState state,
MAM_AgentState parent
)
{
int dim = 2;
int[] curr = { state.lastMove.x, state.lastMove.y };
if (parent == null)
{
state.h = CalculateInitialH();
state.hToMeeting = 0;
for (int i = 0; i < dim; i++)
{
state.hToMeeting += Math.Abs(curr[i] - medians[i][1]);
}
return(state.h);
}
MAM_AgentState[] startStates = instance.m_vAgents;
state.h = 0;
state.hToMeeting = 0;
int[] start = { startStates[instance.GetAgentIndexInArray(state.agentIndex)].lastMove.x, startStates[instance.GetAgentIndexInArray(state.agentIndex)].lastMove.y };
for (int i = 0; i < dim; i++)
{
if (medians[i].Length == 2) // even number of agents
{
if (medians[i][0] < start[i] && medians[i][0] < curr[i])
{
state.h += h0[i] - start[i] + curr[i];
state.hToMeeting += curr[i] - medians[i][0];
}
else if (start[i] < medians[i][1] && curr[i] < medians[i][1])
{
state.h += h0[i] + start[i] - curr[i];
state.hToMeeting += medians[i][1] - curr[i];
}
else if (start[i] <= medians[i][0] && medians[i][1] <= curr[i])
{
state.h += h0[i] + start[i] + curr[i] - 2 * medians[i][1];
state.hToMeeting += curr[i] - medians[i][1];
}
else // i.e., curr[i] <= medians[i][0] && medians[i][1] <= start[i]
{
state.h += h0[i] - start[i] - curr[i] + 2 * medians[i][0];
state.hToMeeting += medians[i][0] - curr[i];
}
}
else // odd number of agents
{
if (medians[i][1] < start[i] && medians[i][1] < curr[i])
{
state.h += h0[i] - start[i] + curr[i];
state.hToMeeting += curr[i] - medians[i][1];
}
else if (start[i] < medians[i][1] && curr[i] < medians[i][1])
{
state.h += h0[i] + start[i] - curr[i];
state.hToMeeting += medians[i][1] - curr[i];
}
else if (start[i] <= medians[i][1] && medians[i][1] <= curr[i])
{
state.h += h0[i] + start[i] - medians[i][1] + Math.Max(curr[i] - medians[i][2], 0);
state.hToMeeting += Math.Max(curr[i] - medians[i][2], 0);
}
else // i.e., curr[i] <= medians[i][1] && medians[i][1] <= start[i]
{
state.h += h0[i] - start[i] + medians[i][1] + Math.Max(medians[i][0] - curr[i], 0);
state.hToMeeting += Math.Max(medians[i][0] - curr[i], 0);
}
}
}
// for debug
/*AgentState temp = startStates[state.agentIndex];
* startStates[state.agentIndex] = state;
* double debug = CalculateH(startStates);
* if (debug != state.h)
* Console.WriteLine("ERROR!!!");
* startStates[state.agentIndex] = temp;*/
return(state.h);
}