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