static void Main(string[] args) { bool isDebug = true; if (isDebug == false) { List <WorldState> initialStates = ServerCommunicator.readLevel(); List <(WorldState, List <(EntityLocation, int[, ], float priority)>)> hMatrix = BFSHeruistic.PrecalcH(initialStates); var testCBS = new CBS(hMatrix); var testSolution = testCBS.findSolution(); List <string> lines = ServerCommunicator.printSolutionToFile(testSolution); int counter = 0; foreach (string line in lines) { Console.WriteLine(line); string result = Console.ReadLine(); counter++; if (!result.Contains("false")) { continue; } else { throw new Exception(line + result + "counter = " + counter); } } } else { var initialStates = ServerCommunicator.readLevelFromFile(); List <(WorldState, List <(EntityLocation, int[, ], float priority)>)> hMatrix = BFSHeruistic.PrecalcH(initialStates); var testCBS = new CBS(hMatrix); var testSolution = testCBS.findSolution(); throw new Exception("at end"); } /* * Box box1 = new Box('a', "red"); * Box box2 = new Box('a', "red"); * * Location location1 = new Location(3, 3); * Location location2 = new Location(3, 3); * * var hash1 = HashCode.Combine(box1, location1); * var hash2 = HashCode.Combine(box2, location2); */ //var test = ServerCommunicator.readLevel(); /// SINGLE AGENT TESTING// /* * Console.WriteLine("TESTAI"); * var initialStates = ServerCommunicator.readLevelFromFile(); * if (initialStates.Count > 1) throw new Exception(); * * var watch1 = System.Diagnostics.Stopwatch.StartNew(); * //List<WorldState> plan1 = initialStates[0].agent.SearchClient.MakePlan(initialStates[0], new HashSet<Constraint>()); * watch1.Stop(); * var elapsedMs1 = watch1.ElapsedMilliseconds; * * var watch2 = System.Diagnostics.Stopwatch.StartNew(); * List<WorldState> plan2 = Astar.MakePlan(initialStates[0].agent, initialStates[0], new HashSet<Constraint>()); * watch2.Stop(); * var elapsedMs2 = watch2.ElapsedMilliseconds; * * List<List<WorldState>> solution = new List<List<WorldState>>(); * solution.Add(plan2); * List<string> lines = ServerCommunicator.printSolutionToFile(solution); * * foreach (string line in lines) * { * Console.WriteLine(line); * string result = Console.ReadLine(); * * if (!result.Contains("false")) continue; * else throw new Exception(line + result); * } * * throw new Exception("at end"); * */ ////////////////////// /* * bool runDebug = true; * if (runDebug) * { * var test = ServerCommunicator.readLevelFromFile(); * * var testCBS = new CBS(test); * * var testSolution = testCBS.findSolution(); * List<string> lines = ServerCommunicator.printSolutionToFile(testSolution); * * * throw new Exception("We are at the end"); * } * else * { * var test = ServerCommunicator.readLevel(); * var testCBS = new CBS(test); * var testSolution = testCBS.findSolution(); * List<string> lines = ServerCommunicator.printSolutionToFile(testSolution); * * foreach (string line in lines) * { * Console.WriteLine(line); * string result = Console.ReadLine(); * * if (!result.Contains("false")) continue; * else throw new Exception(line + result); * } * }*/ }
private static List <WorldState> AssignBoxesToAgents(List <EntityLocation> agents, List <EntityLocation> boxes, List <EntityLocation> agentGoals, List <EntityLocation> boxGoals, HashSet <Location> walls) { List <WorldState> initialStates = new List <WorldState>(); List <Assignment> boxAssignments = new List <Assignment>(); List <Assignment> goalAssignments = new List <Assignment>(); bool isAgentGoal = false; List <WorldState> completeInitStates = new List <WorldState>(); foreach (EntityLocation agentEntity in agents) { WorldState completeInitState = new WorldState(); completeInitState.agent = (Agent)agentEntity.Entity; completeInitState.agentLocation = agentEntity.Location; completeInitState.Walls = walls; completeInitState.assignedBoxes = new Dictionary <Location, Box>(); completeInitState.boxGoals = new List <EntityLocation>(); completeInitState.boxGoals.Add(new EntityLocation(completeInitState.agent, completeInitState.agentLocation)); completeInitStates.Add(completeInitState); } List <(WorldState, List <(EntityLocation, int[, ], float priority)>)> completeInitStatesWithH = BFSHeruistic.PrecalcH(completeInitStates); while (boxGoals.Count > 0) { foreach (EntityLocation agentEntity in agents) { double shortestDistance = double.PositiveInfinity; EntityLocation bestBoxGoalLocation = null; EntityLocation bestInitialBoxGoalLocation = null; Agent agent = (Agent)agentEntity.Entity; EntityLocation agentGoal = null; if (agentGoals.Count > 0) { isAgentGoal = agentGoals.Any(a => a.Entity.Name.Equals(agentEntity.Entity.Name)); if (isAgentGoal) { agentGoal = agentGoals.First(a => a.Entity.Name.Equals(agentEntity.Entity.Name)); } } foreach (EntityLocation boxGoalLocation in boxGoals.Where(b => b.Entity.Colour.Equals(agent.Colour))) { // Assign a goal and a box foreach (EntityLocation boxInitialLocation in boxes.Where(b => b.Entity.Name.Equals(boxGoalLocation.Entity.Name))) { double total_dist = 0.0; total_dist += completeInitStatesWithH.First(c => c.Item1.agent.Name.Equals(agentEntity.Entity.Name)).Item2.First() .Item2[boxInitialLocation.Location.x, boxInitialLocation.Location.y]; //agentEntity.ManhattanDistance(boxInitialLocation); total_dist += completeInitStatesWithH.First(c => c.Item1.agent.Name.Equals(agentEntity.Entity.Name)).Item2.First() .Item2[boxGoalLocation.Location.x, boxGoalLocation.Location.y]; //boxInitialLocation.ManhattanDistance(boxGoalLocation); if (isAgentGoal) { total_dist += completeInitStatesWithH.First(c => c.Item1.agent.Name.Equals(agentEntity.Entity.Name)).Item2.First() .Item2[agentGoal.Location.x, agentGoal.Location.y]; //boxGoalLocation.ManhattanDistance(agentGoal); } if (total_dist < shortestDistance) { shortestDistance = total_dist; bestBoxGoalLocation = boxGoalLocation; bestInitialBoxGoalLocation = boxInitialLocation; } } } if (bestBoxGoalLocation != null && bestInitialBoxGoalLocation != null && shortestDistance < int.MaxValue) { boxAssignments.Add(new Assignment(agent, bestInitialBoxGoalLocation)); goalAssignments.Add(new Assignment(agent, bestBoxGoalLocation)); EntityLocation boxToRemove = null; EntityLocation boxGoalToRemove = null; foreach (EntityLocation b in boxes) { if (b.Location.Equals(bestInitialBoxGoalLocation.Location)) { boxToRemove = b; } } foreach (EntityLocation b in boxGoals) { if (b.Location.Equals(bestBoxGoalLocation.Location)) { boxGoalToRemove = b; } } boxes.Remove(boxToRemove); boxGoals.Remove(boxGoalToRemove); } } } foreach (EntityLocation b in boxes) { int minBoxCount = int.MaxValue; Agent minAgent = null; foreach (EntityLocation agent in agents.Where(ass => ass.Entity.Colour.Equals(b.Entity.Colour))) { int thisAgentCount = 0; foreach (Assignment a in boxAssignments.Where(ass => ass.Agent.Colour.Equals(b.Entity.Colour))) { if (a.Agent.Equals(agent.Entity)) { thisAgentCount++; } } if (thisAgentCount < minBoxCount) { minBoxCount = thisAgentCount; minAgent = (Agent)agent.Entity; } } if (minAgent == null) { throw new Exception(""); } boxAssignments.Add(new Assignment(minAgent, b)); } foreach (EntityLocation agentEntity in agents) { Location agentLocation = agentEntity.Location; Location agentGoal; List <EntityLocation> test = agentGoals.Where(a => a.Entity.Name.Equals(agentEntity.Entity.Name)).ToList(); if (test.Count > 0) { agentGoal = test.ElementAt(0).Location; } else { agentGoal = null; } Dictionary <Location, Box> agentBoxes = new Dictionary <Location, Box>(); List <EntityLocation> boxGoalsforThisAgent = new List <EntityLocation>(); foreach (Assignment boxAssignment in boxAssignments) { if (boxAssignment.Agent.Name.Equals(agentEntity.Entity.Name)) { agentBoxes.Add(boxAssignment.EntityLocation.Location, (Box)boxAssignment.EntityLocation.Entity); } } foreach (Assignment goalAssignment in goalAssignments) { if (goalAssignment.Agent.Name.Equals(agentEntity.Entity.Name)) { boxGoalsforThisAgent.Add(goalAssignment.EntityLocation); } } initialStates.Add(new WorldState(agentLocation, agentGoal, agentBoxes, boxGoalsforThisAgent, walls, (Agent)agentEntity.Entity)); } return(initialStates); }