private (bool isReachable, Box closestBox) CheckIfGoalReachable(Goal goal, HashSet <Goal> otherUnprioritizedGoals, HashSet <Box> matchedBoxesSet) { var visited = new bool[level.Rows, level.Columns]; visited[goal.Position.X, goal.Position.Y] = true; var Q = new Queue <Position>(); Q.Enqueue(goal.Position); while (Q.Any()) { var position = Q.Dequeue(); foreach (var direction in ActionGenerator.AllDirections) { var boxPosition = ActionGenerator.GeneratePositionFromDirection(position, direction); // Check if in bounds if (boxPosition.X < 0 || boxPosition.X >= level.Rows || boxPosition.Y < 0 || boxPosition.Y >= level.Columns) { continue; } if (!visited[boxPosition.X, boxPosition.Y]) { var @object = level.Map[boxPosition.X, boxPosition.Y]; var goalObject = level.GoalsMap[boxPosition.X, boxPosition.Y]; // Return if the goal is reachable if (@object is Box box && box.Symbol == goal.Symbol && !matchedBoxesSet.Contains(box)) { return(true, box); } // what if @object is an Agent? if (@object is Wall) { continue; } if (goalObject is Goal potentialUnprioritizedGoal && otherUnprioritizedGoals.Contains(potentialUnprioritizedGoal)) { continue; } visited[boxPosition.X, boxPosition.Y] = true; Q.Enqueue(boxPosition); } } } return(false, null); }
/// <summary> /// Calculates distances from all positions to all goals /// </summary> private void CalculateDistancesToGoals() { DistancesToGoal = new Dictionary <Goal, int[, ]>(); foreach (var goal in Level.AllGoals) { // Omit if there aren't any boxes for the goal if (!Level.Boxes.Any(box => box.Symbol == goal.Symbol)) { continue; } var distances = new int[Level.Rows, Level.Columns]; for (var i = 0; i < Level.Rows; i++) { for (var j = 0; j < Level.Columns; j++) { distances[i, j] = int.MaxValue; } } distances[goal.Position.X, goal.Position.Y] = 0; var Q = new Queue <Position>(); Q.Enqueue(goal.Position); while (Q.Any()) { var position = Q.Dequeue(); foreach (var direction in ActionGenerator.AllDirections) { var boxPosition = ActionGenerator.GeneratePositionFromDirection(position, direction); // check if in bounds if (boxPosition.X < 0 || boxPosition.X >= Level.Rows || boxPosition.Y < 0 || boxPosition.Y >= Level.Columns) { continue; } if (distances[boxPosition.X, boxPosition.Y] == int.MaxValue) { if (!(Level.Map[boxPosition.X, boxPosition.Y] is Wall)) { distances[boxPosition.X, boxPosition.Y] = distances[position.X, position.Y] + 1; Q.Enqueue(boxPosition); } } } } DistancesToGoal[goal] = distances; } }
public override List <List <Action> > Solve() { // Thoughts: // Goal pull distance doesn't work well when there are a lot of boxes. var actions = new List <Action>(); var(found, goalsPrioritized, goalsUnprioritized) = CalculatePrioritizedGoals(level.AllGoals); if (!found) { var(lockingGoals, otherGoalsPrioritized) = FindLockingGoals(goalsUnprioritized); // TODO: What to do with locking goals? // Put the locking box as far as possible from both prioritized goals and their boxes? // Look at SARiddle.lvl and SALocking2.lvl throw new System.NotImplementedException("Whoops, look at those locking goals ðŸ¤"); } var currentState = new State(level.Map, level.AgentsPositions, level.BoxesPositions, level.GoalsForBox, level.AllGoals); State lastSolutionState = null; frontierSet = new HashSet <State>(new StateEqualityComparer()); foreach (var goal in goalsPrioritized) { // 1. Get to the target box. // 2. Get to the target goal. // 3. Continue. var heuristic = new AStar(level, new PrioritizeHeuristic(goal)); var exploredStates = new HashSet <State>(new StateEqualityComparer()); if (frontier == null) { frontier = new PriorityQueue <State>(new StateHeuristicComparer(heuristic)); AddToFrontier(currentState); } else { frontier.SetComparer(heuristic); } var solutionFound = false; while (!solutionFound) { // If frontier is empty, then no solution was found if (frontier.IsEmpty()) { break; } // Get the next state currentState = RemoveFromFrontier(); // Check if we reached the box if (goal.IsBoxGoal) { var boxFound = false; // Is the box within any direction from the agent? foreach (var direction in ActionGenerator.AllDirections) { var possibleBoxPosition = ActionGenerator.GeneratePositionFromDirection(currentState.AgentPosition, direction); var goalPosition = currentState.BoxesPositions[goal.BoxForGoal]; if (goalPosition.Equals(possibleBoxPosition)) { boxFound = true; break; } } if (boxFound) { solutionFound = true; lastSolutionState = currentState; } } // Check if we reached the goal else if (currentState.Map[goal.Position.X, goal.Position.Y] is Box box && box.Symbol == goal.Symbol) { solutionFound = true; lastSolutionState = currentState; } // Mark state as visited exploredStates.Add(currentState); // If solution for the current goal found if (solutionFound) { frontier.Clear(); frontierSet.Clear(); // lastSolutionState.Print(); if (!goal.IsBoxGoal) { var stateWithFixedGoal = lastSolutionState.WithFixedGoal(goal); // stateWithFixedGoal.Print(); AddToFrontier(stateWithFixedGoal); } } // Add achievable and unvisited nodes to frontier currentState.PopulateAchievableStates(); foreach (var state in currentState.AchievableStates) { if (LeadsToBlock(state)) { state.Penalty += 100000; } if (!exploredStates.Contains(state) && !frontierSet.Contains(state)) { AddToFrontier(state); } } } } return(new List <List <Action> >() { lastSolutionState.ActionsUsedToReachState }); }