From b3ab7b71714b27e649358609c42e6d7324cbc467 Mon Sep 17 00:00:00 2001 From: Artem Kliatchkine Date: Mon, 5 May 2025 09:52:34 +0200 Subject: [PATCH 01/14] Code refresh in Move and Plan classes (+ related changes in other files) --- Agent.cs | 2 +- AgentState.cs | 26 ++--- CBS.cs | 2 +- CbsConflict.cs | 6 +- CbsConstraint.cs | 26 ++--- CbsNode.cs | 28 +++--- ConflictAvoidanceTable.cs | 22 ++--- CooperativeA_Star.cs | 6 +- IndependenceDetection.cs | 2 +- MDD.cs | 2 +- MddMatchAndPrune.cs | 4 +- Move.cs | 149 ++++++++++++---------------- Plan.cs | 162 ++++++++++++++----------------- ProblemInstance.cs | 22 ++--- Run.cs | 20 ++-- TimedMove.cs | 65 ++++++------- WorldStateForPartialExpansion.cs | 16 +-- WorldStateWithOD.cs | 10 +- 18 files changed, 261 insertions(+), 309 deletions(-) diff --git a/Agent.cs b/Agent.cs index 3d38437..af04100 100644 --- a/Agent.cs +++ b/Agent.cs @@ -16,7 +16,7 @@ public Agent(Agent a) public Agent(int Goal_X, int Goal_Y, int agentNum) { this.agentNum = agentNum; - Goal = new Move(Goal_X, Goal_Y, Move.Direction.NO_DIRECTION); + Goal = new Move(Goal_X, Goal_Y, Direction.NO_DIRECTION); } public override string ToString() { diff --git a/AgentState.cs b/AgentState.cs index 7a0b7a8..1bcbace 100644 --- a/AgentState.cs +++ b/AgentState.cs @@ -32,7 +32,7 @@ namespace mapf; public AgentState(int pos_X, int pos_Y, Agent agent) { - this.lastMove = new TimedMove(pos_X, pos_Y, Move.Direction.NO_DIRECTION, 0); + this.lastMove = new TimedMove(pos_X, pos_Y, Direction.NO_DIRECTION, 0); this.agent = agent; } @@ -55,12 +55,12 @@ public AgentState(AgentState copy) /// public void SwapCurrentWithGoal() { - int nTemp = lastMove.x; - lastMove.x = agent.Goal.x; - agent.Goal.x = nTemp; - nTemp = lastMove.y; - lastMove.y = agent.Goal.y; - agent.Goal.y = nTemp; + int nTemp = lastMove.X; + lastMove.X = agent.Goal.X; + agent.Goal.X = nTemp; + nTemp = lastMove.Y; + lastMove.Y = agent.Goal.Y; + agent.Goal.Y = nTemp; } /// @@ -70,7 +70,7 @@ public void MoveTo(TimedMove move) { this.lastMove = move; - bool isWait = move.direction == Move.Direction.Wait; + bool isWait = move.Direction == Direction.Wait; bool atGoal = this.AtGoal(); // If performed a non WAIT move and reached the agent's goal - store the arrival time @@ -131,14 +131,14 @@ public override bool Equals(object obj) if (AgentState.EquivalenceOverDifferentTimes) { return this.agent.Equals(that.agent) && - this.lastMove.x == that.lastMove.x && - this.lastMove.y == that.lastMove.y; // Ignoring the time and the direction + this.lastMove.X == that.lastMove.X && + this.lastMove.Y == that.lastMove.Y; // Ignoring the time and the direction } else { return this.agent.Equals(that.agent) && - this.lastMove.x == that.lastMove.x && - this.lastMove.y == that.lastMove.y && + this.lastMove.X == that.lastMove.X && + this.lastMove.Y == that.lastMove.Y && this.lastMove.time == that.lastMove.time; // Ignoring the direction } } @@ -153,7 +153,7 @@ public override int GetHashCode() unchecked { if (AgentState.EquivalenceOverDifferentTimes) - return 3 * this.agent.GetHashCode() + 5 * this.lastMove.x + 7 * this.lastMove.y; + return 3 * this.agent.GetHashCode() + 5 * this.lastMove.X + 7 * this.lastMove.Y; else return 3 * this.agent.GetHashCode() + 5 * this.lastMove.GetHashCode(); } diff --git a/CBS.cs b/CBS.cs index 1b18eab..1a30e00 100644 --- a/CBS.cs +++ b/CBS.cs @@ -334,7 +334,7 @@ public virtual void Setup(ProblemInstance problemInstance, int minSolutionTimeSt this.SetGlobals(); - CbsNode root = new CbsNode(instance.agents.Length, this.solver, this.singleAgentSolver, this); // Problem instance and various strategy data is all passed under 'this'. + CbsNode root = new(instance.agents.Length, this.solver, this.singleAgentSolver, this); // Problem instance and various strategy data is all passed under 'this'. // Solve the root node bool solved = root.Solve(minSolutionTimeStep); diff --git a/CbsConflict.cs b/CbsConflict.cs index 1f06a3b..828d96e 100644 --- a/CbsConflict.cs +++ b/CbsConflict.cs @@ -30,7 +30,7 @@ public CbsConflict(int conflictingAgentAIndex, int conflictingAgentBIndex, Move this.agentAmove = agentAMove; this.agentBmove = agentBMove; this.timeStep = timeStep; - if (agentAMove.x == agentBMove.x && agentAMove.y == agentBMove.y) // Same dest, from any direction + if (agentAMove.X == agentBMove.X && agentAMove.Y == agentBMove.Y) // Same dest, from any direction this.isVertexConflict = true; else { @@ -62,9 +62,9 @@ public override bool Equals(object obj) return false; if (this.isVertexConflict) { // Compare dests, ignore directions. Enough to compare one agent's move because the other is colliding with it. - if (this.agentAmove.x != other.agentAmove.x) + if (this.agentAmove.X != other.agentAmove.X) return false; - if (this.agentAmove.y != other.agentAmove.y) + if (this.agentAmove.Y != other.agentAmove.Y) return false; } else diff --git a/CbsConstraint.cs b/CbsConstraint.cs index 52c36a4..8e27418 100644 --- a/CbsConstraint.cs +++ b/CbsConstraint.cs @@ -11,7 +11,7 @@ public class CbsConstraint : IComparable public TimedMove move {get; protected set;} public bool queryInstance = false; - public CbsConstraint(int agentNum, int posX, int posY, Move.Direction direction, int timeStep) + public CbsConstraint(int agentNum, int posX, int posY, Direction direction, int timeStep) { this.Init(agentNum, posX, posY, direction, timeStep); } @@ -21,7 +21,7 @@ public CbsConstraint(int agentNum, TimedMove move) this.Init(agentNum, move); } - public CbsConstraint() : this(-1, -1, -1, Move.Direction.NO_DIRECTION, -1) {} // Nonsense values until Init, just allocate move + public CbsConstraint() : this(-1, -1, -1, Direction.NO_DIRECTION, -1) {} // Nonsense values until Init, just allocate move public CbsConstraint(CbsConflict conflict, ProblemInstance instance, bool agentA) { @@ -43,10 +43,10 @@ public CbsConstraint(CbsConflict conflict, ProblemInstance instance, bool agentA this.move = new TimedMove(move, conflict.timeStep); if (conflict.isVertexConflict) - this.move.direction = Move.Direction.NO_DIRECTION; + this.move.Direction = Direction.NO_DIRECTION; } - public void Init(int agentNum, int posX, int posY, Move.Direction direction, int timeStep) + public void Init(int agentNum, int posX, int posY, Direction direction, int timeStep) { this.Init(agentNum, new TimedMove(posX, posY, direction, timeStep)); } @@ -81,8 +81,8 @@ public override bool Equals(object obj) return false; Trace.Assert(this.queryInstance == false || other.queryInstance == false); // At most one of the instances is a query - Trace.Assert(this.queryInstance == false || this.move.direction != Move.Direction.NO_DIRECTION); // Must query regarding a specific direction - Trace.Assert(other.queryInstance == false || other.move.direction != Move.Direction.NO_DIRECTION); // Must query regarding a specific direction + Trace.Assert(this.queryInstance == false || this.move.Direction != Direction.NO_DIRECTION); // Must query regarding a specific direction + Trace.Assert(other.queryInstance == false || other.move.Direction != Direction.NO_DIRECTION); // Must query regarding a specific direction if (this.queryInstance || other.queryInstance) // This way if the constraint is a vertex constraint than it will be equal to a query containing a move from any direction to that position, // and if it is an edge constraint than it will only be equal to queries containing a move from that specific direction to that position. return this.move.Equals(other.move); @@ -90,7 +90,7 @@ public override bool Equals(object obj) // Must check the direction explicitly because vertex constraints have no direction and moves with no direction // compare equal to moves with any direction // TODO: Get rid of all of this using Nathan's advice. - return this.move.Equals(other.move) && this.move.direction == other.move.direction; + return this.move.Equals(other.move) && this.move.Direction == other.move.Direction; } /// @@ -108,17 +108,11 @@ public override int GetHashCode() } } - public int GetTimeStep() { return this.move.time; } // FIXME: Make this into a property + public int GetTimeStep() => move.time; // FIXME: Make this into a property - public Move.Direction GetDirection() - { - return this.move.direction; - } + public Direction GetDirection() => move.Direction; - public override string ToString() - { - return $"{move}-{move.direction,-12} time={move.time} agentNum {agentNum}"; - } + public override string ToString() => "{move}-{move.Direction,-12} time={move.time} agentNum {agentNum}"; /// /// Kind of the opposite of Equals: checks that the moves are unequal or that not one of the other's agents appears in this.agents. diff --git a/CbsNode.cs b/CbsNode.cs index 789520d..9a0e166 100644 --- a/CbsNode.cs +++ b/CbsNode.cs @@ -174,7 +174,7 @@ public CbsNode(CbsNode parent, CbsConstraint newConstraint, int agentToReplan) this.mdds[agentToReplan].levels.Length - 1 > newConstraint.time && (this.mddNarrownessValues[agentToReplan].ContainsKey(newConstraint.time) == false || (this.mddNarrownessValues[agentToReplan][newConstraint.time] == MDD.LevelNarrowness.ONE_LOCATION_MULTIPLE_DIRECTIONS && - newConstraint.move.direction != Move.Direction.NO_DIRECTION))) + newConstraint.move.Direction != Direction.NO_DIRECTION))) { // We have an MDD and same cost can still be achieved - adapt the existing MDD double startTime = this.cbs.runner.ElapsedMilliseconds(); @@ -299,7 +299,7 @@ public bool Solve(int depthToReplan) newPlans[i] = true; } - var internalCAT = new ConflictAvoidanceTable(); + ConflictAvoidanceTable internalCAT = new(); ConflictAvoidanceTable CAT = internalCAT; if (this.cbs.externalCAT != null) { @@ -308,7 +308,7 @@ public bool Solve(int depthToReplan) ((CAT_U)CAT).Join(internalCAT); } - HashSet newConstraints = this.GetConstraints(); // Probably empty as this is probably the root of the CT. + HashSet newConstraints = GetConstraints(); // Probably empty as this is probably the root of the CT. ISet constraints = newConstraints; if (this.cbs.externalConstraints != null) { @@ -349,11 +349,11 @@ public bool Solve(int depthToReplan) // layers of CBS/ID solvers, each one adding its own constraints and respecting those of the solvers above it. // Find all the agents groups: - var subGroups = new List[problem.agents.Length]; + List[] subGroups = new List[problem.agents.Length]; for (int i = 0; i < agentsGroupAssignment.Length; i++) { if (subGroups[agentsGroupAssignment[i]] == null) - subGroups[agentsGroupAssignment[i]] = new List() { problem.agents[i] }; + subGroups[agentsGroupAssignment[i]] = [ problem.agents[i] ]; else subGroups[this.agentsGroupAssignment[i]].Add(problem.agents[i]); } @@ -375,12 +375,12 @@ public bool Solve(int depthToReplan) subGroup.Count == 1) // No constraints on this agent. Shortcut available (that doesn't consider the CAT, though!). { singleAgentPlans[i] = new SinglePlan(problem.agents[i]); // All moves up to starting pos, if any - singleAgentPlans[i].agentNum = problem.agents[this.agentsGroupAssignment[i]].agent.agentNum; // Use the group's representative + singleAgentPlans[i].AgentNum = problem.agents[this.agentsGroupAssignment[i]].agent.agentNum; // Use the group's representative SinglePlan optimalPlan = problem.GetSingleAgentOptimalPlan(problem.agents[i]); // Count conflicts: this.conflictCountsPerAgent[i] = new Dictionary(); this.conflictTimesPerAgent[i] = new Dictionary>(); - foreach (var move in optimalPlan.locationAtTimes) + foreach (var move in optimalPlan.LocationAtTimes) { var timedMove = (TimedMove)move; // GetSingleAgentOptimalPlan actually creates a plan with TimedMove instances timedMove.IncrementConflictCounts(CAT, this.conflictCountsPerAgent[i], this.conflictTimesPerAgent[i]); @@ -580,7 +580,7 @@ public bool Replan(int agentToReplan, int minPathTimeStep, conflictTimes = new Dictionary>(); foreach (var singlePlan in singlePlans) { - foreach (var move in singlePlan.locationAtTimes) + foreach (var move in singlePlan.LocationAtTimes) { var timedMove = (TimedMove)move; // The solver actually creates a plan with TimedMove instances if (CAT != null) @@ -595,7 +595,7 @@ public bool Replan(int agentToReplan, int minPathTimeStep, int agentNum = subGroup[i].agent.agentNum; int agentIndex = this.agentNumToIndex[agentNum]; this.singleAgentPlans[agentIndex] = singlePlans[i]; - this.singleAgentPlans[agentIndex].agentNum = problem.agents[groupNum].agent.agentNum; // Use the group's representative - that's how the plans will be inserted into the CAT later too. + this.singleAgentPlans[agentIndex].AgentNum = problem.agents[groupNum].agent.agentNum; // Use the group's representative - that's how the plans will be inserted into the CAT later too. this.singleAgentCosts[agentIndex] = singleCosts[i]; if (i == 0) // This is the group representative { @@ -796,8 +796,8 @@ protected void UpdateAtGoalConflictCounts(int agentIndex, ConflictAvoidanceTable { ProblemInstance problem = this.cbs.GetProblemInstance(); var afterGoal = new TimedMove( - problem.agents[agentIndex].agent.Goal.x, problem.agents[agentIndex].agent.Goal.y, - Move.Direction.Wait, time: 0); + problem.agents[agentIndex].agent.Goal.X, problem.agents[agentIndex].agent.Goal.Y, + Direction.Wait, time: 0); for (int time = singleAgentPlans[agentIndex].GetSize(); time < CAT.GetMaxPlanSize(); time++) { afterGoal.time = time; @@ -2880,7 +2880,7 @@ public void PrintConflict() { Debug.WriteLine("Conflict:"); Debug.WriteLine("Agents:({0},{1})", conflict.agentAIndex, conflict.agentBIndex); - Debug.WriteLine("Location:({0},{1})", conflict.agentAmove.x, conflict.agentAmove.y); + Debug.WriteLine("Location:({0},{1})", conflict.agentAmove.X, conflict.agentAmove.Y); Debug.WriteLine("Time:{0}", conflict.timeStep); } Debug.WriteLine(""); @@ -2894,7 +2894,7 @@ public void PrintConflict() /// public int PathLength(int agent) { - List moves = singleAgentPlans[agent].locationAtTimes; + List moves = singleAgentPlans[agent].LocationAtTimes; Move goal = moves[moves.Count - 1]; for (int i = moves.Count - 2; i >= 0; i--) { @@ -3031,7 +3031,7 @@ public bool Replan3b(int agentToReplan, int depthToReplan, int minPathCost = -1, if (this.agentsGroupAssignment[i] == groupNum) { this.singleAgentPlans[i] = singlePlans[j]; - this.singleAgentPlans[i].agentNum = problem.agents[groupNum].agent.agentNum; // Use the group's representative + this.singleAgentPlans[i].AgentNum = problem.agents[groupNum].agent.agentNum; // Use the group's representative this.singleAgentCosts[i] = singleCosts[j]; j++; } diff --git a/ConflictAvoidanceTable.cs b/ConflictAvoidanceTable.cs index 7f2e356..e9023aa 100644 --- a/ConflictAvoidanceTable.cs +++ b/ConflictAvoidanceTable.cs @@ -61,15 +61,15 @@ public void AddPlan(SinglePlan plan) { if (ReferenceEquals(step, queryTimedMove)) // Need a separate object that would serve as the key step = new TimedMove(step); - this.timedMovesToAgentNumList[step] = new List() { plan.agentNum }; + this.timedMovesToAgentNumList[step] = new List() { plan.AgentNum }; } else - this.timedMovesToAgentNumList[step].Add(plan.agentNum); + this.timedMovesToAgentNumList[step].Add(plan.AgentNum); } Move lastMove = plan.GetLocationAt(planSize - 1); - var goal = new Move(lastMove.x, lastMove.y, Move.Direction.Wait); - this.atGoalWaitsToTimeAndAgentNum[goal] = (planSize, plan.agentNum); + var goal = new Move(lastMove.X, lastMove.Y, Direction.Wait); + this.atGoalWaitsToTimeAndAgentNum[goal] = (planSize, plan.AgentNum); ++NumPlans; } @@ -87,12 +87,12 @@ public void RemovePlan(SinglePlan plan) queryTimedMove.setup(temp, i); step = queryTimedMove; } - this.timedMovesToAgentNumList[step].Remove(plan.agentNum); + this.timedMovesToAgentNumList[step].Remove(plan.AgentNum); // TODO: Add asserts that check the plan was indeed in the CAT } Move lastMove = plan.GetLocationAt(planSize - 1); - queryMove.setup(lastMove.x, lastMove.y, Move.Direction.Wait); + queryMove.Setup(lastMove.X, lastMove.Y, Direction.Wait); this.atGoalWaitsToTimeAndAgentNum.Remove(queryMove); --NumPlans; } @@ -115,7 +115,7 @@ public IReadOnlyList this[TimedMove key] ans.AddRange(this.timedMovesToAgentNumList[key]); } - queryMove.setup(key); + queryMove.Setup(key); if (this.atGoalWaitsToTimeAndAgentNum.ContainsKey(queryMove)) { var timeAndAgentNum = this.atGoalWaitsToTimeAndAgentNum[queryMove]; @@ -135,10 +135,10 @@ public IReadOnlyList this[TimedMove key] } } - private static readonly List emptyList = new List(0); + private static readonly List emptyList = []; - private Move queryMove = new Move(); - private TimedMove queryTimedMove = new TimedMove(); + private Move queryMove = new(); + private TimedMove queryTimedMove = new(); /// /// Determines whether the read-only dictionary contains an element that has @@ -153,7 +153,7 @@ public bool ContainsKey(TimedMove key) if (this.timedMovesToAgentNumList.ContainsKey(key)) return true; - queryMove.setup(key); + queryMove.Setup(key); if (this.atGoalWaitsToTimeAndAgentNum.ContainsKey(queryMove)) { var timeAndAgentNum = this.atGoalWaitsToTimeAndAgentNum[queryMove]; diff --git a/CooperativeA_Star.cs b/CooperativeA_Star.cs index c5dc599..25e7f02 100644 --- a/CooperativeA_Star.cs +++ b/CooperativeA_Star.cs @@ -171,7 +171,7 @@ private bool singleAgentAStar(AgentState agent) bool valid = true; for (int i = node.lastMove.time ; i <= maxPathCostSoFar; i++) { - queryTimedMove.setup(node.lastMove.x, node.lastMove.y, Move.Direction.NO_DIRECTION, i); + queryTimedMove.setup(node.lastMove.X, node.lastMove.Y, Direction.NO_DIRECTION, i); if (reservationTable.Contains(queryTimedMove)) valid = false; } @@ -180,7 +180,7 @@ private bool singleAgentAStar(AgentState agent) this.paths[agent.agent.agentNum] = new SinglePlan(node); reservePath(node); totalcost += node.lastMove.time; - parked.Add(new Move(node.lastMove.x, node.lastMove.y, Move.Direction.NO_DIRECTION), node.lastMove.time); + parked.Add(new Move(node.lastMove.X, node.lastMove.Y, Direction.NO_DIRECTION), node.lastMove.time); return true; } } @@ -232,7 +232,7 @@ private bool isValidMove(TimedMove move) return false; if (move.IsColliding(this.reservationTable)) return false; - this.queryMove.setup(move.x, move.y, Move.Direction.NO_DIRECTION); + this.queryMove.Setup(move.X, move.Y, Direction.NO_DIRECTION); if (parked.ContainsKey(this.queryMove) && parked[this.queryMove] <= move.time) return false; return true; diff --git a/IndependenceDetection.cs b/IndependenceDetection.cs index 95190fb..8b22e9a 100644 --- a/IndependenceDetection.cs +++ b/IndependenceDetection.cs @@ -952,7 +952,7 @@ protected void IncrementConflictCountsAtGoal(IndependenceDetectionAgentsGroup gr { for (int i = 0; i < group.allAgentsState.Length; ++i) { - var afterGoal = new TimedMove(group.allAgentsState[i].agent.Goal.x, group.allAgentsState[i].agent.Goal.y, Move.Direction.Wait, time: 0); + var afterGoal = new TimedMove(group.allAgentsState[i].agent.Goal.X, group.allAgentsState[i].agent.Goal.Y, Direction.Wait, time: 0); for (int time = group.GetPlan().GetSize(); time < CAT.GetMaxPlanSize(); time++) { afterGoal.time = time; diff --git a/MDD.cs b/MDD.cs index 6a48bbf..5aac661 100644 --- a/MDD.cs +++ b/MDD.cs @@ -580,7 +580,7 @@ public void setMyNode(LinkedListNode node) public int getVertexIndex() { - return move.x * this.mdd.problem.GetMaxY() + move.y; + return move.X * this.mdd.problem.GetMaxY() + move.Y; } /// diff --git a/MddMatchAndPrune.cs b/MddMatchAndPrune.cs index e6835ec..7dd252d 100644 --- a/MddMatchAndPrune.cs +++ b/MddMatchAndPrune.cs @@ -118,7 +118,7 @@ private void reverseExpand(MddMatchAndPruneState toExpand) { ((CostTreeSearchWithEdgesMatrix)this.nodeSolver.solver).edgesMatrix[i, parent.allPositions[i].getVertexIndex(), - (int) Move.Direction.Wait] = + (int) Direction.Wait] = ((CostTreeSearchWithEdgesMatrix)this.nodeSolver.solver).edgesMatrixCounter + 1; } if (closedList.ContainsKey(parent) == false) @@ -153,7 +153,7 @@ private void pruneLevel(int level) //if not legal int edge = ((CostTreeSearchWithEdgesMatrix)this.nodeSolver.solver).edgesMatrix[i, parent.getVertexIndex(), - (int)node.move.direction /*or parent.move.direction, I'm not sure*/]; + (int)node.move.Direction /*or parent.move.direction, I'm not sure*/]; if (edge != ((CostTreeSearchWithEdgesMatrix)this.nodeSolver.solver).edgesMatrixCounter + 1) { parentsToDelete[parentI] = parent; diff --git a/Move.cs b/Move.cs index bedeee6..ed9ff95 100644 --- a/Move.cs +++ b/Move.cs @@ -1,8 +1,28 @@ using System.Collections.Generic; -using System.Linq; namespace mapf; +public enum Direction : byte +{ + Wait = 0, + North, + East, + South, + West, + NorthEast, + SouthEast, + SouthWest, + NorthWest, + /// + /// This constant is set to the direction field to mark that this move does not hold a direction. + /// + /// Directionless moves are a poor design choice instead of making a class that represents a 2D point, + /// but will probably be more efficient. + /// + /// + NO_DIRECTION = 9, +}; + /// /// This class represents a single move of an agent. /// It includes the target location of the move and the direction of the move. @@ -10,52 +30,25 @@ namespace mapf; /// public class Move { - public enum Direction : int - { - Wait = 0, - North, - East, - South, - West, - NorthEast, - SouthEast, - SouthWest, - NorthWest, - /// - /// This constant is set to the direction field to mark that this move does not hold a direction. - /// - /// Directionless moves are a poor design choice instead of making a class that represents a 2D point, - /// but will probably be more efficient. - /// - /// - NO_DIRECTION = 9, - } - public const int FIRST_NON_WAIT = (int)Direction.North; public const int NUM_NON_DIAG_MOVES = 5; public const int NUM_DIRECTIONS = 9; - public int x; - public int y; - public Direction direction; + public int X { get; set; } + public int Y { get; set; } + + public Direction Direction { get; set; } public Move() { } public Move(int x, int y, Direction direction) { - // TODO: Consider calling Setup(x, y, direction) instead of this duplication - this.x = x; - this.y = y; - this.direction = direction; + X = x; + Y = y; + Direction = direction; } - public Move(Move cpy) - { - // TODO: Consider calling Setup(cpy) instead of this duplication - this.x = cpy.x; - this.y = cpy.y; - this.direction = cpy.direction; - } + public Move(Move cpy) : this( cpy.X, cpy.Y, cpy.Direction ) {} protected static readonly int[,] directionToDeltas = { {0, 0, }, // Wait @@ -139,7 +132,7 @@ public IEnumerable GetNextMoves() directions = Move.validDirectionsNoDiag; foreach (Direction op in directions) { - yield return new Move(this.x + Move.directionToDeltas[(int)op, 0], this.y + Move.directionToDeltas[(int)op, 1], op); + yield return new Move(X + Move.directionToDeltas[(int)op, 0], Y + Move.directionToDeltas[(int)op, 1], op); } } @@ -149,25 +142,25 @@ public IEnumerable GetNextMoves() /// public virtual void Update(Direction direction) { - this.x += Move.directionToDeltas[(int)direction, 0]; - this.y += Move.directionToDeltas[(int)direction, 1]; - this.direction = direction; + X += Move.directionToDeltas[(int)direction, 0]; + Y += Move.directionToDeltas[(int)direction, 1]; + Direction = direction; } public Move GetSource() { - var source_x = this.x + directionToOppositeDeltas[(int)direction, 0]; - var source_y = this.y + directionToOppositeDeltas[(int)direction, 1]; + var source_x = X + directionToOppositeDeltas[(int)Direction, 0]; + var source_y = Y + directionToOppositeDeltas[(int)Direction, 1]; return new Move(source_x, source_y, Direction.NO_DIRECTION); } public Move GetOppositeMove() { - if (direction == Direction.Wait || direction == Direction.NO_DIRECTION) + if (Direction == Direction.Wait || Direction == Direction.NO_DIRECTION) return this; // Not Move(this). TODO: Make sure this is correct. - return new Move(this.x + directionToOppositeDeltas[(int)direction, 0], - this.y + directionToOppositeDeltas[(int)direction, 1], - directionToOppositeDirection[(int)direction]); + return new Move(X + directionToOppositeDeltas[(int)Direction, 0], + Y + directionToOppositeDeltas[(int)Direction, 1], + directionToOppositeDirection[(int)Direction]); } /// @@ -176,8 +169,8 @@ public Move GetOppositeMove() /// public Move GetMoveWithoutDirection() { - Move copy = new Move(this); - copy.direction = Direction.NO_DIRECTION; + Move copy = new(this); + copy.Direction = Direction.NO_DIRECTION; return copy; } @@ -186,35 +179,27 @@ public Move GetMoveWithoutDirection() /// public void setOppositeMove() { - this.x += directionToOppositeDeltas[(int)direction, 0]; - this.y += directionToOppositeDeltas[(int)direction, 1]; + X += directionToOppositeDeltas[(int)Direction, 0]; + Y += directionToOppositeDeltas[(int)Direction, 1]; // Consider making directionToOppositeDeltas a jagged array, // reducing the number of table lookups to one for the above lines // since both entries in the sub-array are needed - this.direction = directionToOppositeDirection[(int)direction]; + Direction = directionToOppositeDirection[(int)Direction]; } - public void setup(int x, int y, Direction direction) + public void Setup(int x, int y, Direction direction) { - this.x = x; - this.y = y; - this.direction = direction; + X = x; + Y = y; + Direction = direction; } - public void setup(Move cpy) - { - this.x = cpy.x; - this.y = cpy.y; - this.direction = cpy.direction; - } + public void Setup(Move cpy) => Setup(cpy.X, cpy.Y, cpy.Direction); /// /// Removes the direction of this Move /// - public void RemoveDirection() - { - this.direction = Direction.NO_DIRECTION; - } + public void RemoveDirection() => Direction = Direction.NO_DIRECTION; /// /// Check if the given move collides with this move. @@ -222,12 +207,7 @@ public void RemoveDirection() /// 1. Head on collision /// 2. When other move targets the same location. /// - /// - /// - public bool IsColliding(Move other) - { - return IsColliding(other.x, other.y, other.direction); - } + public bool IsColliding(Move other) => IsColliding(other.X, other.Y, other.Direction); /// /// Check if colliding with an agent moving to the given x,y from the given direction. @@ -237,23 +217,19 @@ public bool IsColliding(Move other) /// TODO: When diagonal moves are allowed, need to also check for diagonal collisions, e.g., (0,0)->(1,1) and (0,1)->(1,0) and such. /// No rush, though. We don't currently work with diagonal moves. /// - /// - /// - /// - /// public bool IsColliding(int other_x, int other_y, Direction other_direction) { // Same target check - if (this.x == other_x && this.y == other_y) + if (X == other_x && Y == other_y) return true; // Head-on collision check if (Constants.ALLOW_HEAD_ON_COLLISION == false) { - var source_x = this.x + directionToOppositeDeltas[(int)this.direction, 0]; - var source_y = this.y + directionToOppositeDeltas[(int)this.direction, 1]; + var source_x = X + directionToOppositeDeltas[(int)Direction, 0]; + var source_y = Y + directionToOppositeDeltas[(int)Direction, 1]; var other_source_x = other_x + directionToOppositeDeltas[(int)other_direction, 0]; var other_source_y = other_y + directionToOppositeDeltas[(int)other_direction, 1]; - return this.x == other_source_x && this.y == other_source_y && other_x == source_x && other_y == source_y; + return X == other_source_x && Y == other_source_y && other_x == source_x && other_y == source_y; } else { @@ -294,8 +270,8 @@ public override int GetHashCode() unchecked // wrap-around is fine in hash functions { int hash = 17; - hash = 23 * hash + x; - hash = 23 * hash + y; + hash = 23 * hash + X; + hash = 23 * hash + Y; // NOT including the direction in the hash. // We want moves with no direction to be equal to moves with direction on the same coordinate, // so they need to have the same hash. Thus, even if this move has a direction it mustn't use it in the hash. @@ -315,13 +291,10 @@ public override bool Equals(object obj) if (obj == null) return false; Move that = (Move) obj; - return (this.x == that.x && this.y == that.y && - ((this.direction == Direction.NO_DIRECTION) || (that.direction == Direction.NO_DIRECTION) || - (this.direction == that.direction))); + return (X == that.X && Y == that.Y && + ((Direction == Direction.NO_DIRECTION) || (that.Direction == Direction.NO_DIRECTION) || + (Direction == that.Direction))); } - public override string ToString() - { - return $"({this.x},{this.y})"; // not describing the direction - } + public override string ToString() => $"({X},{Y})"; // not describing the direction } diff --git a/Plan.cs b/Plan.cs index 2a27943..9394204 100644 --- a/Plan.cs +++ b/Plan.cs @@ -12,7 +12,7 @@ namespace mapf; /// public class Plan { - private LinkedList> locationsAtTimes; + private LinkedList> _locationsAtTimes = []; /// /// Reconstructs the plan by goind backwards from the goal. @@ -21,11 +21,11 @@ public class Plan public Plan(WorldState goalState) { WorldState currentNode = goalState; - this.locationsAtTimes = new LinkedList>(); // TODO: Initialize list with #agents + // TODO: Initialize list with #agents while (currentNode != null) { List agentMoves = currentNode.GetAgentsMoves(); - this.locationsAtTimes.AddFirst(agentMoves); + _locationsAtTimes.AddFirst(agentMoves); currentNode = currentNode.prevStep; } } @@ -37,12 +37,12 @@ public Plan(WorldState goalState) public Plan(AgentState goalState) { AgentState currentNode = goalState; - this.locationsAtTimes = new LinkedList>(); // TODO: Initialize list with #agents + // TODO: Initialize list with #agents while (currentNode != null) { - var l = new List(); + List l = []; l.Add(currentNode.GetMove()); - this.locationsAtTimes.AddFirst(l); + _locationsAtTimes.AddFirst(l); currentNode = currentNode.prev; } } @@ -50,18 +50,16 @@ public Plan(AgentState goalState) /// /// Assumes all routes are of the same length. /// - /// public Plan(LinkedList[] routePerAgent) { - this.locationsAtTimes = new LinkedList>(); for (int i = 0; i < routePerAgent[0].Count; i++) { - locationsAtTimes.AddLast(new List()); + _locationsAtTimes.AddLast([]); } foreach (LinkedList agentRoute in routePerAgent) { - LinkedListNode> locationsAtTime = this.locationsAtTimes.First; + LinkedListNode> locationsAtTime = _locationsAtTimes.First; foreach (Move agentLocation in agentRoute) { locationsAtTime.Value.Add(agentLocation); @@ -73,51 +71,46 @@ public Plan(LinkedList[] routePerAgent) /// /// Generates a big plan from a collection of smaller plans. /// - /// public Plan(IEnumerable subplans) { int maxSize = subplans.Max(plan => plan.GetSize()); - this.locationsAtTimes = new LinkedList>(); + _locationsAtTimes = []; for (int time = 0; time < maxSize; time++) { - var allMoves = new List(); + List allMoves = []; foreach (Plan plan in subplans) foreach (Move move in plan.GetLocationsAt(time)) allMoves.Add(move); - this.locationsAtTimes.AddLast(allMoves); + _locationsAtTimes.AddLast(allMoves); } } public Plan(IEnumerable subplans) // FIXME: Almost fully duplicates the previous method { int maxSize = subplans.Max(plan => plan.GetSize()); - this.locationsAtTimes = new LinkedList>(); for (int time = 0; time < maxSize; time++) { - var allMoves = new List(); + List allMoves = []; foreach (SinglePlan plan in subplans) { allMoves.Add(plan.GetLocationAt(time)); } - this.locationsAtTimes.AddLast(allMoves); + _locationsAtTimes.AddLast(allMoves); } } /// /// Medium-depth copy constructor - uses same Move objects. /// - /// public Plan(Plan cpy) { - this.locationsAtTimes = new LinkedList>(); - - foreach (List cpyStep in cpy.locationsAtTimes) + foreach (List cpyStep in cpy._locationsAtTimes) { - this.locationsAtTimes.AddLast(cpyStep.ToList()); + _locationsAtTimes.AddLast([.. cpyStep]); } } @@ -126,36 +119,35 @@ public Plan(Plan cpy) /// If this plan ends where the other starts, /// the first timestep of the other plan is skipped. /// - /// public void ContinueWith(Plan other) { bool first = true; - foreach (List newLocationsAtTime in other.locationsAtTimes) + foreach (List newLocationsAtTime in other._locationsAtTimes) { if (first) { first = false; - if (newLocationsAtTime.SequenceEqual(this.locationsAtTimes.Last.Value)) + if (newLocationsAtTime.SequenceEqual(_locationsAtTimes.Last.Value)) continue; else Trace.Assert(false, "Continuing a plan doesn't start from the same state"); } - this.locationsAtTimes.AddLast(newLocationsAtTime); + _locationsAtTimes.AddLast(newLocationsAtTime); } } public void Check(ProblemInstance problem) { - SinglePlan[] singles = new SinglePlan[this.locationsAtTimes.First().Count]; + SinglePlan[] singles = new SinglePlan[_locationsAtTimes.First().Count]; for (int i = 0; i < singles.Length; i++) { singles[i] = new SinglePlan(this, i, problem.agents[i].agent.agentNum); - foreach ((int time, var move) in singles[i].locationAtTimes.Enumerate()) + foreach ((int time, var move) in singles[i].LocationAtTimes.Enumerate()) Trace.Assert(problem.IsValid(move), $"Plan of agent {i} uses an invalid location {move} at time {time}!"); } // Check in every time step that the plans do not collide - for (int time = 1; time < this.locationsAtTimes.Count; time++) // Assuming no conflicts exist in time zero. + for (int time = 1; time < _locationsAtTimes.Count; time++) // Assuming no conflicts exist in time zero. { // Check all pairs of agents for a collision at the given time step foreach ((int i1, var plan1) in singles.Enumerate()) @@ -180,24 +172,21 @@ public void Check(ProblemInstance problem) /// A list of Moves that are the locations of the different agents at the requested time public List GetLocationsAt(int time) { - if (time < this.locationsAtTimes.Count) - return this.locationsAtTimes.ElementAt(time); // FIXME: Expensive! + if (time < _locationsAtTimes.Count) + return _locationsAtTimes.ElementAt(time); // FIXME: Expensive! else { - var toCopy = this.locationsAtTimes.Last.Value; - var atRest = toCopy.ToList(); + List toCopy = _locationsAtTimes.Last.Value; + List atRest = [.. toCopy]; for (int i = 0; i < atRest.Count; i++) { - atRest[i] = new Move(atRest[i].x, atRest[i].y, Move.Direction.Wait); + atRest[i] = new Move(atRest[i].X, atRest[i].Y, Direction.Wait); } return atRest; } } - public LinkedList> GetLocations() - { - return this.locationsAtTimes; - } + public LinkedList> GetLocations() => _locationsAtTimes; /// /// NOT the cost, which: @@ -207,10 +196,7 @@ public LinkedList> GetLocations() /// Useful only for iteration over the relevant part of the plan. /// /// The size of the plan, assuming is doesn't end with steps where all agents WAIT at the goal (which should be discounted). - public int GetSize() - { - return this.locationsAtTimes.Count; - } + public int GetSize() => _locationsAtTimes.Count; /// /// Check if this plan collides with another plan at a given time @@ -219,7 +205,7 @@ public int GetSize() /// The plan to check against public bool IsColliding(int time, Plan otherPlan) { - List thisLocations = this.GetLocationsAt(time); + List thisLocations = GetLocationsAt(time); List otherLocations = otherPlan.GetLocationsAt(time); // TODO: Think of a better implementation of this @@ -237,7 +223,7 @@ public bool IsColliding(int time, Plan otherPlan) public void PrintPlanIfShort() { var planSize = GetSize(); - var numAgents = this.locationsAtTimes.First.Value.Count; + var numAgents = _locationsAtTimes.First.Value.Count; if (planSize < 200 && numAgents < 30) PrintPlan(); else if (planSize >= 200) @@ -252,7 +238,7 @@ public void PrintPlanIfShort() /// public void PrintPlan() { - foreach (List locationsAtTime in this.locationsAtTimes) + foreach (List locationsAtTime in _locationsAtTimes) { Console.Write("|"); foreach (Move aMove in locationsAtTime) @@ -265,8 +251,8 @@ public void PrintPlan() public override string ToString() { - var s = new StringBuilder(); - foreach (List locationsAtTime in this.locationsAtTimes) + StringBuilder s = new(); + foreach (List locationsAtTime in _locationsAtTimes) { s.Append($"|{String.Join("|", locationsAtTime)}|\n"); } @@ -277,7 +263,7 @@ public HashSet AddPlanToHashSet(HashSet addTo, int until) { for (int i = 1; i < until; i++) // i = 1 because we assume start positions don't overlap { - List step = this.GetLocationsAt(i); + List step = GetLocationsAt(i); foreach (Move move in step) { addTo.Add(new TimedMove(move,i)); @@ -290,8 +276,8 @@ public HashSet AddPlanToHashSet(HashSet addTo, int until) public class SinglePlan { - public List locationAtTimes { get; private set; } - public int agentNum; + public List LocationAtTimes { get; private set; } + public int AgentNum { get; set; } /// /// Not used @@ -300,15 +286,15 @@ public class SinglePlan /// public SinglePlan(WorldState goalState, int agentIndex) { - this.agentNum = goalState.allAgentsState[agentIndex].agent.agentNum; + AgentNum = goalState.allAgentsState[agentIndex].agent.agentNum; WorldState currentNode = goalState; - LinkedList locations = new LinkedList(); + LinkedList locations = []; while (currentNode != null) { locations.AddFirst(currentNode.GetSingleAgentMove(agentIndex)); currentNode = currentNode.prevStep; } - this.locationAtTimes = locations.ToList(); + LocationAtTimes = [.. locations]; } /// @@ -319,37 +305,37 @@ public SinglePlan(WorldState goalState, int agentIndex) /// To put in the returned SinglePlan public SinglePlan(Plan plan, int agentIndex, int agentNum) { - this.agentNum = agentNum; - this.locationAtTimes = new List(); + AgentNum = agentNum; + LocationAtTimes = []; foreach (List movesAtTimestep in plan.GetLocations()) { - this.locationAtTimes.Add(movesAtTimestep[agentIndex]); + LocationAtTimes.Add(movesAtTimestep[agentIndex]); } } public SinglePlan(AgentState goalState) { - this.agentNum = goalState.agent.agentNum; + AgentNum = goalState.agent.agentNum; AgentState currentNode = goalState; - LinkedList locations = new LinkedList(); + LinkedList locations = []; while (currentNode != null) { locations.AddFirst(currentNode.GetMove()); currentNode = currentNode.prev; } - this.locationAtTimes = locations.ToList(); + LocationAtTimes = locations.ToList(); } public SinglePlan(LinkedList route, int agentNum) { - this.agentNum = agentNum; - locationAtTimes = route.ToList(); + AgentNum = agentNum; + LocationAtTimes = [.. route]; } public SinglePlan(SinglePlan cpy) { - this.locationAtTimes = cpy.locationAtTimes.ToList(); // Behavior change: used to do a deep copy, with cloned moves. - this.agentNum = cpy.agentNum; + LocationAtTimes = [.. cpy.LocationAtTimes]; // Behavior change: used to do a deep copy, with cloned moves. + AgentNum = cpy.AgentNum; } /// @@ -359,12 +345,12 @@ public SinglePlan(SinglePlan cpy) /// public Move GetLocationAt(int time) { - if (time < this.locationAtTimes.Count) - return this.locationAtTimes[time]; + if (time < LocationAtTimes.Count) + return LocationAtTimes[time]; else { - var rest = new TimedMove(this.locationAtTimes[this.locationAtTimes.Count - 1], time); - rest.direction = Move.Direction.Wait; + var rest = new TimedMove(LocationAtTimes[LocationAtTimes.Count - 1], time); + rest.Direction = Direction.Wait; return rest; } } @@ -374,7 +360,7 @@ public override bool Equals(object obj) if (obj == null) return false; SinglePlan other = (SinglePlan)obj; - return this.agentNum == other.agentNum && this.locationAtTimes.SequenceEqual(other.locationAtTimes); + return AgentNum == other.AgentNum && LocationAtTimes.SequenceEqual(other.LocationAtTimes); } public override int GetHashCode() @@ -382,11 +368,11 @@ public override int GetHashCode() int ret; unchecked // wrap-around is fine in hash functions { - ret = Constants.PRIMES_FOR_HASHING[0] * this.agentNum.GetHashCode(); + ret = Constants.PRIMES_FOR_HASHING[0] * AgentNum.GetHashCode(); // Hash the contents and order of locationsAtTimes int i = 0; - foreach (var move in this.locationAtTimes) + foreach (var move in LocationAtTimes) { ret += Constants.PRIMES_FOR_HASHING[1] * i + Constants.PRIMES_FOR_HASHING[2] * move.GetHashCode(); i++; @@ -404,17 +390,17 @@ public override int GetHashCode() public void ContinueWith(SinglePlan other) { bool first = true; - foreach (Move newLocationAtTime in other.locationAtTimes) + foreach (Move newLocationAtTime in other.LocationAtTimes) { if (first) { first = false; - if (this.locationAtTimes[this.locationAtTimes.Count - 1].Equals(newLocationAtTime)) + if (LocationAtTimes[^1].Equals(newLocationAtTime)) continue; else Trace.Assert(false, "Continuing a plan doesn't start from the same state"); } - this.locationAtTimes.Add(newLocationAtTime); + LocationAtTimes.Add(newLocationAtTime); } } @@ -427,8 +413,8 @@ public void ContinueWith(SinglePlan other) /// The size of the plan, excluding WAITs at the goal public int GetSize() { - int lastNonWaitIndex = this.locationAtTimes.Count - 1; - while (lastNonWaitIndex != 0 && locationAtTimes[lastNonWaitIndex].direction == Move.Direction.Wait) + int lastNonWaitIndex = LocationAtTimes.Count - 1; + while (lastNonWaitIndex != 0 && LocationAtTimes[lastNonWaitIndex].Direction == Direction.Wait) lastNonWaitIndex--; return lastNonWaitIndex + 1; } @@ -443,18 +429,18 @@ public int GetCost() { if (Constants.sumOfCostsVariant == Constants.SumOfCostsVariant.ORIG) { - return this.GetSize() - 1; + return GetSize() - 1; } else if (Constants.sumOfCostsVariant == Constants.SumOfCostsVariant.WAITING_AT_GOAL_ALWAYS_FREE) { int cost = 0; - Move goal = this.locationAtTimes.Last(); // Assuming the plan ends at the goal - for (int i = 1; i < this.locationAtTimes.Count; i++) // The beginning position isn't a move + Move goal = LocationAtTimes.Last(); // Assuming the plan ends at the goal + for (int i = 1; i < LocationAtTimes.Count; i++) // The beginning position isn't a move { - Move move = this.locationAtTimes[i]; - if (move.x == goal.x && - move.y == goal.y && - move.direction == Move.Direction.Wait) // Waiting at the goal is free + Move move = LocationAtTimes[i]; + if (move.X == goal.X && + move.Y == goal.Y && + move.Direction == Direction.Wait) // Waiting at the goal is free continue; cost += 1; } @@ -463,7 +449,7 @@ public int GetCost() } else if (Constants.costFunction == Constants.CostFunction.MAKESPAN_THEN_SUM_OF_COSTS) { - return this.GetSize() - 1; + return GetSize() - 1; } return 0; // To quiet the compiler } @@ -475,7 +461,7 @@ public int GetCost() /// The plan to check against public bool IsColliding(int time, SinglePlan otherPlan) { - Move thisLocation = this.GetLocationAt(time); + Move thisLocation = GetLocationAt(time); Move otherLocation = otherPlan.GetLocationAt(time); if (thisLocation.IsColliding(otherLocation) == true) // IsColliding isn't virtual, @@ -492,18 +478,18 @@ public bool IsColliding(int time, SinglePlan otherPlan) /// public void DebugPrint() { - for (int time = 0; time < this.locationAtTimes.Count; time++) + for (int time = 0; time < LocationAtTimes.Count; time++) { - Debug.WriteLine($"|{this.GetLocationAt(time)}|"); + Debug.WriteLine($"|{GetLocationAt(time)}|"); } } public override string ToString() { string s = ""; - for (int time = 0; time < this.locationAtTimes.Count; time++) + for (int time = 0; time < LocationAtTimes.Count; time++) { - s += $"|{this.GetLocationAt(time)}|\n"; + s += $"|{GetLocationAt(time)}|\n"; } return s; } diff --git a/ProblemInstance.cs b/ProblemInstance.cs index 17b3391..473e677 100644 --- a/ProblemInstance.cs +++ b/ProblemInstance.cs @@ -158,7 +158,7 @@ public void ComputeSingleAgentShortestPaths() // 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); + 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); @@ -173,7 +173,7 @@ public void ComputeSingleAgentShortestPaths() { if (IsValid(aMove)) { - int entry = cardinality[aMove.x, aMove.y]; + 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)) { @@ -223,7 +223,7 @@ public int GetSingleAgentOptimalCost(int agentNum, int x, int y) /// The length of the shortest path from x,y to the goal of the agent. public int GetSingleAgentOptimalCost(int agentNum, Move move) { - return this.singleAgentOptimalCosts[agentNum][this.cardinality[move.x, move.y]]; + return this.singleAgentOptimalCosts[agentNum][this.cardinality[move.X, move.Y]]; } /// @@ -233,7 +233,7 @@ public int GetSingleAgentOptimalCost(int agentNum, Move move) /// The length of the shortest path between a given agent's location and the goal of that agent public int GetSingleAgentOptimalCost(AgentState agentState) { - int locationCardinality = this.cardinality[agentState.lastMove.x, agentState.lastMove.y]; + int locationCardinality = this.cardinality[agentState.lastMove.X, agentState.lastMove.Y]; return this.singleAgentOptimalCosts[agentState.agent.agentNum][locationCardinality]; } @@ -244,7 +244,7 @@ public int GetSingleAgentOptimalCost(AgentState agentState) /// public Move GetSingleAgentOptimalMove(AgentState agentState) { - int locationCardinality = this.cardinality[agentState.lastMove.x, agentState.lastMove.y]; + int locationCardinality = this.cardinality[agentState.lastMove.X, agentState.lastMove.Y]; return this.singleAgentOptimalMoves[agentState.agent.agentNum][locationCardinality]; } @@ -660,7 +660,7 @@ public void Export(string fileName, string mapFileName = null) foreach (var agentState in this.agents) { // Output all agent as block 1, with optimal cost -1 - output.WriteLine($"{1}\t{mapFileName}\t{grid[0].Length}\t{grid.Length}\t{agentState.lastMove.y}\t{agentState.lastMove.x}\t{agentState.agent.Goal.y}\t{agentState.agent.Goal.x}\t{-1}"); + output.WriteLine($"{1}\t{mapFileName}\t{grid[0].Length}\t{grid.Length}\t{agentState.lastMove.Y}\t{agentState.lastMove.X}\t{agentState.agent.Goal.Y}\t{agentState.agent.Goal.X}\t{-1}"); } } else if (fileName.EndsWith(".agents")) @@ -669,7 +669,7 @@ public void Export(string fileName, string mapFileName = null) foreach (var agentState in this.agents) { - output.WriteLine($"{agentState.agent.Goal.x},{agentState.agent.Goal.y},{agentState.lastMove.x},{agentState.lastMove.x}"); + output.WriteLine($"{agentState.agent.Goal.X},{agentState.agent.Goal.Y},{agentState.lastMove.X},{agentState.lastMove.X}"); } } else @@ -700,7 +700,7 @@ public void Export(string fileName, string mapFileName = null) for (int i = 0; i < this.agents.Length; i++) { state = this.agents[i]; - output.WriteLine($"{state.agent.agentNum}{EXPORT_DELIMITER}{state.agent.Goal.x}{EXPORT_DELIMITER}{state.agent.Goal.y}{EXPORT_DELIMITER}{state.lastMove.x}{EXPORT_DELIMITER}{state.lastMove.y}"); + output.WriteLine($"{state.agent.agentNum}{EXPORT_DELIMITER}{state.agent.Goal.X}{EXPORT_DELIMITER}{state.agent.Goal.Y}{EXPORT_DELIMITER}{state.lastMove.X}{EXPORT_DELIMITER}{state.lastMove.Y}"); } } output.Flush(); @@ -716,7 +716,7 @@ public void Export(string fileName, string mapFileName = null) /// location in our grid. public int GetCardinality(Move move) { - return cardinality[move.x, move.y]; + return cardinality[move.X, move.Y]; } private void PrecomputeCardinality() @@ -741,7 +741,7 @@ private void PrecomputeCardinality() /// True if the given location is a valid grid location with no obstacles public bool IsValid(Move aMove) { - return IsValidTile(aMove.x, aMove.y); + return IsValidTile(aMove.X, aMove.Y); } /// @@ -751,7 +751,7 @@ public bool IsValid(Move aMove) /// public bool IsValid(TimedMove toCheck) { - if (IsValidTile(toCheck.x, toCheck.y) == false) + if (IsValidTile(toCheck.X, toCheck.Y) == false) return false; return true; diff --git a/Run.cs b/Run.cs index 1b1e84d..404501e 100644 --- a/Run.cs +++ b/Run.cs @@ -656,7 +656,7 @@ public ProblemInstance GenerateProblemInstance(int gridSize, int agentsNum, int // Select random start/goal locations for every agent by performing a random walk for (int i = 0; i < agentsNum; i++) { - aStart[i] = new AgentState(aGoals[i].Goal.x, aGoals[i].Goal.y, aGoals[i]); + aStart[i] = new AgentState(aGoals[i].Goal.X, aGoals[i].Goal.Y, aGoals[i]); } // Initialized here only for the IsValid() call. TODO: Think how this can be sidestepped elegantly. @@ -667,18 +667,18 @@ public ProblemInstance GenerateProblemInstance(int gridSize, int agentsNum, int { for (int i = 0; i < agentsNum; i++) { - goals[aStart[i].lastMove.x][aStart[i].lastMove.y] = false; // We're going to move the goal somewhere else + goals[aStart[i].lastMove.X][aStart[i].lastMove.Y] = false; // We're going to move the goal somewhere else while (true) { - Move.Direction op = (Move.Direction)rand.Next(0, 5); // TODO: fixme + Direction op = (Direction)rand.Next(0, 5); // TODO: fixme aStart[i].lastMove.Update(op); if (problem.IsValid(aStart[i].lastMove) && - !goals[aStart[i].lastMove.x][aStart[i].lastMove.y]) // this spot isn't another agent's goal + !goals[aStart[i].lastMove.X][aStart[i].lastMove.Y]) // this spot isn't another agent's goal break; else aStart[i].lastMove.setOppositeMove(); // Rollback } - goals[aStart[i].lastMove.x][aStart[i].lastMove.y] = true; // Claim agent's new goal + goals[aStart[i].lastMove.X][aStart[i].lastMove.Y] = true; // Claim agent's new goal } } @@ -765,7 +765,7 @@ public ProblemInstance GenerateDragonAgeProblemInstance(string mapFilePath, int // Select random start/goal locations for every agent by performing a random walk for (int i = 0; i < agentsNum; i++) { - agentStates[i] = new AgentState(agentGoals[i].Goal.x, agentGoals[i].Goal.y, agentGoals[i]); + agentStates[i] = new AgentState(agentGoals[i].Goal.X, agentGoals[i].Goal.Y, agentGoals[i]); } ProblemInstance problem = new ProblemInstance(); @@ -776,19 +776,19 @@ public ProblemInstance GenerateDragonAgeProblemInstance(string mapFilePath, int { for (int i = 0; i < agentsNum; i++) { - goals[agentStates[i].lastMove.x][agentStates[i].lastMove.y] = false; // We're going to move the goal somewhere else. + goals[agentStates[i].lastMove.X][agentStates[i].lastMove.Y] = false; // We're going to move the goal somewhere else. // Move in a random legal direction: while (true) { - Move.Direction op = (Move.Direction)rand.Next(0, 5); // TODO: fixme + Direction op = (Direction)rand.Next(0, 5); // TODO: fixme agentStates[i].lastMove.Update(op); if (problem.IsValid(agentStates[i].lastMove) && - !goals[agentStates[i].lastMove.x][agentStates[i].lastMove.y]) // This spot isn't another agent's goal + !goals[agentStates[i].lastMove.X][agentStates[i].lastMove.Y]) // This spot isn't another agent's goal break; else agentStates[i].lastMove.setOppositeMove(); // Rollback } - goals[agentStates[i].lastMove.x][agentStates[i].lastMove.y] = true; // Claim agent's new goal + goals[agentStates[i].lastMove.X][agentStates[i].lastMove.Y] = true; // Claim agent's new goal } } diff --git a/TimedMove.cs b/TimedMove.cs index cdb9817..356a9b3 100644 --- a/TimedMove.cs +++ b/TimedMove.cs @@ -1,5 +1,4 @@ using System.Collections.Generic; -using System.Linq; namespace mapf; @@ -12,7 +11,7 @@ public class TimedMove : Move // public static const int FOREVER_AFTER = int.MaxValue - public TimedMove(int x, int y, Move.Direction direction, int time) + public TimedMove(int x, int y, Direction direction, int time) : base(x, y, direction) { this.time = time; @@ -31,8 +30,8 @@ public TimedMove(int x, int y, Move.Direction direction, int time) directions = Move.validDirectionsNoDiag; foreach (Direction op in directions) { - yield return new TimedMove(this.x + Move.directionToDeltas[(int)op, 0], - this.y + Move.directionToDeltas[(int)op, 1], op, this.time + 1); + yield return new TimedMove(this.X + Move.directionToDeltas[(int)op, 0], + this.Y + Move.directionToDeltas[(int)op, 1], op, this.time + 1); } } @@ -70,9 +69,9 @@ public override bool Equals(object obj) // Begin copied code of base to avoid a method call Move that = (Move)obj; - return (this.x == that.x && this.y == that.y && - ((this.direction == Direction.NO_DIRECTION) || (that.direction == Direction.NO_DIRECTION) || - (this.direction == that.direction))); + return (this.X == that.X && this.Y == that.Y && + ((this.Direction == Direction.NO_DIRECTION) || (that.Direction == Direction.NO_DIRECTION) || + (this.Direction == that.Direction))); // End copied code of base } @@ -84,8 +83,8 @@ public override int GetHashCode() // Begin copied code of base to avoid a method call: int hash = 17; - hash = 23 * hash + x; - hash = 23 * hash + y; + hash = 23 * hash + X; + hash = 23 * hash + Y; // End copied code of base return hash * 3 + this.time; } @@ -94,7 +93,7 @@ public override int GetHashCode() public new TimedMove GetMoveWithoutDirection() { TimedMove copy = new TimedMove(this); - copy.direction = Direction.NO_DIRECTION; + copy.Direction = Direction.NO_DIRECTION; return copy; } @@ -109,7 +108,7 @@ public override int GetHashCode() /// public bool IsColliding(TimedMove other) { - return IsColliding(other.x, other.y, other.direction, other.time); + return IsColliding(other.X, other.Y, other.Direction, other.time); } public bool IsColliding(int other_x, int other_y, Direction other_direction, int time) @@ -126,11 +125,11 @@ public bool IsColliding(int other_x, int other_y, Direction other_direction, int /// public new TimedMove GetOppositeMove() { - if (direction == Direction.Wait || direction == Direction.NO_DIRECTION) + if (Direction == Direction.Wait || Direction == Direction.NO_DIRECTION) return this; - return new TimedMove(this.x + Move.directionToOppositeDeltas[(int)direction, 0], - this.y + directionToOppositeDeltas[(int)direction, 1], - directionToOppositeDirection[(int)direction], this.time); + return new TimedMove(this.X + Move.directionToOppositeDeltas[(int)Direction, 0], + this.Y + directionToOppositeDeltas[(int)Direction, 1], + directionToOppositeDirection[(int)Direction], this.time); } /// @@ -140,7 +139,7 @@ public bool IsColliding(int other_x, int other_y, Direction other_direction, int /// public void setup(Move cpy, int time) { - base.setup(cpy); + base.Setup(cpy); this.time = time; } @@ -150,7 +149,7 @@ public void setup(Move cpy, int time) /// public void setup(TimedMove cpy) { - base.setup(cpy); + base.Setup(cpy); this.time = cpy.time; } @@ -161,22 +160,22 @@ public void setup(TimedMove cpy) /// /// /// - public void setup(int x, int y, Move.Direction direction, int time) + public void setup(int x, int y, Direction direction, int time) { - base.setup(x, y, direction); + base.Setup(x, y, direction); this.time = time; } public bool IsColliding(ICollection moves) { - Move.Direction saveDirection = this.direction; - this.direction = Move.Direction.NO_DIRECTION; + Direction saveDirection = this.Direction; + this.Direction = Direction.NO_DIRECTION; if (moves.Contains(this)) { - this.direction = saveDirection; + this.Direction = saveDirection; return true; } - this.direction = saveDirection; + this.Direction = saveDirection; if (Constants.ALLOW_HEAD_ON_COLLISION == false) { @@ -196,14 +195,14 @@ public bool IsColliding(IReadOnlyDictionary timedMovesToAgentID) { if (timedMovesToAgentID == null) return false; - Move.Direction saveDirection = this.direction; - this.direction = Move.Direction.NO_DIRECTION; + Direction saveDirection = this.Direction; + this.Direction = Direction.NO_DIRECTION; if (timedMovesToAgentID.ContainsKey(this)) { - this.direction = saveDirection; + this.Direction = saveDirection; return true; } - this.direction = saveDirection; + this.Direction = saveDirection; if (Constants.ALLOW_HEAD_ON_COLLISION == false) { @@ -228,7 +227,7 @@ public bool IsColliding(IReadOnlyDictionary timedMovesToAgentID) public List GetColliding(IReadOnlyDictionary timedMovesToAgentIndex) { List ans = null; - Move.Direction saveDirection = this.direction; + Direction saveDirection = this.Direction; Direction[] directions; if (Constants.ALLOW_DIAGONAL_MOVE) directions = Move.validDirections; @@ -236,7 +235,7 @@ public List GetColliding(IReadOnlyDictionary timedMovesToAg directions = Move.validDirectionsNoDiag; foreach (var direction in directions) // TEMP FIX! Need to get rid of the whole NO_DIRECTION SHTICK! It breaks transitivity! { - this.direction = direction; + this.Direction = direction; if (timedMovesToAgentIndex.ContainsKey(this)) { if (ans == null) @@ -244,7 +243,7 @@ public List GetColliding(IReadOnlyDictionary timedMovesToAg ans.Add(timedMovesToAgentIndex[this]); } } - this.direction = saveDirection; + this.Direction = saveDirection; if (Constants.ALLOW_HEAD_ON_COLLISION == false) { @@ -275,7 +274,7 @@ public List GetColliding(IReadOnlyDictionary timedMovesToAg public IReadOnlyList GetColliding(ConflictAvoidanceTable CAT) { List ans = null; - Move.Direction saveDirection = this.direction; + Direction saveDirection = this.Direction; Direction[] directions; if (Constants.ALLOW_DIAGONAL_MOVE) directions = Move.validDirections; @@ -283,7 +282,7 @@ public IReadOnlyList GetColliding(ConflictAvoidanceTable CAT) directions = Move.validDirectionsNoDiag; foreach (var direction in directions) // TEMP FIX! Need to get rid of the whole NO_DIRECTION SHTICK! It breaks transitivity! { - this.direction = direction; + this.Direction = direction; if (CAT.ContainsKey(this)) { if (ans == null) @@ -292,7 +291,7 @@ public IReadOnlyList GetColliding(ConflictAvoidanceTable CAT) ans.AddRange(CAT[this]); } } - this.direction = saveDirection; + this.Direction = saveDirection; if (Constants.ALLOW_HEAD_ON_COLLISION == false) { diff --git a/WorldStateForPartialExpansion.cs b/WorldStateForPartialExpansion.cs index 0f08006..9ddee8b 100644 --- a/WorldStateForPartialExpansion.cs +++ b/WorldStateForPartialExpansion.cs @@ -128,7 +128,7 @@ public void calcSingleAgentDeltaFs(ProblemInstance problem, ValidityChecker isVa { if (isValid(check, noMoves, this.makespan + 1, i, this, this) == false) // Is this move by itself invalid because of constraints or obstacles { - singleAgentDeltaFs[i][(int)check.direction] = byte.MaxValue; + singleAgentDeltaFs[i][(int)check.Direction] = byte.MaxValue; } else { @@ -137,20 +137,20 @@ public void calcSingleAgentDeltaFs(ProblemInstance problem, ValidityChecker isVa if (Constants.sumOfCostsVariant == Constants.SumOfCostsVariant.ORIG) { if (hBefore != 0) - singleAgentDeltaFs[i][(int)check.direction] = (byte)(hAfter - hBefore + 1); // h difference + g difference in this specific domain + singleAgentDeltaFs[i][(int)check.Direction] = (byte)(hAfter - hBefore + 1); // h difference + g difference in this specific domain else if (hAfter != 0) // If agent moved from its goal we must count and add all the steps it was stationed at the goal, since they're now part of its g difference - singleAgentDeltaFs[i][(int)check.direction] = (byte)(hAfter - hBefore + makespan - allAgentsState[i].arrivalTime + 1); + singleAgentDeltaFs[i][(int)check.Direction] = (byte)(hAfter - hBefore + makespan - allAgentsState[i].arrivalTime + 1); else - singleAgentDeltaFs[i][(int)check.direction] = 0; // This is a WAIT move at the goal. + singleAgentDeltaFs[i][(int)check.Direction] = 0; // This is a WAIT move at the goal. } else if (Constants.sumOfCostsVariant == Constants.SumOfCostsVariant.WAITING_AT_GOAL_ALWAYS_FREE) { if (hBefore == 0 && hAfter == 0) - singleAgentDeltaFs[i][(int)check.direction] = 0; // This is a WAIT move at the goal. + singleAgentDeltaFs[i][(int)check.Direction] = 0; // This is a WAIT move at the goal. else - singleAgentDeltaFs[i][(int)check.direction] = (byte)(hAfter - hBefore + 1); // h difference + g difference in this specific domain + singleAgentDeltaFs[i][(int)check.Direction] = (byte)(hAfter - hBefore + 1); // h difference + g difference in this specific domain } - singleAgentMaxLegalDeltaF = Math.Max(singleAgentMaxLegalDeltaF, singleAgentDeltaFs[i][(int)check.direction]); + singleAgentMaxLegalDeltaF = Math.Max(singleAgentMaxLegalDeltaF, singleAgentDeltaFs[i][(int)check.Direction]); } } @@ -245,7 +245,7 @@ public void UpdateRemainingDeltaF(int agentIndex) { Trace.Assert(false, $"Remaining deltaF is ushort.MaxValue, a reserved value with special meaning. agentIndex={agentIndex}"); - byte lastMoveDeltaF = this.singleAgentDeltaFs[agentIndex][(int)this.allAgentsState[agentIndex].lastMove.direction]; + byte lastMoveDeltaF = this.singleAgentDeltaFs[agentIndex][(int)this.allAgentsState[agentIndex].lastMove.Direction]; if (lastMoveDeltaF != byte.MaxValue && this.remainingDeltaF >= lastMoveDeltaF) this.remainingDeltaF -= lastMoveDeltaF; else diff --git a/WorldStateWithOD.cs b/WorldStateWithOD.cs index 9af751a..2a2b8a3 100644 --- a/WorldStateWithOD.cs +++ b/WorldStateWithOD.cs @@ -137,8 +137,8 @@ public override bool Equals(object obj) bool mightCollideLater = false; for (int j = this.agentTurn; j < this.allAgentsState.Length; j++) { - if (this.allAgentsState[i].lastMove.x == this.allAgentsState[j].lastMove.x && - this.allAgentsState[i].lastMove.y == this.allAgentsState[j].lastMove.y) // Can't just remove the direction and use IsColliding since the moves' time is different, so they'll never collide + if (this.allAgentsState[i].lastMove.X == this.allAgentsState[j].lastMove.X && + this.allAgentsState[i].lastMove.Y == this.allAgentsState[j].lastMove.Y) // Can't just remove the direction and use IsColliding since the moves' time is different, so they'll never collide { mightCollideLater = true; break; @@ -147,9 +147,9 @@ public override bool Equals(object obj) if (mightCollideLater == true) // Then check the direction too { - if (this.allAgentsState[i].lastMove.direction != Move.Direction.NO_DIRECTION && - that.allAgentsState[i].lastMove.direction != Move.Direction.NO_DIRECTION && - this.allAgentsState[i].lastMove.direction != that.allAgentsState[i].lastMove.direction) // Can't just use this.allAgentsState[i].lastMove.Equals(that.allAgentsState[i].lastMove) because TimedMoves don't ignore the time. + if (this.allAgentsState[i].lastMove.Direction != Direction.NO_DIRECTION && + that.allAgentsState[i].lastMove.Direction != Direction.NO_DIRECTION && + this.allAgentsState[i].lastMove.Direction != that.allAgentsState[i].lastMove.Direction) // Can't just use this.allAgentsState[i].lastMove.Equals(that.allAgentsState[i].lastMove) because TimedMoves don't ignore the time. return false; } } From 5521dbd35c56d5b9b49c62867ed8455214f59516 Mon Sep 17 00:00:00 2001 From: Artem Kliatchkine Date: Mon, 5 May 2025 11:56:32 +0200 Subject: [PATCH 02/14] TimedMove code refresh (+ related files). --- A_Star_MDDs.cs | 2 +- AgentState.cs | 16 +++---- CBS.cs | 2 +- CbsConstraint.cs | 6 +-- CbsNode.cs | 4 +- ConflictAvoidanceTable.cs | 8 ++-- CooperativeA_Star.cs | 10 ++-- IndependenceDetection.cs | 2 +- MDD.cs | 10 ++-- MddMatchAndPrune.cs | 4 +- Move.cs | 2 +- ProblemInstance.cs | 2 +- Run.cs | 8 ++-- TimedMove.cs | 98 ++++++++++++++++----------------------- WorldState.cs | 2 +- 15 files changed, 80 insertions(+), 96 deletions(-) diff --git a/A_Star_MDDs.cs b/A_Star_MDDs.cs index c416b37..93ca8db 100644 --- a/A_Star_MDDs.cs +++ b/A_Star_MDDs.cs @@ -591,7 +591,7 @@ public override int GetHashCode() } } - public int GetDepth() { return allSteps[0].move.time; } + public int GetDepth() { return allSteps[0].move.Time; } /// /// Updates the conflictCount member according to given CATs. Table may be null. diff --git a/AgentState.cs b/AgentState.cs index 1bcbace..9bd130f 100644 --- a/AgentState.cs +++ b/AgentState.cs @@ -75,14 +75,14 @@ public void MoveTo(TimedMove move) // If performed a non WAIT move and reached the agent's goal - store the arrival time if (atGoal && (isWait == false)) - this.arrivalTime = move.time; + this.arrivalTime = move.Time; if (Constants.sumOfCostsVariant == Constants.SumOfCostsVariant.ORIG) { if (this.AtGoal()) this.g = this.arrivalTime; else - this.g = this.lastMove.time; + this.g = this.lastMove.Time; } else if (Constants.sumOfCostsVariant == Constants.SumOfCostsVariant.WAITING_AT_GOAL_ALWAYS_FREE) { @@ -139,7 +139,7 @@ public override bool Equals(object obj) return this.agent.Equals(that.agent) && this.lastMove.X == that.lastMove.X && this.lastMove.Y == that.lastMove.Y && - this.lastMove.time == that.lastMove.time; // Ignoring the direction + this.lastMove.Time == that.lastMove.Time; // Ignoring the direction } } @@ -172,9 +172,9 @@ public Move GetMove() public int CompareTo(IBinaryHeapItem other) { AgentState that = (AgentState)other; - if (this.h + this.lastMove.time < that.h + that.lastMove.time) + if (this.h + this.lastMove.Time < that.h + that.lastMove.Time) return -1; - if (this.h + this.lastMove.time > that.h + that.lastMove.time) + if (this.h + this.lastMove.Time > that.h + that.lastMove.Time) return 1; if (this.potentialConflictsID < that.potentialConflictsID) @@ -190,15 +190,15 @@ public int CompareTo(IBinaryHeapItem other) // TODO: Prefer goal nodes. // Prefer larger g: - if (this.lastMove.time < that.lastMove.time) + if (this.lastMove.Time < that.lastMove.Time) return 1; - if (this.lastMove.time > that.lastMove.time) + if (this.lastMove.Time > that.lastMove.Time) return -1; return 0; } public override string ToString() { - return $"step-{lastMove.time} position {this.lastMove}"; + return $"step-{lastMove.Time} position {this.lastMove}"; } } diff --git a/CBS.cs b/CBS.cs index 1a30e00..ae495c7 100644 --- a/CBS.cs +++ b/CBS.cs @@ -269,7 +269,7 @@ public void Setup(ProblemInstance problemInstance, Run runner, ConflictAvoidance constraints.Add(new CbsConstraint(agentState.agent.agentNum, illegalMove)); } } - this.Setup(problemInstance, illegalMoves.Max(move => move.time), runner, CAT, constraints, null, targetCost, targetCost); + this.Setup(problemInstance, illegalMoves.Max(move => move.Time), runner, CAT, constraints, null, targetCost, targetCost); } /// diff --git a/CbsConstraint.cs b/CbsConstraint.cs index 8e27418..3fab267 100644 --- a/CbsConstraint.cs +++ b/CbsConstraint.cs @@ -61,7 +61,7 @@ public int time { get { - return this.move.time; + return this.move.Time; } } @@ -108,7 +108,7 @@ public override int GetHashCode() } } - public int GetTimeStep() => move.time; // FIXME: Make this into a property + public int GetTimeStep() => move.Time; // FIXME: Make this into a property public Direction GetDirection() => move.Direction; @@ -132,7 +132,7 @@ public int CompareTo(object item) { CbsConstraint other = (CbsConstraint)item; - return this.move.time.CompareTo(other.move.time); + return this.move.Time.CompareTo(other.move.Time); } public bool ViolatesMustConstraint(byte agent, TimedMove move) diff --git a/CbsNode.cs b/CbsNode.cs index 9a0e166..f225d6d 100644 --- a/CbsNode.cs +++ b/CbsNode.cs @@ -800,7 +800,7 @@ protected void UpdateAtGoalConflictCounts(int agentIndex, ConflictAvoidanceTable Direction.Wait, time: 0); for (int time = singleAgentPlans[agentIndex].GetSize(); time < CAT.GetMaxPlanSize(); time++) { - afterGoal.time = time; + afterGoal.Time = time; afterGoal.IncrementConflictCounts(CAT, this.conflictCountsPerAgent[this.agentsGroupAssignment[agentIndex]], this.conflictTimesPerAgent[this.agentsGroupAssignment[agentIndex]]); @@ -1974,7 +1974,7 @@ private CbsConflict FindConflict(int aConflictingGroupMemberIndex, out specificConflictingAgentA, out specificConflictingAgentB, groups); ProblemInstance problem = this.cbs.GetProblemInstance(); - int initialTimeStep = problem.agents[0].lastMove.time; // To account for solving partially solved problems. + int initialTimeStep = problem.agents[0].lastMove.Time; // To account for solving partially solved problems. // This assumes the makespan of all the agents is the same. Move first = singleAgentPlans[specificConflictingAgentA].GetLocationAt(time); Move second = singleAgentPlans[specificConflictingAgentB].GetLocationAt(time); diff --git a/ConflictAvoidanceTable.cs b/ConflictAvoidanceTable.cs index e9023aa..ff51a88 100644 --- a/ConflictAvoidanceTable.cs +++ b/ConflictAvoidanceTable.cs @@ -54,7 +54,7 @@ public void AddPlan(SinglePlan plan) step = (TimedMove)temp; else // It's a Move object { - queryTimedMove.setup(temp, i); + queryTimedMove.Setup(temp, i); step = queryTimedMove; } if (this.timedMovesToAgentNumList.ContainsKey(step) == false) @@ -84,7 +84,7 @@ public void RemovePlan(SinglePlan plan) step = (TimedMove)temp; else // It's a Move object { - queryTimedMove.setup(temp, i); + queryTimedMove.Setup(temp, i); step = queryTimedMove; } this.timedMovesToAgentNumList[step].Remove(plan.AgentNum); @@ -119,7 +119,7 @@ public IReadOnlyList this[TimedMove key] if (this.atGoalWaitsToTimeAndAgentNum.ContainsKey(queryMove)) { var timeAndAgentNum = this.atGoalWaitsToTimeAndAgentNum[queryMove]; - if (key.time >= timeAndAgentNum.time) + if (key.Time >= timeAndAgentNum.time) { if (ans == null) ans = new List() { timeAndAgentNum.agentNum }; @@ -157,7 +157,7 @@ public bool ContainsKey(TimedMove key) if (this.atGoalWaitsToTimeAndAgentNum.ContainsKey(queryMove)) { var timeAndAgentNum = this.atGoalWaitsToTimeAndAgentNum[queryMove]; - if (key.time >= timeAndAgentNum.time) + if (key.Time >= timeAndAgentNum.time) return true; } return false; diff --git a/CooperativeA_Star.cs b/CooperativeA_Star.cs index 25e7f02..b16f5c3 100644 --- a/CooperativeA_Star.cs +++ b/CooperativeA_Star.cs @@ -169,9 +169,9 @@ private bool singleAgentAStar(AgentState agent) if (node.h == 0) { bool valid = true; - for (int i = node.lastMove.time ; i <= maxPathCostSoFar; i++) + for (int i = node.lastMove.Time ; i <= maxPathCostSoFar; i++) { - queryTimedMove.setup(node.lastMove.X, node.lastMove.Y, Direction.NO_DIRECTION, i); + queryTimedMove.Setup(node.lastMove.X, node.lastMove.Y, Direction.NO_DIRECTION, i); if (reservationTable.Contains(queryTimedMove)) valid = false; } @@ -179,8 +179,8 @@ private bool singleAgentAStar(AgentState agent) { this.paths[agent.agent.agentNum] = new SinglePlan(node); reservePath(node); - totalcost += node.lastMove.time; - parked.Add(new Move(node.lastMove.X, node.lastMove.Y, Direction.NO_DIRECTION), node.lastMove.time); + totalcost += node.lastMove.Time; + parked.Add(new Move(node.lastMove.X, node.lastMove.Y, Direction.NO_DIRECTION), node.lastMove.Time); return true; } } @@ -233,7 +233,7 @@ private bool isValidMove(TimedMove move) if (move.IsColliding(this.reservationTable)) return false; this.queryMove.Setup(move.X, move.Y, Direction.NO_DIRECTION); - if (parked.ContainsKey(this.queryMove) && parked[this.queryMove] <= move.time) + if (parked.ContainsKey(this.queryMove) && parked[this.queryMove] <= move.Time) return false; return true; } diff --git a/IndependenceDetection.cs b/IndependenceDetection.cs index 8b22e9a..9e7fead 100644 --- a/IndependenceDetection.cs +++ b/IndependenceDetection.cs @@ -955,7 +955,7 @@ protected void IncrementConflictCountsAtGoal(IndependenceDetectionAgentsGroup gr var afterGoal = new TimedMove(group.allAgentsState[i].agent.Goal.X, group.allAgentsState[i].agent.Goal.Y, Direction.Wait, time: 0); for (int time = group.GetPlan().GetSize(); time < CAT.GetMaxPlanSize(); time++) { - afterGoal.time = time; + afterGoal.Time = time; afterGoal.IncrementConflictCounts(CAT, this.conflictCountsPerGroup[group.groupNum], this.conflictTimesPerGroup[group.groupNum]); diff --git a/MDD.cs b/MDD.cs index 5aac661..8533c7c 100644 --- a/MDD.cs +++ b/MDD.cs @@ -150,7 +150,7 @@ public MDD(MDD other, CbsConstraint newConstraint) public MDD(MDD other, ISet reserved) : this(other) { - ISet times_of_reservations = reserved.Select(move => move.time).ToHashSet(); + ISet times_of_reservations = reserved.Select(move => move.Time).ToHashSet(); var toDelete = new List(); foreach (int time in times_of_reservations) { @@ -264,12 +264,12 @@ private List GetAllChildren(MDDNode parent, int heuristicBound, int num continue; } - if (mustConstraints != null && move.time < mustConstraints.Length && // There may be a constraint on the timestep + if (mustConstraints != null && move.Time < mustConstraints.Length && // There may be a constraint on the timestep // of the generated node - mustConstraints[move.time] != null && - mustConstraints[move.time].ContainsKey(this.agentNum)) // This agent has a must constraint for this time step + mustConstraints[move.Time] != null && + mustConstraints[move.Time].ContainsKey(this.agentNum)) // This agent has a must constraint for this time step { - if (mustConstraints[move.time][this.agentNum].Equals(move) == false) + if (mustConstraints[move.Time][this.agentNum].Equals(move) == false) continue; } diff --git a/MddMatchAndPrune.cs b/MddMatchAndPrune.cs index 7dd252d..353511d 100644 --- a/MddMatchAndPrune.cs +++ b/MddMatchAndPrune.cs @@ -218,7 +218,7 @@ class MddMatchAndPruneState public MddMatchAndPruneState(MDDNode[] allPositions) { this.allPositions = allPositions; - this.stateLevel = allPositions[0].move.time; + this.stateLevel = allPositions[0].move.Time; this.parents = new LinkedList(); this.childrens = new LinkedList(); } @@ -230,7 +230,7 @@ public MddMatchAndPruneState(LinkedListNode[] allSuccessors) { allPositions[i] = allSuccessors[i].Value; } - this.stateLevel = allPositions[0].move.time; + this.stateLevel = allPositions[0].move.Time; this.parents = new LinkedList(); this.childrens = new LinkedList(); } diff --git a/Move.cs b/Move.cs index ed9ff95..644b0c3 100644 --- a/Move.cs +++ b/Move.cs @@ -177,7 +177,7 @@ public Move GetMoveWithoutDirection() /// /// Changes this move to represent its opposite. Warning: Changes the hash. Not safe after the object is put in a hash table! /// - public void setOppositeMove() + public void SetOppositeMove() { X += directionToOppositeDeltas[(int)Direction, 0]; Y += directionToOppositeDeltas[(int)Direction, 1]; diff --git a/ProblemInstance.cs b/ProblemInstance.cs index 473e677..5e8eee4 100644 --- a/ProblemInstance.cs +++ b/ProblemInstance.cs @@ -258,7 +258,7 @@ public SinglePlan GetSingleAgentOptimalPlan(AgentState agentState) LinkedList moves = new LinkedList(); int agentNum = agentState.agent.agentNum; TimedMove current = agentState.lastMove; // The starting position - int time = current.time; + int time = current.Time; while (true) { diff --git a/Run.cs b/Run.cs index 404501e..114bc19 100644 --- a/Run.cs +++ b/Run.cs @@ -676,7 +676,7 @@ public ProblemInstance GenerateProblemInstance(int gridSize, int agentsNum, int !goals[aStart[i].lastMove.X][aStart[i].lastMove.Y]) // this spot isn't another agent's goal break; else - aStart[i].lastMove.setOppositeMove(); // Rollback + aStart[i].lastMove.SetOppositeMove(); // Rollback } goals[aStart[i].lastMove.X][aStart[i].lastMove.Y] = true; // Claim agent's new goal } @@ -685,7 +685,7 @@ public ProblemInstance GenerateProblemInstance(int gridSize, int agentsNum, int // Zero the agents' timesteps foreach (AgentState agentStart in aStart) { - agentStart.lastMove.time = 0; + agentStart.lastMove.Time = 0; } // TODO: There is some repetition here of previous instantiation of ProblemInstance. Think how to elegantly bypass this. @@ -786,7 +786,7 @@ public ProblemInstance GenerateDragonAgeProblemInstance(string mapFilePath, int !goals[agentStates[i].lastMove.X][agentStates[i].lastMove.Y]) // This spot isn't another agent's goal break; else - agentStates[i].lastMove.setOppositeMove(); // Rollback + agentStates[i].lastMove.SetOppositeMove(); // Rollback } goals[agentStates[i].lastMove.X][agentStates[i].lastMove.Y] = true; // Claim agent's new goal } @@ -794,7 +794,7 @@ public ProblemInstance GenerateDragonAgeProblemInstance(string mapFilePath, int // Zero the agents' timesteps foreach (AgentState agentStart in agentStates) - agentStart.lastMove.time = 0; + agentStart.lastMove.Time = 0; return problem; } diff --git a/TimedMove.cs b/TimedMove.cs index 356a9b3..47367bb 100644 --- a/TimedMove.cs +++ b/TimedMove.cs @@ -7,14 +7,12 @@ namespace mapf; /// public class TimedMove : Move { - public int time; - -// public static const int FOREVER_AFTER = int.MaxValue + public int Time { get; set; } public TimedMove(int x, int y, Direction direction, int time) : base(x, y, direction) { - this.time = time; + Time = time; } /// @@ -31,7 +29,7 @@ public TimedMove(int x, int y, Direction direction, int time) foreach (Direction op in directions) { yield return new TimedMove(this.X + Move.directionToDeltas[(int)op, 0], - this.Y + Move.directionToDeltas[(int)op, 1], op, this.time + 1); + this.Y + Move.directionToDeltas[(int)op, 1], op, this.Time + 1); } } @@ -42,36 +40,36 @@ public TimedMove(int x, int y, Direction direction, int time) public override void Update(Direction direction) { base.Update(direction); - this.time += 1; + Time += 1; } public TimedMove(Move cpy, int time) : base(cpy) { - this.time = time; + Time = time; } public TimedMove() { } public TimedMove(TimedMove cpy) : base(cpy) { - this.time = cpy.time; + Time = cpy.Time; } public override bool Equals(object obj) { if (obj == null) return false; - if (this.time != ((TimedMove)obj).time) + if (Time != ((TimedMove)obj).Time) return false; //return base.Equals(obj); // Begin copied code of base to avoid a method call Move that = (Move)obj; - return (this.X == that.X && this.Y == that.Y && - ((this.Direction == Direction.NO_DIRECTION) || (that.Direction == Direction.NO_DIRECTION) || - (this.Direction == that.Direction))); + return (X == that.X && Y == that.Y && + ((Direction == Direction.NO_DIRECTION) || (that.Direction == Direction.NO_DIRECTION) || + (Direction == that.Direction))); // End copied code of base } @@ -86,13 +84,13 @@ public override int GetHashCode() hash = 23 * hash + X; hash = 23 * hash + Y; // End copied code of base - return hash * 3 + this.time; + return hash * 3 + Time; } } public new TimedMove GetMoveWithoutDirection() { - TimedMove copy = new TimedMove(this); + TimedMove copy = new(this); copy.Direction = Direction.NO_DIRECTION; return copy; } @@ -104,16 +102,11 @@ public override int GetHashCode() /// 1. Head on collision /// 2. When other moves target the same location. /// - /// - /// - public bool IsColliding(TimedMove other) - { - return IsColliding(other.X, other.Y, other.Direction, other.time); - } + public bool IsColliding(TimedMove other) => IsColliding(other.X, other.Y, other.Direction, other.Time); public bool IsColliding(int other_x, int other_y, Direction other_direction, int time) { - if (this.time != time) + if (Time != time) return false; return base.IsColliding(other_x, other_y, other_direction); @@ -122,48 +115,41 @@ public bool IsColliding(int other_x, int other_y, Direction other_direction, int /// /// Reimplemented to avoid creating temporary Move objects /// - /// public new TimedMove GetOppositeMove() { if (Direction == Direction.Wait || Direction == Direction.NO_DIRECTION) return this; - return new TimedMove(this.X + Move.directionToOppositeDeltas[(int)Direction, 0], - this.Y + directionToOppositeDeltas[(int)Direction, 1], - directionToOppositeDirection[(int)Direction], this.time); + return new TimedMove(X + Move.directionToOppositeDeltas[(int)Direction, 0], + Y + directionToOppositeDeltas[(int)Direction, 1], + directionToOppositeDirection[(int)Direction], Time); } /// /// Isn't used anywhere /// - /// - /// - public void setup(Move cpy, int time) + public void Setup(Move cpy, int time) { base.Setup(cpy); - this.time = time; + Time = time; } /// /// /// /// - public void setup(TimedMove cpy) + public void Setup(TimedMove cpy) { base.Setup(cpy); - this.time = cpy.time; + Time = cpy.Time; } /// /// Almost isn't used anywhere /// - /// - /// - /// - /// - public void setup(int x, int y, Direction direction, int time) + public void Setup(int x, int y, Direction direction, int time) { base.Setup(x, y, direction); - this.time = time; + Time = time; } public bool IsColliding(ICollection moves) @@ -179,13 +165,13 @@ public bool IsColliding(ICollection moves) if (Constants.ALLOW_HEAD_ON_COLLISION == false) { - this.setOppositeMove(); + SetOppositeMove(); if (moves.Contains(this)) // Check direction too now { - this.setOppositeMove(); + SetOppositeMove(); return true; } - this.setOppositeMove(); + SetOppositeMove(); } return false; @@ -206,13 +192,13 @@ public bool IsColliding(IReadOnlyDictionary timedMovesToAgentID) if (Constants.ALLOW_HEAD_ON_COLLISION == false) { - this.setOppositeMove(); + SetOppositeMove(); if (timedMovesToAgentID.ContainsKey(this)) // Check direction too now { - this.setOppositeMove(); + SetOppositeMove(); return true; } - this.setOppositeMove(); + SetOppositeMove(); } return false; @@ -238,8 +224,7 @@ public List GetColliding(IReadOnlyDictionary timedMovesToAg this.Direction = direction; if (timedMovesToAgentIndex.ContainsKey(this)) { - if (ans == null) - ans = new List(4); + ans ??= new List(4); ans.Add(timedMovesToAgentIndex[this]); } } @@ -247,14 +232,13 @@ public List GetColliding(IReadOnlyDictionary timedMovesToAg if (Constants.ALLOW_HEAD_ON_COLLISION == false) { - this.setOppositeMove(); + this.SetOppositeMove(); if (timedMovesToAgentIndex.ContainsKey(this)) // Check direction too now { - if (ans == null) - ans = new List(1); + ans ??= new List(1); ans.Add(timedMovesToAgentIndex[this]); } - this.setOppositeMove(); + this.SetOppositeMove(); } if (ans != null) @@ -263,7 +247,7 @@ public List GetColliding(IReadOnlyDictionary timedMovesToAg return TimedMove.emptyList; } - private static readonly List emptyList = new List(0); + private static readonly List emptyList = []; /// /// Gets a dictionary mapping TimedMoves to the agents that already made them @@ -286,7 +270,7 @@ public IReadOnlyList GetColliding(ConflictAvoidanceTable CAT) if (CAT.ContainsKey(this)) { if (ans == null) - ans = new List(CAT[this]); + ans = [.. CAT[this]]; else ans.AddRange(CAT[this]); } @@ -295,15 +279,15 @@ public IReadOnlyList GetColliding(ConflictAvoidanceTable CAT) if (Constants.ALLOW_HEAD_ON_COLLISION == false) { - this.setOppositeMove(); + SetOppositeMove(); if (CAT.ContainsKey(this)) // Check direction too now { if (ans == null) - ans = new List(CAT[this]); + ans = [.. CAT[this]]; else ans.AddRange(CAT[this]); } - this.setOppositeMove(); + SetOppositeMove(); } if (ans != null) @@ -315,7 +299,7 @@ public IReadOnlyList GetColliding(ConflictAvoidanceTable CAT) public void IncrementConflictCounts(ConflictAvoidanceTable conflictAvoidance, Dictionary conflictCounts, Dictionary> conflictTimes) { - IReadOnlyList colliding = this.GetColliding(conflictAvoidance); + IReadOnlyList colliding = GetColliding(conflictAvoidance); foreach (int agentNum in colliding) { if (conflictCounts.ContainsKey(agentNum) == false) @@ -323,9 +307,9 @@ public void IncrementConflictCounts(ConflictAvoidanceTable conflictAvoidance, else conflictCounts[agentNum] += 1; if (conflictTimes.ContainsKey(agentNum) == false) - conflictTimes[agentNum] = new List(4) { this.time }; + conflictTimes[agentNum] = new List(4) { Time }; else - conflictTimes[agentNum].Add(this.time); + conflictTimes[agentNum].Add(Time); } } } diff --git a/WorldState.cs b/WorldState.cs index b60c09e..099a00c 100644 --- a/WorldState.cs +++ b/WorldState.cs @@ -76,7 +76,7 @@ public class WorldState : IComparable, IBinaryHeapItem, IHeuris public WorldState(AgentState[] allAgentsState, int minDepth = -1, int minCost = -1, MDDNode mddNode = null) { this.allAgentsState = allAgentsState.ToArray(); - this.makespan = allAgentsState.Max(state => state.lastMove.time); // We expect to only find at most two G values within the agent group + this.makespan = allAgentsState.Max(state => state.lastMove.Time); // We expect to only find at most two G values within the agent group this.CalculateG(); // G not necessarily zero when solving a partially solved problem. this.primaryTieBreaker = 0; this.secondaryTieBreaker = 0; From 9174e2564f7d28b666c9c6f61404dac9f63e0b30 Mon Sep 17 00:00:00 2001 From: Artem Kliatchkine Date: Mon, 5 May 2025 13:22:23 +0200 Subject: [PATCH 03/14] Using BitMatrix for grid (memory advantage) --- BitMatrix.cs | 34 ++++++++++++++ ProblemInstance.cs | 111 ++++++++++++++++++++++----------------------- Run.cs | 24 +++++----- 3 files changed, 98 insertions(+), 71 deletions(-) create mode 100644 BitMatrix.cs diff --git a/BitMatrix.cs b/BitMatrix.cs new file mode 100644 index 0000000..5218bbc --- /dev/null +++ b/BitMatrix.cs @@ -0,0 +1,34 @@ +using System.Collections; + +namespace mapf; + +/// +/// BitArray-based matrix for storing boolean values. +/// +public class BitMatrix +{ + private readonly BitArray[] _bitArrays; + + public BitMatrix(int cols, int rows) + { + _bitArrays = new BitArray[cols]; + for (int i = 0; i < cols; i++) + { + _bitArrays[i] = new BitArray(rows); + } + } + + public int RowsCount => _bitArrays[0].Length; + + public int ColumnsCount => _bitArrays.Length; + + public void Set(int col, int row, bool value) => _bitArrays[col][row] = value; + + public bool Get(int col, int row) => _bitArrays[col][row]; + + public bool this[int col, int row] + { + get => Get(col, row); + set => Set(col, row, value); + } +} diff --git a/ProblemInstance.cs b/ProblemInstance.cs index 5e8eee4..5f619b7 100644 --- a/ProblemInstance.cs +++ b/ProblemInstance.cs @@ -26,7 +26,7 @@ public class ProblemInstance /// /// Contains true at [x][y] if cell (x,y) is an obstacle /// - public bool[][] grid; + public BitMatrix grid; /// /// We keep a reference to the array of agents in the original problem. @@ -92,7 +92,7 @@ public ProblemInstance Subproblem(AgentState[] selectedAgents) { // Notice selected agents may actually be a completely different set of agents. // Not copying instance id. This isn't the same problem. - ProblemInstance subproblemInstance = new ProblemInstance(this.parameters); + ProblemInstance subproblemInstance = new(this.parameters); subproblemInstance.Init(selectedAgents, this.grid, (int)this.numObstacles, (int)this.numLocations, this.cardinality); subproblemInstance.singleAgentOptimalCosts = this.singleAgentOptimalCosts; // Each subproblem knows every agent's single shortest paths so this.singleAgentOptimalCosts[agent_num] would easily work subproblemInstance.singleAgentOptimalMoves = this.singleAgentOptimalMoves; @@ -110,19 +110,26 @@ public ProblemInstance Subproblem(AgentState[] selectedAgents) /// /// /// - public void Init(AgentState[] agentStartStates, bool[][] grid, int nObstacles=-1, + public void Init(AgentState[] agentStartStates, BitMatrix grid, int nObstacles=-1, int nLocations=-1, int[,] cardinality=null) { agents = agentStartStates; this.grid = grid; - + if (nObstacles == -1) - numObstacles = (uint)grid.Sum(row => row.Count(x => x)); + { + for (int i = 0; i < grid.ColumnsCount; i++) + for (int j = 0; j < grid.RowsCount; j++) + { + if (grid[i, j]) + numObstacles++; + } + } else numObstacles = (uint)nObstacles; if (nLocations == -1) - numLocations = ((uint)(grid.Length * grid[0].Length)) - numObstacles; + numLocations = ((uint)(grid.ColumnsCount * grid.RowsCount)) - numObstacles; else numLocations = (uint)nLocations; @@ -287,20 +294,14 @@ public int GetNumOfAgents() /// /// Utility function that returns the x dimension of the grid /// - public int GetMaxX() - { - return this.grid.Length; - } + public int GetMaxX() => grid.ColumnsCount; /// /// Utility function that returns the y dimension of the grid /// - public int GetMaxY() - { - return this.grid[0].Length; - } + public int GetMaxY() => grid.RowsCount; - private static bool[][] readMapFile(string mapFilePath) + private static BitMatrix readMapFile(string mapFilePath) { using (TextReader input = new StreamReader(mapFilePath)) { @@ -313,9 +314,9 @@ private static bool[][] readMapFile(string mapFilePath) } } - private static bool[][] readBenchmarkMap(TextReader input, string line) + private static BitMatrix readBenchmarkMap(TextReader input, string line) { - bool[][] grid; + string[] lineParts; int maxX, maxY; // Read grid dimensions @@ -329,7 +330,7 @@ private static bool[][] readBenchmarkMap(TextReader input, string line) Trace.Assert(lineParts.Length == 2); Trace.Assert(lineParts[0].Equals("width")); maxX = int.Parse(lineParts[1]); // The width is the number of columns - grid = new bool[maxY][]; + BitMatrix grid = new(maxY, maxX); line = input.ReadLine(); Trace.Assert(line.StartsWith("map")); @@ -338,40 +339,38 @@ private static bool[][] readBenchmarkMap(TextReader input, string line) // Read grid for (int i = 0; i < maxY; i++) { - grid[i] = new bool[maxX]; line = input.ReadLine(); for (int j = 0; j < maxX; j++) { cell = line[j]; if (cell == '@' || cell == 'O' || cell == 'T' || cell == 'W' /* Water isn't traversable from land */) - grid[i][j] = true; + grid[i, j] = true; else - grid[i][j] = false; + grid[i, j] = false; } } return grid; } - private static bool[][] readLironMap(TextReader input, string line) { + private static BitMatrix readLironMap(TextReader input, string line) { string[] lineParts; lineParts = line.Split(','); int maxX = int.Parse(lineParts[0]); int maxY = int.Parse(lineParts[1]); - bool[][] grid = new bool[maxX][]; + BitMatrix grid = new(maxX, maxY); char cell; // Read grid for (int i = 0; i < maxX; i++) { - grid[i] = new bool[maxY]; line = input.ReadLine(); for (int j = 0; j < maxY; j++) { cell = line[j]; if (cell == '1') - grid[i][j] = true; + grid[i, j] = true; else - grid[i][j] = false; + grid[i, j] = false; } } return grid; @@ -407,7 +406,7 @@ public static ProblemInstance Import(string filePath, string mapFilePath = null) mapfileNameWithoutExtension = Path.GetFileNameWithoutExtension(mapFilePath); } - bool[][] grid = readMapFile(mapFilePath); + BitMatrix grid = readMapFile(mapFilePath); string line; string[] lineParts; @@ -446,7 +445,7 @@ public static ProblemInstance Import(string filePath, string mapFilePath = null) } // Generate the problem instance - ProblemInstance instance = new ProblemInstance(); + ProblemInstance instance = new(); instance.Init(states, grid); instance.instanceId = instanceId; instance.gridName = mapfileNameWithoutExtension; @@ -484,11 +483,11 @@ public static ProblemInstance Import(string filePath, string mapFilePath = null) mapfileName = Path.GetFileNameWithoutExtension(mapFilePath); - bool[][] grid = readMapFile(mapFilePath); + BitMatrix grid = readMapFile(mapFilePath); string line; string[] lineParts; - List stateList = new List(); + List stateList = []; using (TextReader input = new StreamReader(filePath)) { // Read the format version number @@ -521,9 +520,9 @@ public static ProblemInstance Import(string filePath, string mapFilePath = null) mapFileNameRow = lineParts[1]; Trace.Assert((mapfileName == mapFileNameRow) || (mapfileName == mapFileNameRow + ".map"), "Row's map name doesn't match map's name"); // Second option is for Omri's scenarios mapRows = int.Parse(lineParts[3]); - Trace.Assert(mapRows == grid.Length, "Row's number of grid rows doesn't match map's"); + Trace.Assert(mapRows == grid.ColumnsCount, "Column's number of grid rows doesn't match map's"); mapCols = int.Parse(lineParts[2]); - Trace.Assert(mapCols == grid[0].Length, "Row's number of grid columns doesn't match map's"); + Trace.Assert(mapCols == grid.RowsCount, "Row's number of grid columns doesn't match map's"); // Read in the start and goal coordinates. // Note that at first glance, https://movingai.com/benchmarks/formats.html seems to indicate a reverse order of for Y,X, @@ -531,11 +530,11 @@ public static ProblemInstance Import(string filePath, string mapFilePath = null) // as (column,row) and we invert it. startY = int.Parse(lineParts[4]); startX = int.Parse(lineParts[5]); - if (grid[startX][startY]) + if (grid[startX,startY]) throw new Exception($"Agent {agentNum} start location ({startX},{startY}) is on an obstacle"); goalY = int.Parse(lineParts[6]); goalX = int.Parse(lineParts[7]); - if (grid[goalX][goalY]) + if (grid[goalX,goalY]) throw new Exception($"Agent {agentNum} goal location ({goalX},{goalY}) is on an obstacle"); optimalCost = double.Parse(lineParts[8]); agent = new Agent(goalX, goalY, agentNum); @@ -546,8 +545,8 @@ public static ProblemInstance Import(string filePath, string mapFilePath = null) } // Generate the problem instance - ProblemInstance instance = new ProblemInstance(); - instance.Init(stateList.ToArray(), grid); + ProblemInstance instance = new(); + instance.Init([.. stateList], grid); instance.instanceId = instanceId; instance.gridName = mapfileName; instance.instanceName = Path.GetFileName(filePath); @@ -581,20 +580,19 @@ public static ProblemInstance Import(string filePath, string mapFilePath = null) lineParts = line.Split(','); int maxX = int.Parse(lineParts[0]); int maxY = int.Parse(lineParts[1]); - bool[][] grid = new bool[maxX][]; + BitMatrix grid = new(maxX, maxY); // Read grid 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[j]; if (cell == '@' || cell == 'O' || cell == 'T' || cell == 'W' /* Water isn't traversable from land */) - grid[i][j] = true; + grid[i, j] = true; else - grid[i][j] = false; + grid[i, j] = false; } } @@ -660,7 +658,7 @@ public void Export(string fileName, string mapFileName = null) foreach (var agentState in this.agents) { // Output all agent as block 1, with optimal cost -1 - output.WriteLine($"{1}\t{mapFileName}\t{grid[0].Length}\t{grid.Length}\t{agentState.lastMove.Y}\t{agentState.lastMove.X}\t{agentState.agent.Goal.Y}\t{agentState.agent.Goal.X}\t{-1}"); + output.WriteLine($"{1}\t{mapFileName}\t{grid.RowsCount}\t{grid.ColumnsCount}\t{agentState.lastMove.Y}\t{agentState.lastMove.X}\t{agentState.agent.Goal.Y}\t{agentState.agent.Goal.X}\t{-1}"); } } else if (fileName.EndsWith(".agents")) @@ -679,13 +677,13 @@ public void Export(string fileName, string mapFileName = null) // Output the grid output.WriteLine("Grid:"); - output.WriteLine($"{this.grid.Length},{this.grid[0].Length}"); + output.WriteLine($"{this.grid.ColumnsCount},{this.grid.RowsCount}"); - for (int i = 0; i < this.grid.Length; i++) + for (int i = 0; i < this.grid.ColumnsCount; i++) { - for (int j = 0; j < this.grid[0].Length; j++) + for (int j = 0; j < this.grid.RowsCount; j++) { - if (this.grid[i][j] == true) + if (this.grid[i, j] == true) output.Write('@'); else output.Write('.'); @@ -721,23 +719,22 @@ public int GetCardinality(Move move) private void PrecomputeCardinality() { - cardinality = new int[grid.Length, grid[0].Length]; + cardinality = new int[grid.ColumnsCount, grid.RowsCount]; int maxCardinality = 0; - for (uint i = 0; i < grid.Length; ++i) - for (uint j = 0; j < grid[i].Length; ++j) - { - if (grid[i][j]) - cardinality[i, j] = -1; - else - cardinality[i, j] = maxCardinality++; - } + for (int i = 0; i < grid.ColumnsCount; ++i) + for (int j = 0; j < grid.RowsCount; ++j) + { + if (grid[i,j]) + cardinality[i, j] = -1; + else + cardinality[i, j] = maxCardinality++; + } } /// /// Check if the tile is valid, i.e. in the grid and without an obstacle. /// NOT checking the direction. A Move could be declared valid even if it came to an edge tile from outside the grid! /// - /// /// True if the given location is a valid grid location with no obstacles public bool IsValid(Move aMove) { @@ -747,8 +744,6 @@ public bool IsValid(Move aMove) /// /// Also checks if the move is illegal /// - /// - /// public bool IsValid(TimedMove toCheck) { if (IsValidTile(toCheck.X, toCheck.Y) == false) @@ -763,7 +758,7 @@ public bool IsValidTile(int x, int y) return false; if (y < 0 || y >= GetMaxY()) return false; - return !grid[x][y]; + return !grid[x, y]; } public override string ToString() diff --git a/Run.cs b/Run.cs index 114bc19..0db2bff 100644 --- a/Run.cs +++ b/Run.cs @@ -621,22 +621,21 @@ public ProblemInstance GenerateProblemInstance(int gridSize, int agentsNum, int int y; Agent[] aGoals = new Agent[agentsNum]; AgentState[] aStart = new AgentState[agentsNum]; - bool[][] grid = new bool[gridSize][]; + BitMatrix grid = new(gridSize, gridSize); bool[][] goals = new bool[gridSize][]; // Generate a random grid for (int i = 0; i < gridSize; i++) { - grid[i] = new bool[gridSize]; goals[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 + if (grid[x, y]) // Already an obstacle i--; - grid[x][y] = true; + grid[x, y] = true; } // Choose random goal locations @@ -644,7 +643,7 @@ public ProblemInstance GenerateProblemInstance(int gridSize, int agentsNum, int { x = rand.Next(gridSize); y = rand.Next(gridSize); - if (goals[x][y] || grid[x][y]) + if (goals[x][y] || grid[x, y]) i--; else { @@ -723,19 +722,18 @@ public ProblemInstance GenerateDragonAgeProblemInstance(string mapFilePath, int int maxY = int.Parse(lineParts[1]); line = input.ReadLine(); Trace.Assert(line.StartsWith("map")); - bool[][] grid = new bool[maxX][]; + BitMatrix grid = new(maxX, maxY); 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[j]; if (cell == '@' || cell == 'O' || cell == 'T' || cell == 'W' /* Water isn't traversable from land */) - grid[i][j] = true; + grid[i, j] = true; else - grid[i][j] = false; + grid[i, j] = false; } } @@ -753,7 +751,7 @@ public ProblemInstance GenerateDragonAgeProblemInstance(string mapFilePath, int { x = rand.Next(maxX); y = rand.Next(maxY); - if (goals[x][y] || grid[x][y]) + if (goals[x][y] || grid[x, y]) i--; else { @@ -768,7 +766,7 @@ public ProblemInstance GenerateDragonAgeProblemInstance(string mapFilePath, int agentStates[i] = new AgentState(agentGoals[i].Goal.X, agentGoals[i].Goal.Y, agentGoals[i]); } - ProblemInstance problem = new ProblemInstance(); + ProblemInstance problem = new(); problem.gridName = Path.GetFileNameWithoutExtension(mapFilePath); problem.Init(agentStates, grid); @@ -1099,9 +1097,9 @@ private void PrintProblemStatistics(ProblemInstance instance) // Grid Name col: this.resultsWriter.Write(instance.gridName + RESULTS_DELIMITER); // Grid Rows col: - this.resultsWriter.Write(instance.grid.Length + RESULTS_DELIMITER); + this.resultsWriter.Write(instance.grid.ColumnsCount + RESULTS_DELIMITER); // Grid Columns col: - this.resultsWriter.Write(instance.grid[0].Length + RESULTS_DELIMITER); + this.resultsWriter.Write(instance.grid.RowsCount + RESULTS_DELIMITER); // Scenario/instance Name col: this.resultsWriter.Write(instance.instanceName + RESULTS_DELIMITER); // Num Of Agents col: From 471b0ec42f6d487d363051744c0efe67bd4dab4b Mon Sep 17 00:00:00 2001 From: Artem Kliatchkine Date: Mon, 5 May 2025 16:43:02 +0200 Subject: [PATCH 04/14] Code refresh. --- A_Star.cs | 66 +- ApproximateMvcHeuristicForCbs.cs | 22 +- CBS.cs | 1459 ++++++++++++++--------------- CbsCacheEntry.cs | 2 +- CbsHeuristicForAStar.cs | 30 +- CbsNode.cs | 1493 +++++++++++++++--------------- DynamicLazyOpenList.cs | 14 +- DynamicRationalLazyOpenList.cs | 24 +- EPEA_Star.cs | 4 +- EnumeratedPDB.cs | 6 +- IHeuristicSearchNode.cs | 8 +- MddPruningHeuristicForCbs.cs | 28 +- MvcHeuristicForCbs.cs | 32 +- PEA_Star.cs | 10 +- Run.cs | 12 +- WorldState.cs | 40 +- WorldStateForPartialExpansion.cs | 6 +- 17 files changed, 1596 insertions(+), 1660 deletions(-) diff --git a/A_Star.cs b/A_Star.cs index 4c538a4..06b1a08 100644 --- a/A_Star.cs +++ b/A_Star.cs @@ -109,9 +109,9 @@ public virtual void Setup(ProblemInstance problemInstance, int minDepth, Run run mddRoot = mdd.levels[0].First.Value; } WorldState root = this.CreateSearchRoot(minDepth, minCost, mddRoot); - root.h = (int)this.heuristic.h(root); // g was already set in the constructor - if (root.f < minCost) - root.h = minCost - root.g; // Will be propagated to children with BPMX as needed + root.H = (int)this.heuristic.h(root); // g was already set in the constructor + if (root.F < minCost) + root.H = minCost - root.G; // Will be propagated to children with BPMX as needed this.openList.Add(root); this.closedList.Add(root, root); this.ClearPrivateStatistics(); @@ -410,14 +410,14 @@ public virtual bool Solve() { totalCost = (int) Constants.SpecialCosts.TIMEOUT_COST; Console.WriteLine("Out of time"); - this.solutionDepth = openList.Peek().g + openList.Peek().h - initialEstimate; // A minimum estimate, assuming h is admissible + this.solutionDepth = openList.Peek().G + openList.Peek().H - initialEstimate; // A minimum estimate, assuming h is admissible this.Clear(); return false; } WorldState currentNode = openList.Remove(); - if (currentNode.f > this.maxSolutionCost) // A late heuristic application may have increased the node's cost + if (currentNode.F > this.maxSolutionCost) // A late heuristic application may have increased the node's cost { continue; // This will exhaust the open list, assuming Fs of nodes chosen for expansions @@ -460,13 +460,13 @@ public virtual bool Solve() (this.openList is DynamicRationalLazyOpenList) == false && currentNode.minGoalCost == -1 // If we were given a minGoalCost (by ID, for example), then at some point we're going to use up the boost to the h-value we gave the root ) - if (currentNode.f < lastF) - Trace.Assert(false, $"A* node with decreasing F: {currentNode.f} < {lastF}."); + if (currentNode.F < lastF) + Trace.Assert(false, $"A* node with decreasing F: {currentNode.F} < {lastF}."); else { // TODO: Record the max F. Assert that the goal's F isn't smaller than it. } - lastF = currentNode.f; + lastF = currentNode.F; lastNode = currentNode; // Calculate expansion delay @@ -556,16 +556,16 @@ public virtual void Expand(WorldState node) return; currentNode.makespan++; currentNode.CalculateG(); - currentNode.h = (int)this.heuristic.h(currentNode); + currentNode.H = (int)this.heuristic.h(currentNode); // Boost h based on minGoalCost - if (currentNode.g < currentNode.minGoalCost) + if (currentNode.G < currentNode.minGoalCost) { - if (currentNode.h == 0) // Agent is at the goal, only too early - currentNode.h = 2; // Otherwise waiting at goal would expand to waiting at the goal for the same too low cost, + if (currentNode.H == 0) // Agent is at the goal, only too early + currentNode.H = 2; // Otherwise waiting at goal would expand to waiting at the goal for the same too low cost, // which would expand to waiting at the goal, etc. // +2 because you need a step out of the goal and another step into it. - currentNode.h = Math.Max(currentNode.h, currentNode.minGoalCost - currentNode.g); // Like a Manhattan Distance on the time dimension + currentNode.H = Math.Max(currentNode.H, currentNode.minGoalCost - currentNode.G); // Like a Manhattan Distance on the time dimension // TODO: Add a statistic for when the H was increased thanks to the minGoalCost } @@ -579,14 +579,14 @@ public virtual void Expand(WorldState node) { // Reverse Path-Max (operators are invertible) - BPMX (Felner et al. 2005) WorldState parent = node; - var childWithMaxH = finalGeneratedNodes.MaxByKeyFunc(child => child.h); - int maxChildH = childWithMaxH.h; - int deltaGOfChildWithMaxH = childWithMaxH.g - parent.g; - if (parent.h < maxChildH - deltaGOfChildWithMaxH) + var childWithMaxH = finalGeneratedNodes.MaxByKeyFunc(child => child.H); + int maxChildH = childWithMaxH.H; + int deltaGOfChildWithMaxH = childWithMaxH.G - parent.G; + if (parent.H < maxChildH - deltaGOfChildWithMaxH) { int newParentH = maxChildH - deltaGOfChildWithMaxH; - parent.hBonus += newParentH - parent.h; - parent.h = newParentH; // Also good for partial expansion algs that reinsert the expanded node into the open list + parent.HBonus += newParentH - parent.H; + parent.H = newParentH; // Also good for partial expansion algs that reinsert the expanded node into the open list // (in addition to aiding the forward Path-Max). ++bpmxBoosts; // FIXME: Code duplication with Forward Path-Max @@ -594,12 +594,12 @@ public virtual void Expand(WorldState node) // Forward Path-Max foreach (var child in finalGeneratedNodes) { - int deltaG = child.g - parent.g; // == (parent.g + c(parent, current)) - parent.g == c(parent, current) - if (child.h < parent.h - deltaG) + int deltaG = child.G - parent.G; // == (parent.g + c(parent, current)) - parent.g == c(parent, current) + if (child.H < parent.H - deltaG) { - int newChildH = parent.h - deltaG; - child.hBonus += newChildH - child.h; - child.h = newChildH; + int newChildH = parent.H - deltaG; + child.HBonus += newChildH - child.H; + child.H = newChildH; ++bpmxBoosts; } } @@ -1067,7 +1067,7 @@ public void Setup(ProblemInstance problemInstance, Run runner, ConflictAvoidance /// Returns whether the node was inserted into the open list. protected virtual bool ProcessGeneratedNode(WorldState currentNode) { - if (currentNode.f <= this.maxSolutionCost) + if (currentNode.F <= this.maxSolutionCost) // Assuming h is an admissible heuristic, no need to generate nodes that won't get us to the goal // within the budget { @@ -1109,7 +1109,7 @@ protected virtual bool ProcessGeneratedNode(WorldState currentNode) if (this.mstar) { - if (currentNode.g == inClosedList.g) + if (currentNode.G == inClosedList.G) { // Unite backpropagation sets and collision sets of inClosedList and currentNode. // Notice only only of them is going to survive this method. @@ -1139,16 +1139,16 @@ protected virtual bool ProcessGeneratedNode(WorldState currentNode) // Since the nodes are equal, give them both the max of their H bool improvedHOfThisNode = false; bool improvedHOfOldNode = false; - if (currentNode.h < inClosedList.h) + if (currentNode.H < inClosedList.H) { - currentNode.hBonus += inClosedList.h - currentNode.h; - currentNode.h = inClosedList.h; + currentNode.HBonus += inClosedList.H - currentNode.H; + currentNode.H = inClosedList.H; improvedHOfThisNode = true; } - if (inClosedList.h < currentNode.h) + if (inClosedList.H < currentNode.H) { - inClosedList.hBonus += currentNode.h - inClosedList.h; - inClosedList.h = currentNode.h; + inClosedList.HBonus += currentNode.H - inClosedList.H; + inClosedList.H = currentNode.H; improvedHOfOldNode = true; } @@ -1174,7 +1174,7 @@ protected virtual bool ProcessGeneratedNode(WorldState currentNode) if (compareVal == -1 || // This node has smaller f, or preferred due to another consideration. // Since we equalised their h, a smaller f means smaller g. - (this.mstar && this.doMstarShuffle && currentNode.g == inClosedList.g)) // Enables re-trying a node with different paths for the agents + (this.mstar && this.doMstarShuffle && currentNode.G == inClosedList.G)) // Enables re-trying a node with different paths for the agents { this.reopened++; this.closedList.Remove(inClosedList); diff --git a/ApproximateMvcHeuristicForCbs.cs b/ApproximateMvcHeuristicForCbs.cs index a387bcc..1746f66 100644 --- a/ApproximateMvcHeuristicForCbs.cs +++ b/ApproximateMvcHeuristicForCbs.cs @@ -75,7 +75,7 @@ public uint h(CbsNode s) public uint h(CbsNode s, int target) { Debug.WriteLine($"Computing heuristic estimate for node hash {s.GetHashCode()}"); - if (target != int.MaxValue && target > s.totalInternalAgentsThatConflict) + if (target != int.MaxValue && target > s.TotalInternalAgentsThatConflict) { Debug.WriteLine($"Target estimate {target} was too high!"); this.targetClearlyTooHigh++; @@ -87,24 +87,24 @@ public uint h(CbsNode s, int target) int targetTimes2 = 2 * target; // Populate the cardinal conflict graph - foreach (var agentIndex in Enumerable.Range(0, s.singleAgentPlans.Length)) + foreach (var agentIndex in Enumerable.Range(0, s.SingleAgentPlans.Length)) { - if (s.conflictTimesPerAgent[agentIndex].Count == 0) + if (s.ConflictTimesPerAgent[agentIndex].Count == 0) continue; // Agent has no conflicts if (vertexCover.Contains(agentIndex)) // All its edges are already covered continue; - bool hasMdd = s.mddNarrownessValues[agentIndex] != null; + bool hasMdd = s.MDDNarrownessValues[agentIndex] != null; bool largeEnough = false; - foreach (int conflictingAgentNum in s.conflictTimesPerAgent[agentIndex].Keys) + foreach (int conflictingAgentNum in s.ConflictTimesPerAgent[agentIndex].Keys) { - int conflictingAgentIndex = s.agentNumToIndex[conflictingAgentNum]; + int conflictingAgentIndex = s.AgentNumToIndex[conflictingAgentNum]; if (conflictingAgentIndex < agentIndex) // check later continue; - bool otherHasMdd = s.mddNarrownessValues[conflictingAgentIndex] != null; + bool otherHasMdd = s.MDDNarrownessValues[conflictingAgentIndex] != null; bool addedToVC = false; - foreach (int conflictTime in s.conflictTimesPerAgent[agentIndex][conflictingAgentNum]) + foreach (int conflictTime in s.ConflictTimesPerAgent[agentIndex][conflictingAgentNum]) { if (hasMdd == false) { @@ -141,11 +141,11 @@ public uint h(CbsNode s, int target) break; } - s.minimumVertexCover = vertexCover.Count / 2; // The approximation is always even. + s.MinimumVertexCover = vertexCover.Count / 2; // The approximation is always even. if (target != int.MaxValue) { - if (s.minimumVertexCover >= target) + if (s.MinimumVertexCover >= target) { Debug.WriteLine($"Target estimate {target} reached"); this.targetReached++; @@ -157,7 +157,7 @@ public uint h(CbsNode s, int target) } } - return (uint)s.minimumVertexCover; + return (uint)s.MinimumVertexCover; } public void Init(ProblemInstance pi, List agents) diff --git a/CBS.cs b/CBS.cs index ae495c7..8673f84 100644 --- a/CBS.cs +++ b/CBS.cs @@ -3,10 +3,19 @@ using System.IO; using System.Diagnostics; using System.Linq; -using ExtensionMethods; namespace mapf; +public enum ConflictChoice : byte +{ + FIRST = 0, + MOST_CONFLICTING_SMALLEST_AGENTS, + CARDINAL_MDD, + CARDINAL_LOOKAHEAD, + CARDINAL_MDD_THEN_MERGE_EARLY_MOST_CONFLICTING_SMALLEST_GROUP, + CARDINAL_MDD_THEN_MERGE_EARLY_MOST_CONFLICTING_AND_SMALLEST_GROUP +} + /// /// Merges agents if they conflict more times than the given threshold in the CT nodes /// from the root to the current CT nodes only. @@ -16,154 +25,143 @@ public class CBS : ICbsSolver, IHeuristicSolver, IIndependenceDetection /// /// For each agent, map sets of constraints to MDDs /// - public Dictionary[] mddCache; + public Dictionary[] MDDCache { get; private set; } /// /// For each agent, map sets of constraints to a dictionary that /// maps each level (timestep) of its mdd to a narrowness degree. /// Non-narrow levels are omitted. /// - public Dictionary>[] mddNarrownessValuesCache; + public Dictionary>[] MDDNarrownessValuesCache { get; private set; } - protected ProblemInstance instance; - public OpenList openList; + private ProblemInstance _instance; + public OpenList OpenList { get; } /// /// Might as well be a HashSet. We don't need to retrieve from it. /// TODO: Consider a closedList for single agent paths under an unordered set of constraints. /// It would get more hits. /// - public Dictionary closedList; - protected ILazyHeuristic heuristic; - protected int highLevelExpanded; - protected int highLevelGenerated; - protected int closedListHits; - protected int partialExpansions; - protected int bypasses; - protected int nodesExpandedWithGoalCost; - protected int bypassLookAheadNodesCreated; - protected int cardinalLookAheadNodesCreated; - protected int conflictsBypassed; - protected int cardinalConflictSplits; - protected int semiCardinalConflictSplits; - protected int nonCardinalConflictSplits; - public int mddsBuilt; - public int mddsAdapted; - protected int restarts; - protected int pathMaxBoosts; - protected int reversePathMaxBoosts; - protected int pathMaxPlusBoosts; - protected int surplusNodesAvoided; - public int mddCacheHits; - public double timePlanningPaths; - public double timeBuildingMdds; + private readonly Dictionary _closedList = []; + private readonly ILazyHeuristic _heuristic; + private int _highLevelExpanded; + private int _highLevelGenerated; + private int _closedListHits; + private int _partialExpansions; + private int _bypasses; + private int _nodesExpandedWithGoalCost; + private int _bypassLookAheadNodesCreated; + private int _cardinalLookAheadNodesCreated; + private int _conflictsBypassed; + private int _cardinalConflictSplits; + private int _semiCardinalConflictSplits; + private int _nonCardinalConflictSplits; + public int MDDsBuilt { get; set; } + public int MDDsAdapted { get; set; } + private int _restarts; + private int _pathMaxBoosts; + private int _reversePathMaxBoosts; + private int _pathMaxPlusBoosts; + private int _surplusNodesAvoided; + public int MDDCacheHits { get; set; } + public double TimePlanningPaths { get; set; } + public double TimeBuildingMdds { get; set; } // TODO: Count shuffles - protected int accHLExpanded; - protected int accHLGenerated; - protected int accClosedListHits; - protected int accPartialExpansions; - protected int accBypasses; - protected int accNodesExpandedWithGoalCost; - protected int accBypassLookAheadNodesCreated; - protected int accCardinalLookAheadNodesCreated; - protected int accConflictsBypassed; - protected int accCardinalConflictSplits; - protected int accSemiCardinalConflictSplits; - protected int accNonCardinalConflictSplits; - protected int accMddsBuilt; - protected int accMddsAdapted; - protected int accRestarts; - protected int accPathMaxBoosts; - protected int accReversePathMaxBoosts; - protected int accPathMaxPlusBoosts; - protected int accSurplusNodesAvoided; - protected int accMddCacheHits; - protected double accTimePlanningPaths; - protected double accTimeBuildingMdds; - - public int solutionCost; + private int _accHLExpanded; + private int _accHLGenerated; + private int _accClosedListHits; + private int _accPartialExpansions; + private int _accBypasses; + private int _accNodesExpandedWithGoalCost; + private int _accBypassLookAheadNodesCreated; + private int _accCardinalLookAheadNodesCreated; + private int _accConflictsBypassed; + private int _accCardinalConflictSplits; + private int _accSemiCardinalConflictSplits; + private int _accNonCardinalConflictSplits; + private int _accMddsBuilt; + private int _accMddsAdapted; + private int _accRestarts; + private int _accPathMaxBoosts; + private int _accReversePathMaxBoosts; + private int _accPathMaxPlusBoosts; + private int _accSurplusNodesAvoided; + private int _accMddCacheHits; + private double _accTimePlanningPaths; + private double _accTimeBuildingMdds; + + public int SolutionCost { get; private set; } /// /// The difference between the solution's cost and the f of the root node. /// Notice root.g != 0 in CBS. /// - protected int solutionDepth; - public Run runner; - protected CbsNode goalNode; - protected Plan solution; - protected Dictionary externalConflictCounts; - protected Dictionary> externalConflictTimes; + private int _solutionDepth; + public Run _runner; // TODO: remove this dependency + private CbsNode _goalNode; + private Plan _solution; + /// /// Nodes with a higher F aren't generated. As a result, goal nodes with a higher cost /// won't be found. /// - protected int maxSolutionCost; + private int _maxSolutionCost; /// /// Goal Nodes with with a lower cost aren't considered a goal. Used directly by CbsNode. /// - public int minSolutionCost; + public int MinSolutionCost { get; private set; } /// /// Search is stopped when the minimum F in the open list reaches the target, /// regardless of whether a goal node was found. Note maxSolutionCost stops the search when /// the same F value is exhausted from the open list later. /// - public int targetF { - get - { - return this.m_targetF; - } + public int TargetF + { + get => _targetF; set { - this.maxSolutionCost = Math.Min(this.maxSolutionCost, value); // No need to generate nodes with a higher F - this.m_targetF = value; + _maxSolutionCost = Math.Min(_maxSolutionCost, value); // No need to generate nodes with a higher F + _targetF = value; } } - protected int m_targetF; + private int _targetF; /// /// Search is stopped when the low level generated nodes count exceeds the cap /// - public int lowLevelGeneratedCap { set; get; } + public int LowLevelGeneratedCap { set; get; } /// /// Search is stopped when the millisecond count exceeds the cap /// - public int milliCap { set; get; } - protected ICbsSolver solver; - protected ICbsSolver singleAgentSolver; - public int mergeThreshold; - public int minSolutionTimeStep; - protected int maxSizeGroup; - protected int accMaxSizeGroup; - public BypassStrategy bypassStrategy; - public bool doMalte; - public ConflictChoice conflictChoice; - public bool disableTieBreakingByMinOpsEstimate; - public int lookaheadMaxExpansions; - public enum ConflictChoice : byte - { - FIRST = 0, - MOST_CONFLICTING_SMALLEST_AGENTS, - CARDINAL_MDD, - CARDINAL_LOOKAHEAD, - CARDINAL_MDD_THEN_MERGE_EARLY_MOST_CONFLICTING_SMALLEST_GROUP, - CARDINAL_MDD_THEN_MERGE_EARLY_MOST_CONFLICTING_AND_SMALLEST_GROUP - } + public int MilliCap { set; get; } + private ICbsSolver _solver; + private ICbsSolver _singleAgentSolver; + public int MergeThreshold { get; set; } + public int MinSolutionTimeStep { get; private set; } + private int _maxSizeGroup; + private int _accMaxSizeGroup; + private BypassStrategy _bypassStrategy; + public bool DoMalte { get; private set; } + public ConflictChoice ConflictChoice { get; private set; } + public bool DisableTieBreakingByMinOpsEstimate { get; private set; } + private int _lookaheadMaxExpansions; + public enum BypassStrategy : byte { NONE = 0, FIRST_FIT_LOOKAHEAD, BEST_FIT_LOOKAHEAD } - protected bool mergeCausesRestart; - private bool useOldCost; - public bool replanSameCostWithMdd; - public bool cacheMdds; - public bool useCAT; - - public ConflictAvoidanceTable externalCAT; - public ISet externalConstraints; + protected bool _mergeCausesRestart; + private readonly bool _useOldCost; + public bool ReplanSameCostWithMdd { get; private set; } + public bool CacheMdds { get; private set; } + private bool _useCAT; + + public ConflictAvoidanceTable ExternalCAT { get; private set; } + public ISet ExternalConstraints { get; private set; } /// /// Note that in this implementation, positive constraints currenly don't act as negative constraints for all other agents, regretably. /// They only rule out every other location for the agent at that time step. /// - public ISet externalPositiveConstraints; + public ISet ExternalPositiveConstraints { get; private set; } /// /// @@ -197,18 +195,17 @@ public CBS(ICbsSolver singleAgentSolver, ICbsSolver generalSolver, bool useCAT = true ) { - this.closedList = new Dictionary(); if (heuristic == null) - this.openList = new OpenList(this); + OpenList = new OpenList(this); else - this.openList = new DynamicLazyOpenList(this, heuristic); - this.mergeThreshold = mergeThreshold; - this.solver = generalSolver; - this.singleAgentSolver = singleAgentSolver; - this.bypassStrategy = bypassStrategy; - this.doMalte = doMalte; - this.conflictChoice = conflictChoice; - this.heuristic = heuristic; + OpenList = new DynamicLazyOpenList(this, heuristic); + MergeThreshold = mergeThreshold; + _solver = generalSolver; + _singleAgentSolver = singleAgentSolver; + _bypassStrategy = bypassStrategy; + DoMalte = doMalte; + ConflictChoice = conflictChoice; + _heuristic = heuristic; if (Constants.costFunction != Constants.CostFunction.SUM_OF_COSTS) { Trace.Assert(conflictChoice != ConflictChoice.CARDINAL_MDD && @@ -219,28 +216,22 @@ public CBS(ICbsSolver singleAgentSolver, ICbsSolver generalSolver, "Before this strategy is enabled we need to add a consideration of whether the agent whose cost will " + "increase has the highest cost in the solution first"); } - this.disableTieBreakingByMinOpsEstimate = disableTieBreakingByMinOpsEstimate; - this.lookaheadMaxExpansions = lookaheadMaxExpansions; - this.mergeCausesRestart = mergeCausesRestart; - this.replanSameCostWithMdd = replanSameCostWithMdd; - this.cacheMdds = cacheMdds; - this.useOldCost = useOldCost; - this.useCAT = useCAT; + DisableTieBreakingByMinOpsEstimate = disableTieBreakingByMinOpsEstimate; + _lookaheadMaxExpansions = lookaheadMaxExpansions; + _mergeCausesRestart = mergeCausesRestart; + ReplanSameCostWithMdd = replanSameCostWithMdd; + CacheMdds = cacheMdds; + _useOldCost = useOldCost; + _useCAT = useCAT; } /// /// Implements IIndependenceDetection.Setup for new groups /// - /// - /// - /// - /// - /// - /// public void Setup(ProblemInstance problemInstance, Run runner, ConflictAvoidanceTable CAT, int parentGroup1Cost, int parentGroup2Cost, int parentGroup1Size) { - this.Setup(problemInstance, -1, runner, CAT, null, null, parentGroup1Cost + parentGroup2Cost); // TODO: Support a makespan cost function + Setup(problemInstance, -1, runner, CAT, null, null, parentGroup1Cost + parentGroup2Cost); // TODO: Support a makespan cost function // I think we don't want to merge the agents in each group. We'd be left with two large meta-agents, // and in CBS one conflict isn't a reason to merge agents. The groups are arbitrarily large so we can't // even increment their conflict counts toward the merge threshold. @@ -249,11 +240,6 @@ public void Setup(ProblemInstance problemInstance, Run runner, ConflictAvoidance /// /// Implements IIndependenceDetection.Setup for replanning groups to resolve a conflict /// - /// - /// - /// - /// /// - /// public void Setup(ProblemInstance problemInstance, Run runner, ConflictAvoidanceTable CAT, int targetCost, ISet illegalMoves) { @@ -269,7 +255,7 @@ public void Setup(ProblemInstance problemInstance, Run runner, ConflictAvoidance constraints.Add(new CbsConstraint(agentState.agent.agentNum, illegalMove)); } } - this.Setup(problemInstance, illegalMoves.Max(move => move.Time), runner, CAT, constraints, null, targetCost, targetCost); + Setup(problemInstance, illegalMoves.Max(move => move.Time), runner, CAT, constraints, null, targetCost, targetCost); } /// @@ -286,70 +272,70 @@ public virtual void Setup(ProblemInstance problemInstance, int minSolutionTimeSt ISet externalPositiveConstraints, int minSolutionCost = -1, int maxSolutionCost = int.MaxValue, MDD mdd = null) { - this.instance = problemInstance; - this.runner = runner; - if (this.openList is DynamicLazyOpenList) - ((DynamicLazyOpenList)this.openList).runner = runner; - - this.ClearPrivateStatistics(); - this.solutionCost = 0; - this.solutionDepth = -1; - this.lowLevelGeneratedCap = int.MaxValue; - this.targetF = int.MaxValue; - this.milliCap = int.MaxValue; - this.goalNode = null; - this.solution = null; + _instance = problemInstance; + _runner = runner; + if (OpenList is DynamicLazyOpenList list) + list.runner = runner; + + ClearPrivateStatistics(); + SolutionCost = 0; + _solutionDepth = -1; + LowLevelGeneratedCap = int.MaxValue; + TargetF = int.MaxValue; + MilliCap = int.MaxValue; + _goalNode = null; + _solution = null; // CBS parameters - this.minSolutionTimeStep = minSolutionTimeStep; - this.minSolutionCost = minSolutionCost; - this.maxSolutionCost = Math.Max(this.maxSolutionCost, maxSolutionCost); - if (this.replanSameCostWithMdd) - Trace.Assert(this.mergeThreshold == -1, "Using MDDs to replan same-cost paths is currently only supported for single agents"); + MinSolutionTimeStep = minSolutionTimeStep; + MinSolutionCost = minSolutionCost; + _maxSolutionCost = Math.Max(_maxSolutionCost, maxSolutionCost); + if (ReplanSameCostWithMdd) + Trace.Assert(MergeThreshold == -1, "Using MDDs to replan same-cost paths is currently only supported for single agents"); - if (this.cacheMdds) + if (CacheMdds) { - this.mddCache = new Dictionary[instance.agents.Length]; - this.mddNarrownessValuesCache = new Dictionary>[instance.agents.Length]; - for (int i = 0; i < instance.agents.Length; i++) + MDDCache = new Dictionary[_instance.agents.Length]; + MDDNarrownessValuesCache = new Dictionary>[_instance.agents.Length]; + for (int i = 0; i < _instance.agents.Length; i++) { - this.mddCache[i] = new Dictionary(); - this.mddNarrownessValuesCache[i] = new Dictionary>(); + MDDCache[i] = []; + MDDNarrownessValuesCache[i] = []; } } - this.externalCAT = externalCAT; + ExternalCAT = externalCAT; CAT_U CAT = null; - if (this.useCAT) + if (_useCAT) { CAT = new CAT_U(); if (externalCAT != null) CAT.Join(externalCAT); } - this.externalConstraints = externalConstraints; - this.externalPositiveConstraints = externalPositiveConstraints; - if (externalPositiveConstraints == null && this.doMalte) + ExternalConstraints = externalConstraints; + ExternalPositiveConstraints = externalPositiveConstraints; + if (externalPositiveConstraints == null && DoMalte) externalPositiveConstraints = new HashSet(); - this.SetGlobals(); + SetGlobals(); - CbsNode root = new(instance.agents.Length, this.solver, this.singleAgentSolver, this); // Problem instance and various strategy data is all passed under 'this'. + CbsNode root = new(_instance.agents.Length, _solver, _singleAgentSolver, this); // Problem instance and various strategy data is all passed under 'this'. // Solve the root node bool solved = root.Solve(minSolutionTimeStep); if (solved) { - if (root.f <= this.maxSolutionCost) + if (root.F <= _maxSolutionCost) { - this.addToGlobalConflictCount(root); // TODO: Make MACBS_WholeTreeThreshold use nodes that do this automatically after choosing a conflict - this.openList.Add(root); - this.highLevelGenerated++; - this.closedList.Add(root, root); + addToGlobalConflictCount(root); // TODO: Make MACBS_WholeTreeThreshold use nodes that do this automatically after choosing a conflict + OpenList.Add(root); + _highLevelGenerated++; + _closedList.Add(root, root); } else { - this.surplusNodesAvoided++; + _surplusNodesAvoided++; } } } @@ -359,20 +345,11 @@ public virtual void Setup(ProblemInstance problemInstance, int minSolutionTimeSt /// /// /// - public virtual void Setup(ProblemInstance problemInstance, Run runner) - { - this.Setup(problemInstance, 0, runner, null, null, null); - } + public virtual void Setup(ProblemInstance problemInstance, Run runner) => Setup(problemInstance, 0, runner, null, null, null); - public IHeuristicCalculator GetHeuristic() - { - return this.heuristic; - } + public IHeuristicCalculator GetHeuristic() => _heuristic; - public ICbsSolver GetSolver() - { - return this.solver; - } + public ICbsSolver GetSolver() => _solver; public Dictionary GetExternalConflictCounts() { @@ -384,86 +361,83 @@ public Dictionary> GetConflictTimes() throw new NotImplementedException(); // For now. Also need to take care of generalised goal nodes! } - public ProblemInstance GetProblemInstance() - { - return this.instance; - } + public ProblemInstance GetProblemInstance() => _instance; public void Clear() { - this.openList.Clear(); - this.closedList.Clear(); - this.solver.Clear(); + OpenList.Clear(); + _closedList.Clear(); + _solver.Clear(); // Statistics are reset on Setup. } public virtual string GetName() { string lowLevelSolvers; - if (mergeThreshold == -1 || Object.ReferenceEquals(this.singleAgentSolver, this.solver)) - lowLevelSolvers = $"({this.singleAgentSolver})"; + if (MergeThreshold == -1 || Object.ReferenceEquals(_singleAgentSolver, _solver)) + lowLevelSolvers = $"({_singleAgentSolver})"; else - lowLevelSolvers = $"(single:{singleAgentSolver} multi:{solver})"; + lowLevelSolvers = $"(single:{_singleAgentSolver} multi:{_solver})"; string variants = ""; - if (this.bypassStrategy == BypassStrategy.FIRST_FIT_LOOKAHEAD) + if (_bypassStrategy == BypassStrategy.FIRST_FIT_LOOKAHEAD) { - if (this.lookaheadMaxExpansions != int.MaxValue) + if (_lookaheadMaxExpansions != int.MaxValue) { - if (this.lookaheadMaxExpansions != 1) - variants += $" with first fit adoption max expansions: {this.lookaheadMaxExpansions}"; + if (_lookaheadMaxExpansions != 1) + variants += $" with first fit adoption max expansions: {_lookaheadMaxExpansions}"; else variants += " + BP"; } else variants += " with first fit adoption max expansions: $\\infty$"; // LaTeX infinity symbol } - if (this.bypassStrategy == BypassStrategy.BEST_FIT_LOOKAHEAD) + if (_bypassStrategy == BypassStrategy.BEST_FIT_LOOKAHEAD) { - if (this.lookaheadMaxExpansions == int.MaxValue) + if (_lookaheadMaxExpansions == int.MaxValue) variants += " with infinite lookahead best fit adoption"; else - variants += $" with {this.lookaheadMaxExpansions} lookahead best fit adoption"; + variants += $" with {_lookaheadMaxExpansions} lookahead best fit adoption"; } - if (this.doMalte) + if (DoMalte) variants += " with Malte"; - if (this.conflictChoice == ConflictChoice.FIRST) + if (ConflictChoice == ConflictChoice.FIRST) variants += " choosing the first conflict in CBS nodes"; - else if (this.conflictChoice == ConflictChoice.CARDINAL_MDD) + else if (ConflictChoice == ConflictChoice.CARDINAL_MDD) variants += " + PC"; - else if (this.conflictChoice == ConflictChoice.CARDINAL_LOOKAHEAD) + else if (ConflictChoice == ConflictChoice.CARDINAL_LOOKAHEAD) variants += " + PC choosing cardinal conflicts using lookahead"; - else if (this.conflictChoice == ConflictChoice.CARDINAL_MDD_THEN_MERGE_EARLY_MOST_CONFLICTING_SMALLEST_GROUP) + else if (ConflictChoice == ConflictChoice.CARDINAL_MDD_THEN_MERGE_EARLY_MOST_CONFLICTING_SMALLEST_GROUP) variants += " + PC MERGE EARLY MOST CONFLICTING SMALLEST GROUP"; - else if (this.conflictChoice == ConflictChoice.CARDINAL_MDD_THEN_MERGE_EARLY_MOST_CONFLICTING_AND_SMALLEST_GROUP) + else if (ConflictChoice == ConflictChoice.CARDINAL_MDD_THEN_MERGE_EARLY_MOST_CONFLICTING_AND_SMALLEST_GROUP) variants += " + PC MERGE EARLY MOST CONFLICTING & SMALLEST GROUP"; - if (this.disableTieBreakingByMinOpsEstimate == true) + if (DisableTieBreakingByMinOpsEstimate) variants += " without smart tie breaking"; - if (this.mergeCausesRestart == true && mergeThreshold != -1) + if (_mergeCausesRestart == true && MergeThreshold != -1) variants += " with merge&restart"; - if (this.replanSameCostWithMdd) + if (ReplanSameCostWithMdd) variants += " with replanning same cost paths with MDDs"; - if (this.cacheMdds) + if (CacheMdds) variants += " with caching MDDs"; - if (this.useOldCost) + if (_useOldCost) variants += " with using old path costs"; - if (this.useCAT == false) + if (_useCAT == false) variants += " without a CAT"; - if (this.openList.GetType() != typeof(OpenList)) + if (OpenList.GetType() != typeof(OpenList)) { - variants += $" using {this.openList.GetName()}"; + variants += $" using {OpenList.GetName()}"; } - if (mergeThreshold == -1) + if (MergeThreshold == -1) return $"CBS/{lowLevelSolvers}{variants}"; - return $"MA-CBS-Local-{mergeThreshold}/{lowLevelSolvers}{variants}"; + return $"MA-CBS-Local-{MergeThreshold}/{lowLevelSolvers}{variants}"; } public override string ToString() @@ -471,337 +445,333 @@ public override string ToString() return GetName(); } - public int GetSolutionCost() { return this.solutionCost; } + public int GetSolutionCost() { return SolutionCost; } protected void ClearPrivateStatistics() { - this.highLevelExpanded = 0; - this.highLevelGenerated = 0; - this.closedListHits = 0; - this.partialExpansions = 0; - this.bypasses = 0; - this.nodesExpandedWithGoalCost = 0; - this.bypassLookAheadNodesCreated = 0; - this.cardinalLookAheadNodesCreated = 0; - this.conflictsBypassed = 0; - this.cardinalConflictSplits = 0; - this.semiCardinalConflictSplits = 0; - this.nonCardinalConflictSplits = 0; - this.mddsBuilt = 0; - this.mddsAdapted = 0; - this.restarts = 0; - this.pathMaxBoosts = 0; - this.reversePathMaxBoosts = 0; - this.pathMaxPlusBoosts = 0; - this.maxSizeGroup = 1; - this.surplusNodesAvoided = 0; - this.mddCacheHits = 0; - this.timePlanningPaths = 0; - this.timeBuildingMdds = 0; + _highLevelExpanded = 0; + _highLevelGenerated = 0; + _closedListHits = 0; + _partialExpansions = 0; + _bypasses = 0; + _nodesExpandedWithGoalCost = 0; + _bypassLookAheadNodesCreated = 0; + _cardinalLookAheadNodesCreated = 0; + _conflictsBypassed = 0; + _cardinalConflictSplits = 0; + _semiCardinalConflictSplits = 0; + _nonCardinalConflictSplits = 0; + MDDsBuilt = 0; + MDDsAdapted = 0; + _restarts = 0; + _pathMaxBoosts = 0; + _reversePathMaxBoosts = 0; + _pathMaxPlusBoosts = 0; + _maxSizeGroup = 1; + _surplusNodesAvoided = 0; + MDDCacheHits = 0; + TimePlanningPaths = 0; + TimeBuildingMdds = 0; } public virtual void OutputStatisticsHeader(TextWriter output) { - output.Write(this.ToString() + " Expanded (HL)"); + output.Write(ToString() + " Expanded (HL)"); output.Write(Run.RESULTS_DELIMITER); - output.Write(this.ToString() + " Generated (HL)"); + output.Write(ToString() + " Generated (HL)"); output.Write(Run.RESULTS_DELIMITER); - output.Write(this.ToString() + " Closed List Hits (HL)"); + output.Write(ToString() + " Closed List Hits (HL)"); output.Write(Run.RESULTS_DELIMITER); - output.Write(this.ToString() + " Partial Expansions (HL)"); + output.Write(ToString() + " Partial Expansions (HL)"); output.Write(Run.RESULTS_DELIMITER); - output.Write(this.ToString() + " Adoptions (HL)"); + output.Write(ToString() + " Adoptions (HL)"); output.Write(Run.RESULTS_DELIMITER); - output.Write(this.ToString() + " Nodes Expanded With Goal Cost (HL)"); + output.Write(ToString() + " Nodes Expanded With Goal Cost (HL)"); output.Write(Run.RESULTS_DELIMITER); - output.Write(this.ToString() + " Bypass Look Ahead Nodes Created (HL)"); + output.Write(ToString() + " Bypass Look Ahead Nodes Created (HL)"); output.Write(Run.RESULTS_DELIMITER); - output.Write(this.ToString() + " Cardinal Look Ahead Nodes Created (HL)"); + output.Write(ToString() + " Cardinal Look Ahead Nodes Created (HL)"); output.Write(Run.RESULTS_DELIMITER); - output.Write(this.ToString() + " Conflicts Bypassed With Adoption (HL)"); + output.Write(ToString() + " Conflicts Bypassed With Adoption (HL)"); output.Write(Run.RESULTS_DELIMITER); - output.Write(this.ToString() + " Cardinal Conflict Splits (HL)"); + output.Write(ToString() + " Cardinal Conflict Splits (HL)"); output.Write(Run.RESULTS_DELIMITER); - output.Write(this.ToString() + " Semi-Cardinal Conflict Splits (HL)"); + output.Write(ToString() + " Semi-Cardinal Conflict Splits (HL)"); output.Write(Run.RESULTS_DELIMITER); - output.Write(this.ToString() + " Non-Cardinal Conflict Splits (HL)"); + output.Write(ToString() + " Non-Cardinal Conflict Splits (HL)"); output.Write(Run.RESULTS_DELIMITER); - output.Write(this.ToString() + " MDDs Built (HL)"); + output.Write(ToString() + " MDDs Built (HL)"); output.Write(Run.RESULTS_DELIMITER); - output.Write(this.ToString() + " MDDs Adapted (HL)"); + output.Write(ToString() + " MDDs Adapted (HL)"); output.Write(Run.RESULTS_DELIMITER); - output.Write(this.ToString() + " Restarts (HL)"); + output.Write(ToString() + " Restarts (HL)"); output.Write(Run.RESULTS_DELIMITER); - output.Write(this.ToString() + " Path-Max Boosts (HL)"); + output.Write(ToString() + " Path-Max Boosts (HL)"); output.Write(Run.RESULTS_DELIMITER); - output.Write(this.ToString() + " Reverse Path-Max Boosts (HL)"); + output.Write(ToString() + " Reverse Path-Max Boosts (HL)"); output.Write(Run.RESULTS_DELIMITER); - output.Write(this.ToString() + " Path-Max Plus Boosts (HL)"); + output.Write(ToString() + " Path-Max Plus Boosts (HL)"); output.Write(Run.RESULTS_DELIMITER); - output.Write(this.ToString() + " Max Group Size (HL)"); + output.Write(ToString() + " Max Group Size (HL)"); output.Write(Run.RESULTS_DELIMITER); - output.Write(this.ToString() + " Surplus Nodes Avoided (HL)"); + output.Write(ToString() + " Surplus Nodes Avoided (HL)"); output.Write(Run.RESULTS_DELIMITER); - output.Write(this.ToString() + " MDD Cache Hits (HL)"); + output.Write(ToString() + " MDD Cache Hits (HL)"); output.Write(Run.RESULTS_DELIMITER); - output.Write(this.ToString() + " Time Planning Paths (HL)"); + output.Write(ToString() + " Time Planning Paths (HL)"); output.Write(Run.RESULTS_DELIMITER); - output.Write(this.ToString() + " Time Building MDDs (HL)"); + output.Write(ToString() + " Time Building MDDs (HL)"); output.Write(Run.RESULTS_DELIMITER); - this.solver.OutputStatisticsHeader(output); - if (Object.ReferenceEquals(this.singleAgentSolver, this.solver) == false) - this.singleAgentSolver.OutputStatisticsHeader(output); + _solver.OutputStatisticsHeader(output); + if (Object.ReferenceEquals(_singleAgentSolver, _solver) == false) + _singleAgentSolver.OutputStatisticsHeader(output); - this.openList.OutputStatisticsHeader(output); + OpenList.OutputStatisticsHeader(output); } public virtual void OutputStatistics(TextWriter output) { - Console.WriteLine("Total Expanded Nodes (High-Level): {0}", this.GetHighLevelExpanded()); - Console.WriteLine("Total Generated Nodes (High-Level): {0}", this.GetHighLevelGenerated()); - Console.WriteLine("Closed List Hits (High-Level): {0}", this.closedListHits); - Console.WriteLine("Partial Expansions (High-Level): {0}", this.partialExpansions); - Console.WriteLine("Adoptions (High-Level): {0}", this.bypasses); - Console.WriteLine("Nodes expanded with goal cost (High-Level): {0}", this.nodesExpandedWithGoalCost); - Console.WriteLine("Bypass lookahead nodes created (High-Level): {0}", this.bypassLookAheadNodesCreated); - Console.WriteLine("Cardinal lookahead nodes created (High-Level): {0}", this.cardinalLookAheadNodesCreated); - Console.WriteLine("Conflicts Bypassed With Adoption (High-Level): {0}", this.conflictsBypassed); - Console.WriteLine("Cardinal Conflicts Splits (High-Level): {0}", this.cardinalConflictSplits); - Console.WriteLine("Semi-Cardinal Conflicts Splits (High-Level): {0}", this.semiCardinalConflictSplits); - Console.WriteLine("Non-Cardinal Conflicts Splits (High-Level): {0}", this.nonCardinalConflictSplits); - Console.WriteLine("MDDs Built (High-Level): {0}", this.mddsBuilt); - Console.WriteLine("MDDs adapted (High-Level): {0}", this.mddsAdapted); - Console.WriteLine("Restarts (High-Level): {0}", this.restarts); - Console.WriteLine("Path-Max Boosts (High-Level): {0}", this.pathMaxBoosts); - Console.WriteLine("Reverse Path-Max Boosts (High-Level): {0}", this.reversePathMaxBoosts); - Console.WriteLine("Path-Max Plus Boosts (High-Level): {0}", this.pathMaxPlusBoosts); - Console.WriteLine("Max Group Size (High-Level): {0}", this.maxSizeGroup); - Console.WriteLine("Surplus Nodes Avoided (High-Level): {0}", this.surplusNodesAvoided); - Console.WriteLine("MDD cache hits (High-Level): {0}", this.mddCacheHits); - Console.WriteLine("Time planning paths (High-Level): {0}", this.timePlanningPaths); - Console.WriteLine("Time building mdds (High-Level): {0}", this.timeBuildingMdds); - - output.Write(this.highLevelExpanded + Run.RESULTS_DELIMITER); - output.Write(this.highLevelGenerated + Run.RESULTS_DELIMITER); - output.Write(this.closedListHits + Run.RESULTS_DELIMITER); - output.Write(this.partialExpansions + Run.RESULTS_DELIMITER); - output.Write(this.bypasses + Run.RESULTS_DELIMITER); - output.Write(this.nodesExpandedWithGoalCost + Run.RESULTS_DELIMITER); - output.Write(this.bypassLookAheadNodesCreated + Run.RESULTS_DELIMITER); - output.Write(this.cardinalLookAheadNodesCreated + Run.RESULTS_DELIMITER); - output.Write(this.conflictsBypassed + Run.RESULTS_DELIMITER); - output.Write(this.cardinalConflictSplits + Run.RESULTS_DELIMITER); - output.Write(this.semiCardinalConflictSplits + Run.RESULTS_DELIMITER); - output.Write(this.nonCardinalConflictSplits + Run.RESULTS_DELIMITER); - output.Write(this.mddsBuilt + Run.RESULTS_DELIMITER); - output.Write(this.mddsAdapted + Run.RESULTS_DELIMITER); - output.Write(this.restarts + Run.RESULTS_DELIMITER); - output.Write(this.pathMaxBoosts + Run.RESULTS_DELIMITER); - output.Write(this.reversePathMaxBoosts + Run.RESULTS_DELIMITER); - output.Write(this.pathMaxPlusBoosts + Run.RESULTS_DELIMITER); - output.Write(this.maxSizeGroup + Run.RESULTS_DELIMITER); - output.Write(this.surplusNodesAvoided + Run.RESULTS_DELIMITER); - output.Write(this.mddCacheHits + Run.RESULTS_DELIMITER); - output.Write(this.timePlanningPaths + Run.RESULTS_DELIMITER); - output.Write(this.timeBuildingMdds + Run.RESULTS_DELIMITER); - - this.solver.OutputAccumulatedStatistics(output); - if (Object.ReferenceEquals(this.singleAgentSolver, this.solver) == false) - this.singleAgentSolver.OutputAccumulatedStatistics(output); - - this.openList.OutputStatistics(output); + Console.WriteLine("Total Expanded Nodes (High-Level): {0}", GetHighLevelExpanded()); + Console.WriteLine("Total Generated Nodes (High-Level): {0}", GetHighLevelGenerated()); + Console.WriteLine("Closed List Hits (High-Level): {0}", _closedListHits); + Console.WriteLine("Partial Expansions (High-Level): {0}", _partialExpansions); + Console.WriteLine("Adoptions (High-Level): {0}", _bypasses); + Console.WriteLine("Nodes expanded with goal cost (High-Level): {0}", _nodesExpandedWithGoalCost); + Console.WriteLine("Bypass lookahead nodes created (High-Level): {0}", _bypassLookAheadNodesCreated); + Console.WriteLine("Cardinal lookahead nodes created (High-Level): {0}", _cardinalLookAheadNodesCreated); + Console.WriteLine("Conflicts Bypassed With Adoption (High-Level): {0}", _conflictsBypassed); + Console.WriteLine("Cardinal Conflicts Splits (High-Level): {0}", _cardinalConflictSplits); + Console.WriteLine("Semi-Cardinal Conflicts Splits (High-Level): {0}", _semiCardinalConflictSplits); + Console.WriteLine("Non-Cardinal Conflicts Splits (High-Level): {0}", _nonCardinalConflictSplits); + Console.WriteLine("MDDs Built (High-Level): {0}", MDDsBuilt); + Console.WriteLine("MDDs adapted (High-Level): {0}", MDDsAdapted); + Console.WriteLine("Restarts (High-Level): {0}", _restarts); + Console.WriteLine("Path-Max Boosts (High-Level): {0}", _pathMaxBoosts); + Console.WriteLine("Reverse Path-Max Boosts (High-Level): {0}", _reversePathMaxBoosts); + Console.WriteLine("Path-Max Plus Boosts (High-Level): {0}", _pathMaxPlusBoosts); + Console.WriteLine("Max Group Size (High-Level): {0}", _maxSizeGroup); + Console.WriteLine("Surplus Nodes Avoided (High-Level): {0}", _surplusNodesAvoided); + Console.WriteLine("MDD cache hits (High-Level): {0}", MDDCacheHits); + Console.WriteLine("Time planning paths (High-Level): {0}", TimePlanningPaths); + Console.WriteLine("Time building mdds (High-Level): {0}", TimeBuildingMdds); + + output.Write(_highLevelExpanded + Run.RESULTS_DELIMITER); + output.Write(_highLevelGenerated + Run.RESULTS_DELIMITER); + output.Write(_closedListHits + Run.RESULTS_DELIMITER); + output.Write(_partialExpansions + Run.RESULTS_DELIMITER); + output.Write(_bypasses + Run.RESULTS_DELIMITER); + output.Write(_nodesExpandedWithGoalCost + Run.RESULTS_DELIMITER); + output.Write(_bypassLookAheadNodesCreated + Run.RESULTS_DELIMITER); + output.Write(_cardinalLookAheadNodesCreated + Run.RESULTS_DELIMITER); + output.Write(_conflictsBypassed + Run.RESULTS_DELIMITER); + output.Write(_cardinalConflictSplits + Run.RESULTS_DELIMITER); + output.Write(_semiCardinalConflictSplits + Run.RESULTS_DELIMITER); + output.Write(_nonCardinalConflictSplits + Run.RESULTS_DELIMITER); + output.Write(MDDsBuilt + Run.RESULTS_DELIMITER); + output.Write(MDDsAdapted + Run.RESULTS_DELIMITER); + output.Write(_restarts + Run.RESULTS_DELIMITER); + output.Write(_pathMaxBoosts + Run.RESULTS_DELIMITER); + output.Write(_reversePathMaxBoosts + Run.RESULTS_DELIMITER); + output.Write(_pathMaxPlusBoosts + Run.RESULTS_DELIMITER); + output.Write(_maxSizeGroup + Run.RESULTS_DELIMITER); + output.Write(_surplusNodesAvoided + Run.RESULTS_DELIMITER); + output.Write(MDDCacheHits + Run.RESULTS_DELIMITER); + output.Write(TimePlanningPaths + Run.RESULTS_DELIMITER); + output.Write(TimeBuildingMdds + Run.RESULTS_DELIMITER); + + _solver.OutputAccumulatedStatistics(output); + if (Object.ReferenceEquals(_singleAgentSolver, _solver) == false) + _singleAgentSolver.OutputAccumulatedStatistics(output); + + OpenList.OutputStatistics(output); } public virtual int NumStatsColumns { get { - int numSolverStats = this.solver.NumStatsColumns; - if (Object.ReferenceEquals(this.singleAgentSolver, this.solver) == false) - numSolverStats += this.singleAgentSolver.NumStatsColumns; - return 23 + numSolverStats + this.openList.NumStatsColumns; + int numSolverStats = _solver.NumStatsColumns; + if (Object.ReferenceEquals(_singleAgentSolver, _solver) == false) + numSolverStats += _singleAgentSolver.NumStatsColumns; + return 23 + numSolverStats + OpenList.NumStatsColumns; } } public virtual void ClearStatistics() { - this.solver.ClearAccumulatedStatistics(); // Is this correct? Or is it better not to do it? - if (Object.ReferenceEquals(this.singleAgentSolver, this.solver) == false) - this.singleAgentSolver.ClearAccumulatedStatistics(); - this.ClearPrivateStatistics(); - this.openList.ClearStatistics(); + _solver.ClearAccumulatedStatistics(); // Is this correct? Or is it better not to do it? + if (Object.ReferenceEquals(_singleAgentSolver, _solver) == false) + _singleAgentSolver.ClearAccumulatedStatistics(); + ClearPrivateStatistics(); + OpenList.ClearStatistics(); } public virtual void ClearAccumulatedStatistics() { - this.accHLExpanded = 0; - this.accHLGenerated = 0; - this.accClosedListHits = 0; - this.accPartialExpansions = 0; - this.accBypasses = 0; - this.accNodesExpandedWithGoalCost = 0; - this.accBypassLookAheadNodesCreated = 0; - this.accCardinalLookAheadNodesCreated = 0; - this.accConflictsBypassed = 0; - this.accCardinalConflictSplits = 0; - this.accSemiCardinalConflictSplits = 0; - this.accNonCardinalConflictSplits = 0; - this.accMddsBuilt = 0; - this.accMddsAdapted = 0; - this.accRestarts = 0; - this.accPathMaxBoosts = 0; - this.accReversePathMaxBoosts = 0; - this.accPathMaxPlusBoosts = 0; - this.accMaxSizeGroup = 1; - this.accSurplusNodesAvoided = 0; - this.accMddCacheHits = 0; - this.accTimePlanningPaths = 0; - this.accTimeBuildingMdds = 0; - - this.solver.ClearAccumulatedStatistics(); - if (Object.ReferenceEquals(this.singleAgentSolver, this.solver) == false) - this.singleAgentSolver.ClearAccumulatedStatistics(); - - this.openList.ClearAccumulatedStatistics(); + _accHLExpanded = 0; + _accHLGenerated = 0; + _accClosedListHits = 0; + _accPartialExpansions = 0; + _accBypasses = 0; + _accNodesExpandedWithGoalCost = 0; + _accBypassLookAheadNodesCreated = 0; + _accCardinalLookAheadNodesCreated = 0; + _accConflictsBypassed = 0; + _accCardinalConflictSplits = 0; + _accSemiCardinalConflictSplits = 0; + _accNonCardinalConflictSplits = 0; + _accMddsBuilt = 0; + _accMddsAdapted = 0; + _accRestarts = 0; + _accPathMaxBoosts = 0; + _accReversePathMaxBoosts = 0; + _accPathMaxPlusBoosts = 0; + _accMaxSizeGroup = 1; + _accSurplusNodesAvoided = 0; + _accMddCacheHits = 0; + _accTimePlanningPaths = 0; + _accTimeBuildingMdds = 0; + + _solver.ClearAccumulatedStatistics(); + if (Object.ReferenceEquals(_singleAgentSolver, _solver) == false) + _singleAgentSolver.ClearAccumulatedStatistics(); + + OpenList.ClearAccumulatedStatistics(); } public virtual void AccumulateStatistics() { - this.accHLExpanded += this.highLevelExpanded; - this.accHLGenerated += this.highLevelGenerated; - this.accClosedListHits += this.closedListHits; - this.accPartialExpansions += this.partialExpansions; - this.accBypasses += this.bypasses; - this.accNodesExpandedWithGoalCost += this.nodesExpandedWithGoalCost; - this.accBypassLookAheadNodesCreated += this.bypassLookAheadNodesCreated; - this.accCardinalLookAheadNodesCreated += this.cardinalLookAheadNodesCreated; - this.accConflictsBypassed += this.conflictsBypassed; - this.accCardinalConflictSplits += this.cardinalConflictSplits; - this.accSemiCardinalConflictSplits += this.semiCardinalConflictSplits; - this.accNonCardinalConflictSplits += this.nonCardinalConflictSplits; - this.accMddsBuilt += this.mddsBuilt; - this.accMddsAdapted += this.mddsAdapted; - this.accRestarts += this.restarts; - this.accPathMaxBoosts += this.pathMaxBoosts; - this.accReversePathMaxBoosts += this.reversePathMaxBoosts; - this.accPathMaxPlusBoosts += this.pathMaxPlusBoosts; - this.accMaxSizeGroup = Math.Max(this.accMaxSizeGroup, this.maxSizeGroup); - this.accSurplusNodesAvoided += this.surplusNodesAvoided; - this.accMddCacheHits += this.mddCacheHits; - this.accTimePlanningPaths += this.timePlanningPaths; - this.accTimeBuildingMdds += this.timeBuildingMdds; - - // this.solver statistics are accumulated every time it's used. - - this.openList.AccumulateStatistics(); + _accHLExpanded += _highLevelExpanded; + _accHLGenerated += _highLevelGenerated; + _accClosedListHits += _closedListHits; + _accPartialExpansions += _partialExpansions; + _accBypasses += _bypasses; + _accNodesExpandedWithGoalCost += _nodesExpandedWithGoalCost; + _accBypassLookAheadNodesCreated += _bypassLookAheadNodesCreated; + _accCardinalLookAheadNodesCreated += _cardinalLookAheadNodesCreated; + _accConflictsBypassed += _conflictsBypassed; + _accCardinalConflictSplits += _cardinalConflictSplits; + _accSemiCardinalConflictSplits += _semiCardinalConflictSplits; + _accNonCardinalConflictSplits += _nonCardinalConflictSplits; + _accMddsBuilt += MDDsBuilt; + _accMddsAdapted += MDDsAdapted; + _accRestarts += _restarts; + _accPathMaxBoosts += _pathMaxBoosts; + _accReversePathMaxBoosts += _reversePathMaxBoosts; + _accPathMaxPlusBoosts += _pathMaxPlusBoosts; + _accMaxSizeGroup = Math.Max(_accMaxSizeGroup, _maxSizeGroup); + _accSurplusNodesAvoided += _surplusNodesAvoided; + _accMddCacheHits += MDDCacheHits; + _accTimePlanningPaths += TimePlanningPaths; + _accTimeBuildingMdds += TimeBuildingMdds; + + // solver statistics are accumulated every time it's used. + + OpenList.AccumulateStatistics(); } public virtual void OutputAccumulatedStatistics(TextWriter output) { - Console.WriteLine("{0} Accumulated Expanded Nodes (High-Level): {1}", this, this.accHLExpanded); - Console.WriteLine("{0} Accumulated Generated Nodes (High-Level): {1}", this, this.accHLGenerated); - Console.WriteLine("{0} Accumulated Closed List Hits (High-Level): {1}", this, this.accClosedListHits); - Console.WriteLine("{0} Accumulated Partial Expansions (High-Level): {1}", this, this.accPartialExpansions); - Console.WriteLine("{0} Accumulated Adoptions (High-Level): {1}", this, this.accBypasses); - Console.WriteLine("{0} Accumulated Nodes Expanded With Goal Cost (High-Level): {1}", this, this.accNodesExpandedWithGoalCost); - Console.WriteLine("{0} Accumulated Look Ahead Nodes Created (High-Level): {1}", this, this.accNodesExpandedWithGoalCost); - Console.WriteLine("{0} Accumulated Conflicts Bypassed With Adoption (High-Level): {1}", this, this.accConflictsBypassed); - Console.WriteLine("{0} Accumulated Cardinal Conflicts Splits (High-Level): {1}", this, this.accCardinalConflictSplits); - Console.WriteLine("{0} Accumulated Semi-Cardinal Conflicts Splits (High-Level): {1}", this, this.accSemiCardinalConflictSplits); - Console.WriteLine("{0} Accumulated Non-Cardinal Conflicts Splits (High-Level): {1}", this, this.accNonCardinalConflictSplits); - Console.WriteLine("{0} Accumulated MDDs Built (High-Level): {1}", this, this.accMddsBuilt); - Console.WriteLine("{0} Accumulated MDDs adapted (High-Level): {1}", this, this.accMddsAdapted); - Console.WriteLine("{0} Accumulated Restarts (High-Level): {1}", this, this.accRestarts); - Console.WriteLine("{0} Accumulated Path-Max Boosts (High-Level): {1}", this, this.accPathMaxBoosts); - Console.WriteLine("{0} Accumulated Reverse Path-Max Boosts (High-Level): {1}", this, this.accReversePathMaxBoosts); - Console.WriteLine("{0} Accumulated Path-Max Plus Boosts (High-Level): {1}", this, this.accPathMaxPlusBoosts); - Console.WriteLine("{0} Max Group Size (High-Level): {1}", this, this.accMaxSizeGroup); - Console.WriteLine("{0} Accumulated Surplus Nodes Avoided (High-Level): {1}", this, this.accSurplusNodesAvoided); - Console.WriteLine("{0} Accumulated MDD cache hits (High-Level): {1}", this, this.accMddCacheHits); - Console.WriteLine("{0} Accumulated time planning paths (High-Level): {1}", this, this.accTimePlanningPaths); - Console.WriteLine("{0} Accumulated time building MDDs (High-Level): {1}", this, this.accTimeBuildingMdds); - - output.Write(this.accHLExpanded + Run.RESULTS_DELIMITER); - output.Write(this.accHLGenerated + Run.RESULTS_DELIMITER); - output.Write(this.accClosedListHits + Run.RESULTS_DELIMITER); - output.Write(this.accPartialExpansions + Run.RESULTS_DELIMITER); - output.Write(this.accBypasses + Run.RESULTS_DELIMITER); - output.Write(this.accNodesExpandedWithGoalCost + Run.RESULTS_DELIMITER); - output.Write(this.accBypassLookAheadNodesCreated + Run.RESULTS_DELIMITER); - output.Write(this.accCardinalLookAheadNodesCreated + Run.RESULTS_DELIMITER); - output.Write(this.accConflictsBypassed + Run.RESULTS_DELIMITER); - output.Write(this.accCardinalConflictSplits + Run.RESULTS_DELIMITER); - output.Write(this.accSemiCardinalConflictSplits + Run.RESULTS_DELIMITER); - output.Write(this.accNonCardinalConflictSplits + Run.RESULTS_DELIMITER); - output.Write(this.accMddsBuilt + Run.RESULTS_DELIMITER); - output.Write(this.accMddsAdapted + Run.RESULTS_DELIMITER); - output.Write(this.accRestarts + Run.RESULTS_DELIMITER); - output.Write(this.accPathMaxBoosts + Run.RESULTS_DELIMITER); - output.Write(this.accReversePathMaxBoosts + Run.RESULTS_DELIMITER); - output.Write(this.accPathMaxPlusBoosts + Run.RESULTS_DELIMITER); - output.Write(this.accMaxSizeGroup + Run.RESULTS_DELIMITER); - output.Write(this.accSurplusNodesAvoided + Run.RESULTS_DELIMITER); - output.Write(this.accMddCacheHits + Run.RESULTS_DELIMITER); - output.Write(this.accTimePlanningPaths + Run.RESULTS_DELIMITER); - output.Write(this.accTimeBuildingMdds + Run.RESULTS_DELIMITER); - - this.solver.OutputAccumulatedStatistics(output); - if (Object.ReferenceEquals(this.singleAgentSolver, this.solver) == false) - this.singleAgentSolver.OutputAccumulatedStatistics(output); - - this.openList.OutputAccumulatedStatistics(output); + Console.WriteLine("{0} Accumulated Expanded Nodes (High-Level): {1}", this, _accHLExpanded); + Console.WriteLine("{0} Accumulated Generated Nodes (High-Level): {1}", this, _accHLGenerated); + Console.WriteLine("{0} Accumulated Closed List Hits (High-Level): {1}", this, _accClosedListHits); + Console.WriteLine("{0} Accumulated Partial Expansions (High-Level): {1}", this, _accPartialExpansions); + Console.WriteLine("{0} Accumulated Adoptions (High-Level): {1}", this, _accBypasses); + Console.WriteLine("{0} Accumulated Nodes Expanded With Goal Cost (High-Level): {1}", this, _accNodesExpandedWithGoalCost); + Console.WriteLine("{0} Accumulated Look Ahead Nodes Created (High-Level): {1}", this, _accNodesExpandedWithGoalCost); + Console.WriteLine("{0} Accumulated Conflicts Bypassed With Adoption (High-Level): {1}", this, _accConflictsBypassed); + Console.WriteLine("{0} Accumulated Cardinal Conflicts Splits (High-Level): {1}", this, _accCardinalConflictSplits); + Console.WriteLine("{0} Accumulated Semi-Cardinal Conflicts Splits (High-Level): {1}", this, _accSemiCardinalConflictSplits); + Console.WriteLine("{0} Accumulated Non-Cardinal Conflicts Splits (High-Level): {1}", this, _accNonCardinalConflictSplits); + Console.WriteLine("{0} Accumulated MDDs Built (High-Level): {1}", this, _accMddsBuilt); + Console.WriteLine("{0} Accumulated MDDs adapted (High-Level): {1}", this, _accMddsAdapted); + Console.WriteLine("{0} Accumulated Restarts (High-Level): {1}", this, _accRestarts); + Console.WriteLine("{0} Accumulated Path-Max Boosts (High-Level): {1}", this, _accPathMaxBoosts); + Console.WriteLine("{0} Accumulated Reverse Path-Max Boosts (High-Level): {1}", this, _accReversePathMaxBoosts); + Console.WriteLine("{0} Accumulated Path-Max Plus Boosts (High-Level): {1}", this, _accPathMaxPlusBoosts); + Console.WriteLine("{0} Max Group Size (High-Level): {1}", this, _accMaxSizeGroup); + Console.WriteLine("{0} Accumulated Surplus Nodes Avoided (High-Level): {1}", this, _accSurplusNodesAvoided); + Console.WriteLine("{0} Accumulated MDD cache hits (High-Level): {1}", this, _accMddCacheHits); + Console.WriteLine("{0} Accumulated time planning paths (High-Level): {1}", this, _accTimePlanningPaths); + Console.WriteLine("{0} Accumulated time building MDDs (High-Level): {1}", this, _accTimeBuildingMdds); + + output.Write(_accHLExpanded + Run.RESULTS_DELIMITER); + output.Write(_accHLGenerated + Run.RESULTS_DELIMITER); + output.Write(_accClosedListHits + Run.RESULTS_DELIMITER); + output.Write(_accPartialExpansions + Run.RESULTS_DELIMITER); + output.Write(_accBypasses + Run.RESULTS_DELIMITER); + output.Write(_accNodesExpandedWithGoalCost + Run.RESULTS_DELIMITER); + output.Write(_accBypassLookAheadNodesCreated + Run.RESULTS_DELIMITER); + output.Write(_accCardinalLookAheadNodesCreated + Run.RESULTS_DELIMITER); + output.Write(_accConflictsBypassed + Run.RESULTS_DELIMITER); + output.Write(_accCardinalConflictSplits + Run.RESULTS_DELIMITER); + output.Write(_accSemiCardinalConflictSplits + Run.RESULTS_DELIMITER); + output.Write(_accNonCardinalConflictSplits + Run.RESULTS_DELIMITER); + output.Write(_accMddsBuilt + Run.RESULTS_DELIMITER); + output.Write(_accMddsAdapted + Run.RESULTS_DELIMITER); + output.Write(_accRestarts + Run.RESULTS_DELIMITER); + output.Write(_accPathMaxBoosts + Run.RESULTS_DELIMITER); + output.Write(_accReversePathMaxBoosts + Run.RESULTS_DELIMITER); + output.Write(_accPathMaxPlusBoosts + Run.RESULTS_DELIMITER); + output.Write(_accMaxSizeGroup + Run.RESULTS_DELIMITER); + output.Write(_accSurplusNodesAvoided + Run.RESULTS_DELIMITER); + output.Write(_accMddCacheHits + Run.RESULTS_DELIMITER); + output.Write(_accTimePlanningPaths + Run.RESULTS_DELIMITER); + output.Write(_accTimeBuildingMdds + Run.RESULTS_DELIMITER); + + _solver.OutputAccumulatedStatistics(output); + if (Object.ReferenceEquals(_singleAgentSolver, _solver) == false) + _singleAgentSolver.OutputAccumulatedStatistics(output); + + OpenList.OutputAccumulatedStatistics(output); } - public bool debug = false; - private bool equivalenceWasOn; + private bool _debug = false; + private bool _equivalenceWasOn; - /// - /// - /// - /// protected void SetGlobals() { - this.equivalenceWasOn = AgentState.EquivalenceOverDifferentTimes; + _equivalenceWasOn = AgentState.EquivalenceOverDifferentTimes; AgentState.EquivalenceOverDifferentTimes = false; } protected void CleanGlobals() { - AgentState.EquivalenceOverDifferentTimes = this.equivalenceWasOn; + AgentState.EquivalenceOverDifferentTimes = _equivalenceWasOn; } public bool Solve() { - //this.SetGlobals(); // Again, because we might be resuming a search that was stopped. + //SetGlobals(); // Again, because we might be resuming a search that was stopped. int initialEstimate = 0; - if (openList.Count > 0) - initialEstimate = openList.Peek().f; + if (OpenList.Count > 0) + initialEstimate = OpenList.Peek().F; int maxExpandedNodeF = -1; int currentCost = -1; - while (openList.Count > 0) + while (OpenList.Count > 0) { // Check if max time has been exceeded - if (this.runner.ElapsedMilliseconds() > Constants.MAX_TIME) + if (_runner.ElapsedMilliseconds() > Constants.MAX_TIME) { - this.solutionCost = (int) Constants.SpecialCosts.TIMEOUT_COST; + SolutionCost = (int) Constants.SpecialCosts.TIMEOUT_COST; Console.WriteLine("Out of time"); - this.solutionDepth = maxExpandedNodeF - initialEstimate; // A minimum estimate. + _solutionDepth = maxExpandedNodeF - initialEstimate; // A minimum estimate. // Can't use top of OPEN's f-value instead of openList.Peek().f because we may have // timed out while expanding and before generating nodes that would have led to the // optimal solution, and the remaining nodes in OPEN are junk - this.Clear(); // Total search time exceeded - we're not going to resume this search. - this.CleanGlobals(); + Clear(); // Total search time exceeded - we're not going to resume this search. + CleanGlobals(); return false; } Debug.WriteLine("Getting the next node from OPEN"); - CbsNode currentNode = openList.Remove(); + CbsNode currentNode = OpenList.Remove(); - if (currentNode.f > this.maxSolutionCost) // A late heuristic application may have increased the node's cost + if (currentNode.F > _maxSolutionCost) // A late heuristic application may have increased the node's cost { continue; // Don't expand the node. @@ -826,21 +796,21 @@ public bool Solve() currentNode.DebugPrint(); // Update nodesExpandedWithGoalCost statistic - if (currentNode.g > currentCost) // Needs to be here because the goal may have a cost unseen before + if (currentNode.G > currentCost) // Needs to be here because the goal may have a cost unseen before { - currentCost = currentNode.g; - this.nodesExpandedWithGoalCost = 0; + currentCost = currentNode.G; + _nodesExpandedWithGoalCost = 0; } - else if (currentNode.g == currentCost) // check needed because macbs node cost isn't exactly monotonous + else if (currentNode.G == currentCost) // check needed because macbs node cost isn't exactly monotonous { - this.nodesExpandedWithGoalCost++; + _nodesExpandedWithGoalCost++; } // Check if node is the goal if (currentNode.GoalTest()) { - Trace.Assert(currentNode.g >= maxExpandedNodeF, - $"CBS goal node found with lower cost than the max cost node ever expanded ({currentNode.g} < {maxExpandedNodeF})"); + Trace.Assert(currentNode.G >= maxExpandedNodeF, + $"CBS goal node found with lower cost than the max cost node ever expanded ({currentNode.G} < {maxExpandedNodeF})"); // This is subtle, but MA-CBS may expand nodes in a non non-decreasing order: // If a non-optimal constraint is expanded upon and we decide to merge the agents, // the resulting node can have a lower cost than before, since we ignore the non-optimal constraint @@ -857,73 +827,73 @@ public bool Solve() // 55 grid cells and 9 obstacles. Debug.WriteLine("-----------------"); - this.solutionCost = currentNode.g; - this.solutionDepth = this.solutionCost - initialEstimate; - this.goalNode = currentNode; // Saves the single agent plans and costs + SolutionCost = currentNode.G; + _solutionDepth = SolutionCost - initialEstimate; + _goalNode = currentNode; // Saves the single agent plans and costs // The joint plan is calculated on demand. - this.Clear(); // Goal found - we're not going to resume this search - this.CleanGlobals(); + Clear(); // Goal found - we're not going to resume this search + CleanGlobals(); return true; } - if (maxExpandedNodeF < currentNode.f) + if (maxExpandedNodeF < currentNode.F) { - maxExpandedNodeF = currentNode.f; + maxExpandedNodeF = currentNode.F; Debug.WriteLine("New max F: {0}", maxExpandedNodeF); } // Check conditions that stop the search before a goal is found - if (currentNode.f >= this.targetF || // Node is good enough - this.singleAgentSolver.GetAccumulatedGenerated() + this.solver.GetAccumulatedGenerated() > - this.lowLevelGeneratedCap || // Stop because this is taking too long. + if (currentNode.F >= TargetF || // Node is good enough + _singleAgentSolver.GetAccumulatedGenerated() + _solver.GetAccumulatedGenerated() > + LowLevelGeneratedCap || // Stop because this is taking too long. // We're looking at _generated_ low level nodes since that's an indication to the amount of work done, // while expanded nodes is an indication of the amount of good work done. - (this.milliCap != int.MaxValue && // (This check is much cheaper than the method call) - this.runner.ElapsedMilliseconds() > this.milliCap)) // Search is taking too long. + (MilliCap != int.MaxValue && // (This check is much cheaper than the method call) + _runner.ElapsedMilliseconds() > MilliCap)) // Search is taking too long. { Debug.WriteLine("-----------------"); - this.solutionCost = maxExpandedNodeF; // This is the min possible cost so far. - this.openList.Add(currentNode); // To be able to continue the search later - this.CleanGlobals(); + SolutionCost = maxExpandedNodeF; // This is the min possible cost so far. + OpenList.Add(currentNode); // To be able to continue the search later + CleanGlobals(); return false; } // Expand - bool wasUnexpandedNode = (currentNode.agentAExpansion == CbsNode.ExpansionState.NOT_EXPANDED && - currentNode.agentBExpansion == CbsNode.ExpansionState.NOT_EXPANDED); + bool wasUnexpandedNode = (currentNode.AgentAExpansion == CbsNode.ExpansionState.NOT_EXPANDED && + currentNode.AgentBExpansion == CbsNode.ExpansionState.NOT_EXPANDED); Expand(currentNode); if (wasUnexpandedNode) - highLevelExpanded++; + _highLevelExpanded++; // Consider moving the following into Expand() - if (currentNode.agentAExpansion == CbsNode.ExpansionState.EXPANDED && - currentNode.agentBExpansion == CbsNode.ExpansionState.EXPANDED) // Fully expanded + if (currentNode.AgentAExpansion == CbsNode.ExpansionState.EXPANDED && + currentNode.AgentBExpansion == CbsNode.ExpansionState.EXPANDED) // Fully expanded currentNode.Clear(); } // Check if max time has been exceeded - if (this.runner.ElapsedMilliseconds() > Constants.MAX_TIME) + if (_runner.ElapsedMilliseconds() > Constants.MAX_TIME) { - this.solutionCost = (int)Constants.SpecialCosts.TIMEOUT_COST; + SolutionCost = (int)Constants.SpecialCosts.TIMEOUT_COST; Console.WriteLine("Out of time"); - this.solutionDepth = maxExpandedNodeF - initialEstimate; // A minimum estimate + _solutionDepth = maxExpandedNodeF - initialEstimate; // A minimum estimate } else - this.solutionCost = (int) Constants.SpecialCosts.NO_SOLUTION_COST; - this.Clear(); // we're not going to resume this search - it either timed out or the problem is unsolvable - this.CleanGlobals(); + SolutionCost = (int) Constants.SpecialCosts.NO_SOLUTION_COST; + Clear(); // we're not going to resume this search - it either timed out or the problem is unsolvable + CleanGlobals(); return false; } protected virtual bool ShouldMerge(CbsNode node) { - return node.ShouldMerge(mergeThreshold, node.conflict.agentAIndex, node.conflict.agentBIndex); //TODO: Add an option to use the old merge criterion + return node.ShouldMerge(MergeThreshold, node.Conflict.agentAIndex, node.Conflict.agentBIndex); //TODO: Add an option to use the old merge criterion } public virtual void Reset() { - this.restarts++; - this.openList.Clear(); - this.closedList.Clear(); + _restarts++; + OpenList.Clear(); + _closedList.Clear(); } /// @@ -938,21 +908,21 @@ public virtual void Reset() protected (bool adopted, IList children, bool reinsertParent) ExpandImpl(CbsNode node, bool adopt, CbsNode adoptBy = null) { CbsConflict conflict = node.GetConflict(); - var children = new List(); + List children = []; CbsNode child; int closedListHitChildCost; if (adoptBy == null) adoptBy = node; - int adoptByH = adoptBy.h; + int adoptByH = adoptBy.H; bool leftSameCost = false; // To quiet the compiler bool rightSameCost = false; - if (this.mergeThreshold != -1 && ShouldMerge(node)) + if (MergeThreshold != -1 && ShouldMerge(node)) { - if (this.mergeCausesRestart == false) + if (_mergeCausesRestart == false) { - (child, closedListHitChildCost) = this.MergeExpand(node); + (child, closedListHitChildCost) = MergeExpand(node); if (child == null) return (adopted: false, children, reinsertParent: false); // A timeout occured, // or the child was already in the closed list, @@ -965,19 +935,19 @@ public virtual void Reset() // cost as the sum of their current paths and no other conflicts exist? Should just // adopt this solution and get a goal node. // TODO: Save the cost of the group in a table, and use it as a heuristic in the future! - child = new CbsNode(this.instance.agents.Length, this.solver, - this.singleAgentSolver, this, node.agentsGroupAssignment); // This will be the new root node - child.MergeGroups(node.agentsGroupAssignment[conflict.agentAIndex], node.agentsGroupAssignment[conflict.agentBIndex], + child = new CbsNode(_instance.agents.Length, _solver, + _singleAgentSolver, this, node.AgentsGroupAssignment); // This will be the new root node + child.MergeGroups(node.AgentsGroupAssignment[conflict.agentAIndex], node.AgentsGroupAssignment[conflict.agentBIndex], fixCounts: false // This is a new root node, it doesn't have conflict counts yet ); - this.maxSizeGroup = Math.Max(this.maxSizeGroup, child.GetGroupSize(conflict.agentAIndex)); - bool solved = child.Solve(this.minSolutionTimeStep); - child.h = node.f - child.g; + _maxSizeGroup = Math.Max(_maxSizeGroup, child.GetGroupSize(conflict.agentAIndex)); + bool solved = child.Solve(MinSolutionTimeStep); + child.H = node.F - child.G; - //if (this.debug) - Debug.WriteLine($"Restarting the search with agents {node.agentsGroupAssignment[conflict.agentAIndex]} and" + - $" {node.agentsGroupAssignment[conflict.agentBIndex]} merged."); - this.Reset(); + //if (debug) + Debug.WriteLine($"Restarting the search with agents {node.AgentsGroupAssignment[conflict.agentAIndex]} and" + + $" {node.AgentsGroupAssignment[conflict.agentBIndex]} merged."); + Reset(); if (solved == false) // Likely due to a time-out return (adopted: false, children, reinsertParent: false); @@ -1002,24 +972,24 @@ public virtual void Reset() AdoptConditionally(adoptBy, child, adoptByH)) return (adopted: true, children, reinsertParent); children.Add(child); - leftSameCost = child.g == node.g; + leftSameCost = child.G == node.G; if (leftSameCost) { - Trace.Assert(node.conflict.willCostIncreaseForAgentA != CbsConflict.WillCostIncrease.YES); + Trace.Assert(node.Conflict.willCostIncreaseForAgentA != CbsConflict.WillCostIncrease.YES); } else { - Trace.Assert(node.conflict.willCostIncreaseForAgentA != CbsConflict.WillCostIncrease.NO); + Trace.Assert(node.Conflict.willCostIncreaseForAgentA != CbsConflict.WillCostIncrease.NO); } } } else // A timeout occured, or the child was already in the closed list. { if (closedListHitChildCost != -1) - leftSameCost = closedListHitChildCost == node.g; + leftSameCost = closedListHitChildCost == node.G; } - if (runner.ElapsedMilliseconds() > Constants.MAX_TIME) + if (_runner.ElapsedMilliseconds() > Constants.MAX_TIME) return (adopted: false, children, reinsertParent); // Generate right child: @@ -1035,33 +1005,33 @@ public virtual void Reset() AdoptConditionally(adoptBy, child, adoptByH)) return (adopted: true, children, reinsertParent); children.Add(child); - rightSameCost = child.g == node.g; + rightSameCost = child.G == node.G; if (rightSameCost) { - Trace.Assert(node.conflict.willCostIncreaseForAgentB != CbsConflict.WillCostIncrease.YES); + Trace.Assert(node.Conflict.willCostIncreaseForAgentB != CbsConflict.WillCostIncrease.YES); } else { - Trace.Assert(node.conflict.willCostIncreaseForAgentB != CbsConflict.WillCostIncrease.NO); + Trace.Assert(node.Conflict.willCostIncreaseForAgentB != CbsConflict.WillCostIncrease.NO); } } } else // A timeout occured, or the child was already in the closed list. { if (closedListHitChildCost != -1) - rightSameCost = closedListHitChildCost == node.g; + rightSameCost = closedListHitChildCost == node.G; } - if (node.agentAExpansion == CbsNode.ExpansionState.DEFERRED || node.agentBExpansion == CbsNode.ExpansionState.DEFERRED) - this.semiCardinalConflictSplits++; // Count only on the first expansion of the node. On the second expansion no count will be incremented. + if (node.AgentAExpansion == CbsNode.ExpansionState.DEFERRED || node.AgentBExpansion == CbsNode.ExpansionState.DEFERRED) + _semiCardinalConflictSplits++; // Count only on the first expansion of the node. On the second expansion no count will be incremented. else { if (leftSameCost && rightSameCost) - this.nonCardinalConflictSplits++; + _nonCardinalConflictSplits++; else if (leftSameCost || rightSameCost) - this.semiCardinalConflictSplits++; + _semiCardinalConflictSplits++; else - this.cardinalConflictSplits++; + _cardinalConflictSplits++; } return (adopted: false, children, reinsertParent); @@ -1069,29 +1039,29 @@ public virtual void Reset() private void ExpandIgnoringCardinalsButSupportingBP2(CbsNode node) { - int parentCost = node.g; - int parentH = node.h; + int parentCost = node.G; + int parentH = node.H; IList children = new List(2); bool reinsertParent = false; bool adopted = false; - int origCardinalConflictSplits = this.cardinalConflictSplits; - int origSemiCardinalConflictSplits = this.semiCardinalConflictSplits; - int origNonCardinalConflictSplits = this.nonCardinalConflictSplits; + int origCardinalConflictSplits = _cardinalConflictSplits; + int origSemiCardinalConflictSplits = _semiCardinalConflictSplits; + int origNonCardinalConflictSplits = _nonCardinalConflictSplits; - if (this.bypassStrategy == BypassStrategy.NONE) + if (_bypassStrategy == BypassStrategy.NONE) { - (adopted, children, reinsertParent) = this.ExpandImpl(node, adopt: false, adoptBy: null); + (adopted, children, reinsertParent) = ExpandImpl(node, adopt: false, adoptBy: null); } - else if (this.bypassStrategy == BypassStrategy.FIRST_FIT_LOOKAHEAD || node.parentAlreadyLookedAheadOf) // Do bypass but don't look ahead + else if (_bypassStrategy == BypassStrategy.FIRST_FIT_LOOKAHEAD || node.ParentAlreadyLookedAheadOf) // Do bypass but don't look ahead { bool adoptionPerformedBefore = false; while (true) // Until a node with a higher cost is found or a goal is found { - var lookAheadOpenList = new OpenList(this); - var lookAheadSameCostNodes = new HashSet(); - var lookAheadLargerCostNodes = new HashSet(); - var lookAheadSameCostNodesToReinsertWithHigherCost = new HashSet(); + OpenList lookAheadOpenList = new(this); + HashSet lookAheadSameCostNodes = []; + HashSet lookAheadLargerCostNodes = []; + HashSet lookAheadSameCostNodesToReinsertWithHigherCost = []; lookAheadOpenList.Add(node); if (lookAheadOpenList.Count != 0) @@ -1101,13 +1071,13 @@ private void ExpandIgnoringCardinalsButSupportingBP2(CbsNode node) int expansions = 0; while (lookAheadOpenList.Count != 0) { - if (expansions + 1 > this.lookaheadMaxExpansions) // + 1 because we're checking before the coming expansion. lookaheadMaxExpansions = 1 is the minimum working value. + if (expansions + 1 > _lookaheadMaxExpansions) // + 1 because we're checking before the coming expansion. lookaheadMaxExpansions = 1 is the minimum working value. { Debug.WriteLine("Lookahead count exceeded. Stopping lookahead."); break; } - if (runner.ElapsedMilliseconds() > Constants.MAX_TIME) + if (_runner.ElapsedMilliseconds() > Constants.MAX_TIME) return; CbsNode lookAheadNode = lookAheadOpenList.Remove(); @@ -1115,15 +1085,15 @@ private void ExpandIgnoringCardinalsButSupportingBP2(CbsNode node) Debug.WriteLine($"Looking ahead from node hash: {lookAheadNode.GetHashCode()}."); - (adopted, lookAheadChildren, lookAheadReinsertParent) = this.ExpandImpl(lookAheadNode, adopt: true, adoptBy: node); + (adopted, lookAheadChildren, lookAheadReinsertParent) = ExpandImpl(lookAheadNode, adopt: true, adoptBy: node); expansions++; if (adopted == true) { - this.bypassLookAheadNodesCreated += lookAheadChildren.Count + 1; // Either 1, if the first child was adopted, or 2 otherwise. +1 for the node itself before it adopted another solution. - this.cardinalConflictSplits = origCardinalConflictSplits; // No split was done - this.semiCardinalConflictSplits = origSemiCardinalConflictSplits; - this.nonCardinalConflictSplits = origNonCardinalConflictSplits; + _bypassLookAheadNodesCreated += lookAheadChildren.Count + 1; // Either 1, if the first child was adopted, or 2 otherwise. +1 for the node itself before it adopted another solution. + _cardinalConflictSplits = origCardinalConflictSplits; // No split was done + _semiCardinalConflictSplits = origSemiCardinalConflictSplits; + _nonCardinalConflictSplits = origNonCardinalConflictSplits; break; // Insert children into open list } @@ -1132,9 +1102,9 @@ private void ExpandIgnoringCardinalsButSupportingBP2(CbsNode node) foreach (var child in lookAheadChildren) { - this.bypassLookAheadNodesCreated++; - this.closedList.Add(child, child); // Temporarily! Just so look ahead expansion can use this data - if (child.g == node.g) + _bypassLookAheadNodesCreated++; + _closedList.Add(child, child); // Temporarily! Just so look ahead expansion can use this data + if (child.G == node.G) { lookAheadOpenList.Add(child); lookAheadSameCostNodes.Add(child); @@ -1150,20 +1120,20 @@ private void ExpandIgnoringCardinalsButSupportingBP2(CbsNode node) if (adopted && adoptionPerformedBefore == false) { adoptionPerformedBefore = true; - this.bypasses++; // Only count one adoption for the entire look ahead subtree branch traversed. + _bypasses++; // Only count one adoption for the entire look ahead subtree branch traversed. } if (node.GoalTest()) // Lookahead found an admissible solution! (The original node to expand wasn't a goal) { Debug.WriteLine("Goal found with same cost - stopping lookahead."); - this.openList.Add(node); + OpenList.Add(node); return; } if (adopted == false) // Insert children into open list { - if (node.h < 1 && lookAheadOpenList.Count == 0) // Looked ahead exhaustively - node.h = 1; // We know the goal isn't under it with the same cost + if (node.H < 1 && lookAheadOpenList.Count == 0) // Looked ahead exhaustively + node.H = 1; // We know the goal isn't under it with the same cost // We already expanded this whole subtree of same cost nodes (at least partially). Insert the frontier to the open list. reinsertParent = false; // It may be reinserted as an element of the lookAheadSameCostNodesToReinsertWithHigherCost @@ -1173,7 +1143,7 @@ private void ExpandIgnoringCardinalsButSupportingBP2(CbsNode node) { CbsNode child = lookAheadOpenList.Remove(); children.Add(child); // Unexapnded so it's also on the frontier - this.closedList.Remove(child); // Just so it'll be inserted into the open list at the end of the method + _closedList.Remove(child); // Just so it'll be inserted into the open list at the end of the method } foreach (var reInsertNode in lookAheadSameCostNodesToReinsertWithHigherCost) { @@ -1183,37 +1153,37 @@ private void ExpandIgnoringCardinalsButSupportingBP2(CbsNode node) continue; } children.Add(reInsertNode); - this.closedList.Remove(reInsertNode); // Just so it'll be inserted into the open list at the end of the method + _closedList.Remove(reInsertNode); // Just so it'll be inserted into the open list at the end of the method } foreach (CbsNode lookAheadNode in lookAheadLargerCostNodes) - this.closedList.Remove(lookAheadNode); // Just so they'll be inserted into the open list at the end of the method - this.bypassLookAheadNodesCreated -= lookAheadSameCostNodes.Count + lookAheadLargerCostNodes.Count; // None of these nodes weren't wasted effort, they were properly generated - this.highLevelGenerated += lookAheadSameCostNodes.Count + _closedList.Remove(lookAheadNode); // Just so they'll be inserted into the open list at the end of the method + _bypassLookAheadNodesCreated -= lookAheadSameCostNodes.Count + lookAheadLargerCostNodes.Count; // None of these nodes weren't wasted effort, they were properly generated + _highLevelGenerated += lookAheadSameCostNodes.Count - lookAheadSameCostNodesToReinsertWithHigherCost.Count - unexpandedSameCostNodes; // You could say they were generated and not looked ahead at, and they won't be counted later. - this.highLevelExpanded += lookAheadSameCostNodes.Count - unexpandedSameCostNodes; + _highLevelExpanded += lookAheadSameCostNodes.Count - unexpandedSameCostNodes; break; } else // Adopted. Do a new round of expansions. { foreach (CbsNode lookAheadNode in lookAheadLargerCostNodes) - this.closedList.Remove(lookAheadNode); // Just so they'll be inserted into the open list at the end of the method + _closedList.Remove(lookAheadNode); // Just so they'll be inserted into the open list at the end of the method foreach (CbsNode lookAheadNode in lookAheadSameCostNodes) - this.closedList.Remove(lookAheadNode); // Just so they'll be inserted into the open list at the end of the method + _closedList.Remove(lookAheadNode); // Just so they'll be inserted into the open list at the end of the method // The only difference in closed list cleanup after adoption is that expanded same cost nodes are also removed from the closed list. } } } - else // this.bypassStrategy == BypassStrategy.BEST_FIT_LOOKAHEAD and this set of costs not already done + else // bypassStrategy == BypassStrategy.BEST_FIT_LOOKAHEAD and this set of costs not already done { // FIXME: lookaheadMaxExpansions isn't respected correctly here. We actually limit the number of same-cost nodes generated, which is similar but not the same. - var lookAheadOpenList = new OpenList(this); - var lookAheadSameCostNodes = new HashSet(); - var lookAheadLargerCostNodes = new HashSet(); - var lookAheadSameCostNodesToReinsertWithHigherCost = new HashSet(); + OpenList lookAheadOpenList = new(this); + HashSet lookAheadSameCostNodes = []; + HashSet lookAheadLargerCostNodes = []; + HashSet lookAheadSameCostNodesToReinsertWithHigherCost = []; lookAheadOpenList.Add(node); - node.parentAlreadyLookedAheadOf = true; + node.ParentAlreadyLookedAheadOf = true; if (lookAheadOpenList.Count != 0) Debug.WriteLine("Starting lookahead:"); @@ -1221,7 +1191,7 @@ private void ExpandIgnoringCardinalsButSupportingBP2(CbsNode node) bool lookAheadReinsertParent; while (lookAheadOpenList.Count != 0) { - if (lookAheadSameCostNodes.Count + 1 >= this.lookaheadMaxExpansions) // + 1 for the root + if (lookAheadSameCostNodes.Count + 1 >= _lookaheadMaxExpansions) // + 1 for the root { Debug.WriteLine("Lookahead count exceeded. Stopping lookahead."); break; @@ -1230,27 +1200,27 @@ private void ExpandIgnoringCardinalsButSupportingBP2(CbsNode node) CbsNode lookAheadNode = lookAheadOpenList.Remove(); lookAheadNode.ChooseConflict(); - if (runner.ElapsedMilliseconds() > Constants.MAX_TIME) + if (_runner.ElapsedMilliseconds() > Constants.MAX_TIME) return; Debug.WriteLine($"Looking ahead from node hash: {lookAheadNode.GetHashCode()}."); - (adopted, lookAheadChildren, lookAheadReinsertParent) = this.ExpandImpl(lookAheadNode, adopt: false); + (adopted, lookAheadChildren, lookAheadReinsertParent) = ExpandImpl(lookAheadNode, adopt: false); if (lookAheadReinsertParent) lookAheadSameCostNodesToReinsertWithHigherCost.Add(lookAheadNode); foreach (var lookaheadChild in lookAheadChildren) { - this.bypassLookAheadNodesCreated++; - this.closedList.Add(lookaheadChild, lookaheadChild); // Temporarily! Just so look ahead expansion can use this data - if (lookaheadChild.g == node.g) + _bypassLookAheadNodesCreated++; + _closedList.Add(lookaheadChild, lookaheadChild); // Temporarily! Just so look ahead expansion can use this data + if (lookaheadChild.G == node.G) { if (lookaheadChild.GoalTest()) // Lookahead found an admissible solution! { Debug.WriteLine("Goal found with same cost - stopping lookahead."); - this.openList.Add(lookaheadChild); // Technically should have just breaked and let node adopt child. This is just a short-cut. - this.bypasses++; // Just a technicality needed to make the adoption count not lower than if we didn't use immediate adoption. You could say we adopt the goal's solution. + OpenList.Add(lookaheadChild); // Technically should have just breaked and let node adopt child. This is just a short-cut. + _bypasses++; // Just a technicality needed to make the adoption count not lower than if we didn't use immediate adoption. You could say we adopt the goal's solution. return; } @@ -1265,14 +1235,14 @@ private void ExpandIgnoringCardinalsButSupportingBP2(CbsNode node) } } - if (node.h < 1 && lookAheadOpenList.Count == 0) // Looked ahead exhaustively - node.h = 1; // We know the goal isn't under it with the same cost + if (node.H < 1 && lookAheadOpenList.Count == 0) // Looked ahead exhaustively + node.H = 1; // We know the goal isn't under it with the same cost bool bypassPerformed = false; // Find the best look ahead node and check if it's worth adopting if (lookAheadSameCostNodes.Count != 0) { - IList lookAheadSameCostNodesSerialzed = lookAheadSameCostNodes.ToList(); + IList lookAheadSameCostNodesSerialzed = [.. lookAheadSameCostNodes]; CbsNode adoptionCandidate = lookAheadSameCostNodesSerialzed[0]; for (int i = 1; i < lookAheadSameCostNodesSerialzed.Count; i++) { @@ -1282,33 +1252,33 @@ private void ExpandIgnoringCardinalsButSupportingBP2(CbsNode node) if (AdoptConditionally(node, adoptionCandidate, parentH)) { bypassPerformed = true; - this.bypasses++; + _bypasses++; - this.cardinalConflictSplits = origCardinalConflictSplits; - this.semiCardinalConflictSplits = origSemiCardinalConflictSplits; - this.nonCardinalConflictSplits = origNonCardinalConflictSplits; + _cardinalConflictSplits = origCardinalConflictSplits; + _semiCardinalConflictSplits = origSemiCardinalConflictSplits; + _nonCardinalConflictSplits = origNonCardinalConflictSplits; // Remove cancelled look-ahead nodes from closed list foreach (CbsNode lookAheadNode in lookAheadSameCostNodes) - this.closedList.Remove(lookAheadNode); + _closedList.Remove(lookAheadNode); foreach (CbsNode lookAheadNode in lookAheadLargerCostNodes) - this.closedList.Remove(lookAheadNode); + _closedList.Remove(lookAheadNode); - if (this.openList.Count != 0 && this.openList.Peek().f < node.f) + if (OpenList.Count != 0 && OpenList.Peek().F < node.F) // This is a new unexpanded node, and we just raised its h, so we can push it back // FIXME: More duplication of the push back logic { reinsertParent = true; - children = new List(); // Children will be generated when this node comes out of the open list + children = []; // Children will be generated when this node comes out of the open list - if (this.debug) + if (_debug) Debug.WriteLine("Reinserting node into the open list with h=1, since the goal wasn't found with the same cost under it."); } else { // We updated the node, need to re-expand it. Surprisingly, re-expansion can produce even better nodes for adoption, so we need to allow immediate expansion too. // TODO: Just re-run the infinite lookahead? That would be the immediate adoption variant above - this.Expand(node); + Expand(node); return; } } @@ -1318,13 +1288,13 @@ private void ExpandIgnoringCardinalsButSupportingBP2(CbsNode node) { // Adoption not performed, and we already expanded this whole subtree of same cost nodes (at least partially) reinsertParent = false; // It may reinserted as an element of the children list - children = lookAheadLargerCostNodes.ToList(); + children = [.. lookAheadLargerCostNodes]; int unexpandedSameCostNodes = lookAheadOpenList.Count; while (lookAheadOpenList.Count != 0) { CbsNode child = lookAheadOpenList.Remove(); children.Add(child); - this.closedList.Remove(child); // Just so it'll be inserted into the open list at the end of the method + _closedList.Remove(child); // Just so it'll be inserted into the open list at the end of the method } foreach (var reInsertNode in lookAheadSameCostNodesToReinsertWithHigherCost) { @@ -1334,50 +1304,50 @@ private void ExpandIgnoringCardinalsButSupportingBP2(CbsNode node) continue; } children.Add(reInsertNode); - this.closedList.Remove(reInsertNode); // Just so it'll be inserted into the open list at the end of the method + _closedList.Remove(reInsertNode); // Just so it'll be inserted into the open list at the end of the method if (reInsertNode == node) // Re-inserting the original node that was to be expanded, with a higher h - this.highLevelGenerated--; // It'll be counted as a new generated child later, and we don't need to count it twice + _highLevelGenerated--; // It'll be counted as a new generated child later, and we don't need to count it twice } foreach (CbsNode lookAheadNode in lookAheadLargerCostNodes) - this.closedList.Remove(lookAheadNode); // Just so they'll be inserted into the open list at the end of the method - this.bypassLookAheadNodesCreated -= lookAheadSameCostNodes.Count + lookAheadLargerCostNodes.Count; // These nodes weren't wasted effort - this.highLevelGenerated += lookAheadSameCostNodes.Count - lookAheadSameCostNodesToReinsertWithHigherCost.Count - unexpandedSameCostNodes; // You could say they were generated and not looked ahead at, and they won't be counted later. - this.highLevelExpanded += lookAheadSameCostNodes.Count - unexpandedSameCostNodes; + _closedList.Remove(lookAheadNode); // Just so they'll be inserted into the open list at the end of the method + _bypassLookAheadNodesCreated -= lookAheadSameCostNodes.Count + lookAheadLargerCostNodes.Count; // These nodes weren't wasted effort + _highLevelGenerated += lookAheadSameCostNodes.Count - lookAheadSameCostNodesToReinsertWithHigherCost.Count - unexpandedSameCostNodes; // You could say they were generated and not looked ahead at, and they won't be counted later. + _highLevelExpanded += lookAheadSameCostNodes.Count - unexpandedSameCostNodes; } } // Both children considered. None adopted. Add them to the open list, and re-insert the partially expanded parent too if necessary. if (reinsertParent) - this.openList.Add(node); // Re-insert node into open list with higher cost, don't re-increment global conflict counts + OpenList.Add(node); // Re-insert node into open list with higher cost, don't re-increment global conflict counts foreach (var child in children) { - closedList.Add(child, child); + _closedList.Add(child, child); // Bequeath remainder of h from parent - int remainingParentH = parentH - (child.g - parentCost); - if (child.h < remainingParentH) - child.h = (ushort) remainingParentH; + int remainingParentH = parentH - (child.G - parentCost); + if (child.H < remainingParentH) + child.H = (ushort) remainingParentH; - if (this.bypassStrategy == BypassStrategy.BEST_FIT_LOOKAHEAD) + if (_bypassStrategy == BypassStrategy.BEST_FIT_LOOKAHEAD) { - if (child.g == parentCost) // Total cost didn't increase (yet) - child.parentAlreadyLookedAheadOf = true; + if (child.G == parentCost) // Total cost didn't increase (yet) + child.ParentAlreadyLookedAheadOf = true; else - child.parentAlreadyLookedAheadOf = false; + child.ParentAlreadyLookedAheadOf = false; } - if (child.f <= this.maxSolutionCost) + if (child.F <= _maxSolutionCost) // Assuming h is an admissible heuristic, no need to generate nodes that won't get us // to the goal within the budget { - this.highLevelGenerated++; - this.addToGlobalConflictCount(child); // TODO: Make MACBS_WholeTreeThreshold use nodes that do this automatically after choosing a conflict - openList.Add(child); + _highLevelGenerated++; + addToGlobalConflictCount(child); // TODO: Make MACBS_WholeTreeThreshold use nodes that do this automatically after choosing a conflict + OpenList.Add(child); } else { - this.surplusNodesAvoided++; + _surplusNodesAvoided++; } } } @@ -1389,27 +1359,27 @@ private void ExpandIgnoringCardinalsButSupportingBP2(CbsNode node) /// private void IcbsExpand(CbsNode node) { - int parentCost = node.g; - int parentH = node.h; + int parentCost = node.G; + int parentH = node.H; IList children = new List(2); bool reinsertParent; bool adopted = false; - int origCardinalConflictSplits = this.cardinalConflictSplits; - int origSemiCardinalConflictSplits = this.semiCardinalConflictSplits; - int origNonCardinalConflictSplits = this.nonCardinalConflictSplits; + int origCardinalConflictSplits = _cardinalConflictSplits; + int origSemiCardinalConflictSplits = _semiCardinalConflictSplits; + int origNonCardinalConflictSplits = _nonCardinalConflictSplits; - Trace.Assert(this.bypassStrategy == BypassStrategy.NONE || this.lookaheadMaxExpansions == 1, "Only BP1 is supported with cardinal conflict choice"); // Assumed in order to simplify code - Trace.Assert(this.bypassStrategy != BypassStrategy.BEST_FIT_LOOKAHEAD, "Only first-fit adoption is supported with cardinal conflict choice"); // Assumed in order to simplify code + Trace.Assert(_bypassStrategy == BypassStrategy.NONE || _lookaheadMaxExpansions == 1, "Only BP1 is supported with cardinal conflict choice"); // Assumed in order to simplify code + Trace.Assert(_bypassStrategy != BypassStrategy.BEST_FIT_LOOKAHEAD, "Only first-fit adoption is supported with cardinal conflict choice"); // Assumed in order to simplify code bool adoptionPerformedBefore = false; // Adoption and cardinal-lookahead cycle while (true) { - (adopted, children, reinsertParent) = this.ExpandImpl(node, adopt: bypassStrategy != BypassStrategy.NONE); + (adopted, children, reinsertParent) = ExpandImpl(node, adopt: _bypassStrategy != BypassStrategy.NONE); - if (this.mergeCausesRestart && (this.closedList.Count == 0)) // HACK: Means a restart was triggered + if (_mergeCausesRestart && (_closedList.Count == 0)) // HACK: Means a restart was triggered break; if (adopted) // Either we found a goal, or we need to re-expand using a remaining conflict @@ -1417,21 +1387,21 @@ private void IcbsExpand(CbsNode node) if (adoptionPerformedBefore == false) { adoptionPerformedBefore = true; - this.bypasses++; // Only count one adoption for the entire look ahead subtree branch traversed. + _bypasses++; // Only count one adoption for the entire look ahead subtree branch traversed. } - this.bypassLookAheadNodesCreated += children.Count + 1; // Either 1, if the first child was adopted, or 2 otherwise, +1 because the adopted child isn't returned. - this.cardinalConflictSplits = origCardinalConflictSplits; - this.semiCardinalConflictSplits = origSemiCardinalConflictSplits; - this.nonCardinalConflictSplits = origNonCardinalConflictSplits; + _bypassLookAheadNodesCreated += children.Count + 1; // Either 1, if the first child was adopted, or 2 otherwise, +1 because the adopted child isn't returned. + _cardinalConflictSplits = origCardinalConflictSplits; + _semiCardinalConflictSplits = origSemiCardinalConflictSplits; + _nonCardinalConflictSplits = origNonCardinalConflictSplits; if (node.GoalTest()) // Adoption found an admissible solution! (The original node to expand wasn't a goal) { Debug.WriteLine("Bypass found a goal! Inserting it into OPEN."); - this.openList.Add(node); + OpenList.Add(node); return; } - Trace.Assert(node.conflict != null, "A conflict should have been found"); + Trace.Assert(node.Conflict != null, "A conflict should have been found"); // We might re-encounter conflicts we've already found aren't cardinal, but // since at least one path changed, the number of conflicts after adopting a child // might become smaller after adopting a child's solution this time @@ -1439,13 +1409,13 @@ private void IcbsExpand(CbsNode node) } if ( - children.All(child => child.g > node.g) && // All generated nodes have a higher cost - ((node.agentAExpansion == CbsNode.ExpansionState.EXPANDED && node.agentBExpansion == CbsNode.ExpansionState.EXPANDED) || // Both children generated by now (possibly one in the past and one deferred to now) - (node.agentAExpansion == CbsNode.ExpansionState.EXPANDED && node.agentBExpansion == CbsNode.ExpansionState.DEFERRED) || // One child still deferred, will surely have a higher cost (that's why it was deferred) - (node.agentAExpansion == CbsNode.ExpansionState.DEFERRED && node.agentBExpansion == CbsNode.ExpansionState.EXPANDED)) // One child still deferred, will surely have a higher cost (that's why it was deferred) + children.All(child => child.G > node.G) && // All generated nodes have a higher cost + ((node.AgentAExpansion == CbsNode.ExpansionState.EXPANDED && node.AgentBExpansion == CbsNode.ExpansionState.EXPANDED) || // Both children generated by now (possibly one in the past and one deferred to now) + (node.AgentAExpansion == CbsNode.ExpansionState.EXPANDED && node.AgentBExpansion == CbsNode.ExpansionState.DEFERRED) || // One child still deferred, will surely have a higher cost (that's why it was deferred) + (node.AgentAExpansion == CbsNode.ExpansionState.DEFERRED && node.AgentBExpansion == CbsNode.ExpansionState.EXPANDED)) // One child still deferred, will surely have a higher cost (that's why it was deferred) ) // A cardinal conflict, or deferred expansion of a cardinal or semi-cardinal conflict. { - if (this.debug) + if (_debug) Debug.WriteLine("This was a cardinal conflict, or deferred expansion of a cardinal or semi-cardinal conflict. Inserting the generated children into OPEN."); break; // Look no further @@ -1455,9 +1425,9 @@ private void IcbsExpand(CbsNode node) // This wasn't a cardinal conflict, continue cycling through conflicts } - if (children.Any(child => child.GoalTest() && child.g == node.g)) // Admissable goal found (and adoption isn't enabled, otherwise the goal would have been adopted above) + if (children.Any(child => child.GoalTest() && child.G == node.G)) // Admissable goal found (and adoption isn't enabled, otherwise the goal would have been adopted above) { - if (this.debug) + if (_debug) Debug.WriteLine("Admissable goal found! Inserting the generated children into OPEN."); break; // Look no further @@ -1472,21 +1442,21 @@ private void IcbsExpand(CbsNode node) } else { - Debug.WriteLine($"This was a non-cardinal conflict. Trying potentially-cardinal conflict: {node.conflict}"); + Debug.WriteLine($"This was a non-cardinal conflict. Trying potentially-cardinal conflict: {node.Conflict}"); } // Prepare to restart the loop // Cancel partial expansion effects: - node.agentAExpansion = CbsNode.ExpansionState.NOT_EXPANDED; - node.agentBExpansion = CbsNode.ExpansionState.NOT_EXPANDED; - node.h = parentH; + node.AgentAExpansion = CbsNode.ExpansionState.NOT_EXPANDED; + node.AgentBExpansion = CbsNode.ExpansionState.NOT_EXPANDED; + node.H = parentH; // Take care of counts: - this.cardinalLookAheadNodesCreated += children.Count; + _cardinalLookAheadNodesCreated += children.Count; } if (reinsertParent) { - this.openList.Add(node); // Re-insert node into open list with higher cost, don't re-increment global conflict counts + OpenList.Add(node); // Re-insert node into open list with higher cost, don't re-increment global conflict counts } else { @@ -1502,67 +1472,66 @@ private void IcbsExpand(CbsNode node) // Path-Max stage // Reverse Path-Max (Mero, 1984) - operators aren't invertible, so we can only take min(h(Ci) + dist(P, Ci)) for h(P) (if it's higher) - int minChildSumHAndOperatorCost = children.Max(child => child.h + (child.g - node.g)); - if (node.h < minChildSumHAndOperatorCost) + int minChildSumHAndOperatorCost = children.Max(child => child.H + (child.G - node.G)); + if (node.H < minChildSumHAndOperatorCost) { - node.hBonus += minChildSumHAndOperatorCost - node.h; - node.h = minChildSumHAndOperatorCost; - ++reversePathMaxBoosts; + node.HBonus += minChildSumHAndOperatorCost - node.H; + node.H = minChildSumHAndOperatorCost; + ++_reversePathMaxBoosts; } // Forward Path-Max (Mero, 1984) foreach (var child in children) { - closedList.Add(child, child); + _closedList.Add(child, child); // Bequeath remainder of h from parent - int remainingParentH = parentH - (child.g - parentCost); - if (child.h < remainingParentH) + int remainingParentH = parentH - (child.G - parentCost); + if (child.H < remainingParentH) { - child.h = (ushort)remainingParentH; - this.pathMaxBoosts++; + child.H = (ushort)remainingParentH; + _pathMaxBoosts++; } // "Path-Max Plus" - if (node.minimumVertexCover > 0 && - (this.conflictChoice == ConflictChoice.CARDINAL_MDD || this.conflictChoice == ConflictChoice.CARDINAL_LOOKAHEAD || - this.conflictChoice == ConflictChoice.CARDINAL_MDD_THEN_MERGE_EARLY_MOST_CONFLICTING_SMALLEST_GROUP || - this.conflictChoice == ConflictChoice.CARDINAL_MDD_THEN_MERGE_EARLY_MOST_CONFLICTING_AND_SMALLEST_GROUP + if (node.MinimumVertexCover > 0 && + (ConflictChoice == ConflictChoice.CARDINAL_MDD || ConflictChoice == ConflictChoice.CARDINAL_LOOKAHEAD || + ConflictChoice == ConflictChoice.CARDINAL_MDD_THEN_MERGE_EARLY_MOST_CONFLICTING_SMALLEST_GROUP || + ConflictChoice == ConflictChoice.CARDINAL_MDD_THEN_MERGE_EARLY_MOST_CONFLICTING_AND_SMALLEST_GROUP )) { - child.h = (ushort)Math.Max(child.h, node.minimumVertexCover - 1); // -1 because we've just resolved a cardinal conflict. + child.H = (ushort)Math.Max(child.H, node.MinimumVertexCover - 1); // -1 because we've just resolved a cardinal conflict. // Even if the cost increased by more than 1 for the child, this is still our estimate, based on all the other conflicts - this.pathMaxPlusBoosts++; + _pathMaxPlusBoosts++; } - if (child.f <= this.maxSolutionCost) + if (child.F <= _maxSolutionCost) { - this.highLevelGenerated++; - this.addToGlobalConflictCount(child); // TODO: Make MACBS_WholeTreeThreshold use nodes that do this automatically after choosing a conflict - openList.Add(child); + _highLevelGenerated++; + addToGlobalConflictCount(child); // TODO: Make MACBS_WholeTreeThreshold use nodes that do this automatically after choosing a conflict + OpenList.Add(child); } else - this.surplusNodesAvoided++; + _surplusNodesAvoided++; } } public virtual void Expand(CbsNode node) { - if (this.bypassStrategy == BypassStrategy.BEST_FIT_LOOKAHEAD && node.parentAlreadyLookedAheadOf) + if (_bypassStrategy == BypassStrategy.BEST_FIT_LOOKAHEAD && node.ParentAlreadyLookedAheadOf) Debug.WriteLine("Not looking ahead from this node, one of its ancestors of the same cost was already looked ahead of"); - if (this.conflictChoice == ConflictChoice.FIRST || this.conflictChoice == ConflictChoice.MOST_CONFLICTING_SMALLEST_AGENTS) // Then just choose a conflict once and stick with it. + if (ConflictChoice == ConflictChoice.FIRST || ConflictChoice == ConflictChoice.MOST_CONFLICTING_SMALLEST_AGENTS) // Then just choose a conflict once and stick with it. { - this.ExpandIgnoringCardinalsButSupportingBP2(node); + ExpandIgnoringCardinalsButSupportingBP2(node); } else // We try to split according to cardinal conflicts { - Trace.Assert(bypassStrategy != BypassStrategy.BEST_FIT_LOOKAHEAD, "For simplicity, BP2 is not supported when choosing cardinal conflicts"); - this.IcbsExpand(node); + Trace.Assert(_bypassStrategy != BypassStrategy.BEST_FIT_LOOKAHEAD, "For simplicity, BP2 is not supported when choosing cardinal conflicts"); + IcbsExpand(node); } } /// /// - /// /// A ValueTuple with: /// child: null if planning the child's path failed or it was a closed list hit, otherwise - the new child /// closedListHitChildCost: Used to tell if conflict is cardinal, semi-cardinal or @@ -1573,12 +1542,12 @@ public virtual void Expand(CbsNode node) CbsConflict conflict = node.GetConflict(); int closedListHitChildCost = -1; - CbsNode child = new CbsNode(node, node.agentsGroupAssignment[conflict.agentAIndex], node.agentsGroupAssignment[conflict.agentBIndex]); - if (closedList.ContainsKey(child) == false) // We may have already merged these agents in another node + CbsNode child = new(node, node.AgentsGroupAssignment[conflict.agentAIndex], node.AgentsGroupAssignment[conflict.agentBIndex]); + if (_closedList.ContainsKey(child) == false) // We may have already merged these agents in another node { Debug.WriteLine($"Merging agents {conflict.agentAIndex} and {conflict.agentBIndex}"); - int aCost = node.GetGroupCost(node.agentsGroupAssignment[conflict.agentAIndex]); - int bCost = node.GetGroupCost(node.agentsGroupAssignment[conflict.agentBIndex]); + int aCost = node.GetGroupCost(node.AgentsGroupAssignment[conflict.agentAIndex]); + int bCost = node.GetGroupCost(node.AgentsGroupAssignment[conflict.agentBIndex]); int minNewCost; if (Constants.costFunction == Constants.CostFunction.SUM_OF_COSTS) minNewCost = aCost + bCost; @@ -1587,36 +1556,33 @@ public virtual void Expand(CbsNode node) minNewCost = Math.Max(aCost, bCost); else throw new NotImplementedException($"Unsupported cost function {Constants.costFunction}"); - if (this.useOldCost == false) + if (_useOldCost == false) minNewCost = -1; bool success = child.Replan(conflict.agentAIndex, // or agentBIndex. Doesn't matter - they're in the same group. - this.minSolutionTimeStep, minPathCost: minNewCost); + MinSolutionTimeStep, minPathCost: minNewCost); if (success == false) // A timeout probably occured return (child: null, closedListHitChildCost); Debug.WriteLine($"Child hash: {child.GetHashCode()}"); - Debug.WriteLine($"Child cost: {child.g}"); - Debug.WriteLine($"Child min ops to solve: {child.minOpsToSolve}"); + Debug.WriteLine($"Child cost: {child.G}"); + Debug.WriteLine($"Child min ops to solve: {child.MinOpsToSolve}"); Debug.WriteLine(""); - this.maxSizeGroup = Math.Max(this.maxSizeGroup, child.replanSize); + _maxSizeGroup = Math.Max(_maxSizeGroup, child.ReplanSize); return (child, closedListHitChildCost); } else { - closedListHits++; - return (child: null, closedListHitChildCost: closedList[child].g); + _closedListHits++; + return (child: null, closedListHitChildCost: _closedList[child].G); } } /// /// /// - /// - /// - /// /// A ValueTuple with: /// child: null if planning the child's path failed or it was a closed list hit, /// the parent node if expansion was deferred, or otherwise - the new child. @@ -1628,12 +1594,12 @@ public virtual void Expand(CbsNode node) { CbsConflict conflict = node.GetConflict(); int conflictingAgentIndex = doLeftChild? conflict.agentAIndex : conflict.agentBIndex; - CbsNode.ExpansionState expansionsState = doLeftChild ? node.agentAExpansion : node.agentBExpansion; - CbsNode.ExpansionState otherChildExpansionsState = doLeftChild ? node.agentBExpansion : node.agentAExpansion; + CbsNode.ExpansionState expansionsState = doLeftChild ? node.AgentAExpansion : node.AgentBExpansion; + CbsNode.ExpansionState otherChildExpansionsState = doLeftChild ? node.AgentBExpansion : node.AgentAExpansion; string agentSide = doLeftChild? "left" : "right"; - int planSize = node.singleAgentPlans[conflictingAgentIndex].GetSize(); // Used to check if the conflict occurs while the agent is at its goal + int planSize = node.SingleAgentPlans[conflictingAgentIndex].GetSize(); // Used to check if the conflict occurs while the agent is at its goal int groupSize = node.GetGroupSize(conflictingAgentIndex); - CbsConflict.WillCostIncrease willCostIncrease = doLeftChild ? node.conflict.willCostIncreaseForAgentA : node.conflict.willCostIncreaseForAgentB; + CbsConflict.WillCostIncrease willCostIncrease = doLeftChild ? node.Conflict.willCostIncreaseForAgentA : node.Conflict.willCostIncreaseForAgentB; // Check if expansion should be deferred if (Constants.costFunction == Constants.CostFunction.SUM_OF_COSTS && // Otherwise adding a constraint to an agent at a time step after @@ -1646,10 +1612,10 @@ public virtual void Expand(CbsNode node) ((Constants.sumOfCostsVariant == Constants.SumOfCostsVariant.ORIG && expansionsState == CbsNode.ExpansionState.NOT_EXPANDED && conflict.isVertexConflict == true && // An edge conflict at the goal isn't guaranteed // to increase the cost - the agent might be able to reach the goal from another direction - conflict.timeStep >= node.singleAgentCosts[conflictingAgentIndex] && // Can't just check whether the node is at its goal - + conflict.timeStep >= node.SingleAgentCosts[conflictingAgentIndex] && // Can't just check whether the node is at its goal - // the plan may involve it passing through its goal and returning to it later because of preexisting constraints. // This assumes unit move costs - node.h < conflict.timeStep + 1 - node.singleAgentCosts[conflictingAgentIndex] && // Otherwise we won't be increasing its h and there would be no reason to delay expansion + node.H < conflict.timeStep + 1 - node.SingleAgentCosts[conflictingAgentIndex] && // Otherwise we won't be increasing its h and there would be no reason to delay expansion // The agent's new cost will be at least conflict.timeStep + 1, so however much this is more than its current cost // is an admissible heuristic groupSize == 1) || // Otherwise an agent in the group can be forced to take a longer @@ -1657,8 +1623,8 @@ public virtual void Expand(CbsNode node) // another agent would be able to take a shorter route. (Constants.sumOfCostsVariant == Constants.SumOfCostsVariant.WAITING_AT_GOAL_ALWAYS_FREE && expansionsState == CbsNode.ExpansionState.NOT_EXPANDED && conflict.isVertexConflict == true && - ((conflict.timeStep > planSize - 1 && node.h < 2) || - (conflict.timeStep == planSize - 1 && node.h < 1)) && // Otherwise we won't be increasing its h and there would be no reason to delay expansion + ((conflict.timeStep > planSize - 1 && node.H < 2) || + (conflict.timeStep == planSize - 1 && node.H < 1)) && // Otherwise we won't be increasing its h and there would be no reason to delay expansion groupSize == 1))) // Otherwise an agent in the group can be forced to take a longer // route without increasing the group's cost because // another agent would be able to take a shorter route. @@ -1685,27 +1651,27 @@ public virtual void Expand(CbsNode node) Debug.WriteLine($"Skipping {agentSide} child for now"); if (doLeftChild) - node.agentAExpansion = CbsNode.ExpansionState.DEFERRED; + node.AgentAExpansion = CbsNode.ExpansionState.DEFERRED; else - node.agentBExpansion = CbsNode.ExpansionState.DEFERRED; + node.AgentBExpansion = CbsNode.ExpansionState.DEFERRED; // Add the minimal delta in the child's cost: // since we're banning the goal at conflict.timeStep, it must at least do conflict.timeStep+1 steps if (Constants.sumOfCostsVariant == Constants.SumOfCostsVariant.ORIG) { - node.h = Math.Max(node.h, (ushort)(conflict.timeStep + 1 - node.singleAgentCosts[conflictingAgentIndex])); + node.H = Math.Max(node.H, (ushort)(conflict.timeStep + 1 - node.SingleAgentCosts[conflictingAgentIndex])); // Technically, we've already made sure above we're going to increase the node's h, // This is just to make the line look correct without reading the complex if statement above. } else if (Constants.sumOfCostsVariant == Constants.SumOfCostsVariant.WAITING_AT_GOAL_ALWAYS_FREE) { if (conflict.timeStep > planSize - 1) // Agent will need to step out and step in to the goal, at least - node.h = Math.Max(node.h, (ushort) 1); + node.H = Math.Max(node.H, (ushort) 1); else // Conflict is just when agent enters the goal, it'll have to at least wait one timestep. - node.h = Math.Max(node.h, (ushort) 1); + node.H = Math.Max(node.H, (ushort) 1); // Technically, we've already made sure above we're going to increase the node's h, // This is just to make the line look correct without reading the complex if statement above. } - this.partialExpansions++; // The other child was surely generated, so we can count a partial expansion here. + _partialExpansions++; // The other child was surely generated, so we can count a partial expansion here. return (node, closedListHitChildCost: -1); } else if (expansionsState != CbsNode.ExpansionState.EXPANDED) @@ -1714,26 +1680,26 @@ public virtual void Expand(CbsNode node) Debug.WriteLine($"Generating {agentSide} child"); if (doLeftChild) - node.agentAExpansion = CbsNode.ExpansionState.EXPANDED; + node.AgentAExpansion = CbsNode.ExpansionState.EXPANDED; else - node.agentBExpansion = CbsNode.ExpansionState.EXPANDED; + node.AgentBExpansion = CbsNode.ExpansionState.EXPANDED; - var newConstraint = new CbsConstraint(conflict, instance, doLeftChild); - CbsNode child = new CbsNode(node, newConstraint, conflictingAgentIndex); + CbsConstraint newConstraint = new(conflict, _instance, doLeftChild); + CbsNode child = new(node, newConstraint, conflictingAgentIndex); - if (this.doMalte && doLeftChild == false) + if (DoMalte && doLeftChild == false) { // Add the case where both agents shouldn't be at the conflict point to the _left_ child // by forcing the first agent in the _right_ child to _always_ be at the conflict point. // Notice that vertex conflicts create must constraint with NO_DIRECTION and edge conflicts // don't, as necessary. - var newMustConstraint = new CbsConstraint(conflict, instance, true); + CbsConstraint newMustConstraint = new(conflict, _instance, true); child.SetMustConstraint(newMustConstraint); } - if (closedList.ContainsKey(child) == false) + if (_closedList.ContainsKey(child) == false) { - int oldCost = node.GetGroupCost(node.agentsGroupAssignment[conflictingAgentIndex]); + int oldCost = node.GetGroupCost(node.AgentsGroupAssignment[conflictingAgentIndex]); int minNewCost = oldCost; if (willCostIncrease == CbsConflict.WillCostIncrease.YES) minNewCost = oldCost + 1; @@ -1742,46 +1708,46 @@ public virtual void Expand(CbsNode node) if (willCostIncrease == CbsConflict.WillCostIncrease.NO) maxNewCost = oldCost; - if (this.useOldCost == false) + if (_useOldCost == false) { minNewCost = -1; maxNewCost = int.MaxValue; } - bool success = child.Replan(conflictingAgentIndex, this.minSolutionTimeStep, // The node takes the max between minSolutionTimeStep and the max time over all constraints. + bool success = child.Replan(conflictingAgentIndex, MinSolutionTimeStep, // The node takes the max between minSolutionTimeStep and the max time over all constraints. minPathCost: minNewCost, maxPathCost: maxNewCost); if (success == false) return (null, closedListHitChildCost: -1); // A timeout probably occured Debug.WriteLine($"Child hash: {child.GetHashCode()}"); - Debug.WriteLine($"Child cost: {child.g}"); - Debug.WriteLine($"Child min ops to solve: {child.minOpsToSolve}"); - Debug.WriteLine($"Child num of agents that conflict: {child.totalInternalAgentsThatConflict}"); - Debug.WriteLine($"Child num of internal conflicts: {child.totalConflictsBetweenInternalAgents}"); + Debug.WriteLine($"Child cost: {child.G}"); + Debug.WriteLine($"Child min ops to solve: {child.MinOpsToSolve}"); + Debug.WriteLine($"Child num of agents that conflict: {child.TotalInternalAgentsThatConflict}"); + Debug.WriteLine($"Child num of internal conflicts: {child.TotalConflictsBetweenInternalAgents}"); Debug.WriteLine(""); - if (child.g < node.g && groupSize == 1) // Catch the error early + if (child.G < node.G && groupSize == 1) // Catch the error early { child.DebugPrint(); - Debug.WriteLine("Child plan: (cost {0})", child.singleAgentCosts[conflictingAgentIndex]); - child.singleAgentPlans[conflictingAgentIndex].DebugPrint(); - Debug.WriteLine("Parent plan: (cost {0})", node.singleAgentCosts[conflictingAgentIndex]); - node.singleAgentPlans[conflictingAgentIndex].DebugPrint(); - Trace.Assert(false, $"Single agent node with lower cost than parent! {child.g} < {node.g}"); + Debug.WriteLine("Child plan: (cost {0})", child.SingleAgentCosts[conflictingAgentIndex]); + child.SingleAgentPlans[conflictingAgentIndex].DebugPrint(); + Debug.WriteLine("Parent plan: (cost {0})", node.SingleAgentCosts[conflictingAgentIndex]); + node.SingleAgentPlans[conflictingAgentIndex].DebugPrint(); + Trace.Assert(false, $"Single agent node with lower cost than parent! {child.G} < {node.G}"); } return (child, closedListHitChildCost: -1); } else { - this.closedListHits++; + _closedListHits++; Debug.WriteLine("Child already in closed list!"); // No need to check if we need to reopen - reopening is only necessary if F is lower, // but for the same constraints and group assignment we're going to have the same // g, and the h in the closed list can only be higher (if it was already expanded // and the open list's expensive heuristic was computed for it). - return (child: null, closedListHitChildCost: this.closedList[child].g); + return (child: null, closedListHitChildCost: _closedList[child].G); } } else @@ -1794,22 +1760,19 @@ public virtual void Expand(CbsNode node) /// /// /// - /// - /// - /// /// Whether the candidate's solution was adopted protected bool AdoptConditionally(CbsNode node, CbsNode adoptionCandidate, int nodeOrigH) { Debug.WriteLine($"Considering adoption of node hash: {adoptionCandidate.GetHashCode()}."); - if (adoptionCandidate.g == node.g && // No need to branch :) + if (adoptionCandidate.G == node.G && // No need to branch :) adoptionCandidate.TieBreak(node, ignorePartialExpansion: true, ignoreDepth: true) == -1 ) { - this.conflictsBypassed += node.totalConflictsWithExternalAgents + node.totalConflictsBetweenInternalAgents - - adoptionCandidate.totalConflictsWithExternalAgents - adoptionCandidate.totalConflictsBetweenInternalAgents; + _conflictsBypassed += node.TotalConflictsWithExternalAgents + node.TotalConflictsBetweenInternalAgents + - adoptionCandidate.TotalConflictsWithExternalAgents - adoptionCandidate.TotalConflictsBetweenInternalAgents; node.AdoptSolutionOf(adoptionCandidate); - node.h = nodeOrigH; // Cancel partial expansion h boost. This wasn't a cardinal conflict, so h could only have been from a partial expansion. + node.H = nodeOrigH; // Cancel partial expansion h boost. This wasn't a cardinal conflict, so h could only have been from a partial expansion. // FIXME: when semi-cardinal and non-cardinal conflicts start contributing to h, this won't be correct Debug.WriteLine("Child has same cost as parent and a better solution - child solution adopted by parent! (other generated children and partial expansions cancelled)"); @@ -1828,34 +1791,28 @@ protected virtual void addToGlobalConflictCount(CbsNode node) { } public virtual Plan GetPlan() { - if (this.solution == null) - this.solution = this.goalNode.CalculateJointPlan(); - return this.solution; + if (_solution == null) + _solution = _goalNode.CalculateJointPlan(); + return _solution; } - public int GetSolutionDepth() { return this.solutionDepth; } + public int GetSolutionDepth() { return _solutionDepth; } public long GetMemoryUsed() { return Process.GetCurrentProcess().VirtualMemorySize64; } - public SinglePlan[] GetSinglePlans() - { - return goalNode.singleAgentPlans; - } - - public virtual int[] GetSingleCosts() - { - return goalNode.singleAgentCosts; - } - - public int GetHighLevelExpanded() { return highLevelExpanded; } - public int GetHighLevelGenerated() { return highLevelGenerated; } - public int GetLowLevelExpanded() { return this.solver.GetAccumulatedExpanded(); } - public int GetLowLevelGenerated() { return this.solver.GetAccumulatedGenerated(); } - public int GetExpanded() { return highLevelExpanded; } - public int GetGenerated() { return highLevelGenerated; } - public int GetAccumulatedExpanded() { return accHLExpanded; } - public int GetAccumulatedGenerated() { return accHLGenerated; } - public int GetMaxGroupSize() { return this.maxSizeGroup; } + public SinglePlan[] GetSinglePlans() => _goalNode.SingleAgentPlans; + + public virtual int[] GetSingleCosts() => _goalNode.SingleAgentCosts; + + public int GetHighLevelExpanded() => _highLevelExpanded; + public int GetHighLevelGenerated() => _highLevelGenerated; + public int GetLowLevelExpanded() => _solver.GetAccumulatedExpanded(); + public int GetLowLevelGenerated() => _solver.GetAccumulatedGenerated(); + public int GetExpanded() => _highLevelExpanded; + public int GetGenerated() => _highLevelGenerated; + public int GetAccumulatedExpanded() => _accHLExpanded; + public int GetAccumulatedGenerated() => _accHLGenerated; + public int GetMaxGroupSize() => _maxSizeGroup; } /// @@ -1893,26 +1850,26 @@ public MACBS_WholeTreeThreshold(ICbsSolver singleAgentSolver, ICbsSolver general /// public override void Setup(ProblemInstance problemInstance, Run runner) { - this.MakeConflictMatrix(problemInstance); + MakeConflictMatrix(problemInstance); base.Setup(problemInstance, runner); } private void MakeConflictMatrix(ProblemInstance problemInstance) { - this.globalConflictsCounter = new int[problemInstance.agents.Length][]; + globalConflictsCounter = new int[problemInstance.agents.Length][]; for (int i = 0; i < globalConflictsCounter.Length; i++) { - this.globalConflictsCounter[i] = new int[i]; + globalConflictsCounter[i] = new int[i]; for (int j = 0; j < i; j++) { - this.globalConflictsCounter[i][j] = 0; + globalConflictsCounter[i][j] = 0; } } } protected override bool ShouldMerge(CbsNode node) { - return node.ShouldMerge(mergeThreshold, globalConflictsCounter, node.conflict.agentAIndex, node.conflict.agentBIndex); //TODO: Add an option to choose the old merge criterion + return node.ShouldMerge(MergeThreshold, globalConflictsCounter, node.Conflict.agentAIndex, node.Conflict.agentBIndex); //TODO: Add an option to choose the old merge criterion } /// @@ -1927,11 +1884,11 @@ protected override void addToGlobalConflictCount(CbsConflict conflict) protected override void addToGlobalConflictCount(CbsNode node) { - if (this.mergeCausesRestart == false) + if (_mergeCausesRestart == false) { // Old logic: - if (node.conflict != null) - globalConflictsCounter[Math.Max(node.conflict.agentAIndex, node.conflict.agentBIndex)][Math.Min(node.conflict.agentAIndex, node.conflict.agentBIndex)]++; + if (node.Conflict != null) + globalConflictsCounter[Math.Max(node.Conflict.agentAIndex, node.Conflict.agentBIndex)][Math.Min(node.Conflict.agentAIndex, node.Conflict.agentBIndex)]++; // This only looks at conflicts between individual agents - doesn't take into account merges. // Merges are local when merge & restart is off so the global counts might be incorrect. // FIXME: Currently only looks at the chosen conflict. @@ -1939,16 +1896,16 @@ protected override void addToGlobalConflictCount(CbsNode node) else { //TODO: Add an option to use the old logic above here too - for (int i = 0; i < this.GetProblemInstance().GetNumOfAgents(); i++) + for (int i = 0; i < GetProblemInstance().GetNumOfAgents(); i++) { - if (node.newPlans[i] == false) + if (node.NewPlans[i] == false) continue; - foreach (var kvp in node.conflictCountsPerAgent[i]) + foreach (var kvp in node.ConflictCountsPerAgent[i]) { if (i > kvp.Key) continue; - int groupRepA = node.agentsGroupAssignment[i]; - int groupRepB = node.agentsGroupAssignment[kvp.Key]; + int groupRepA = node.AgentsGroupAssignment[i]; + int groupRepB = node.AgentsGroupAssignment[kvp.Key]; if (groupRepA > groupRepB) globalConflictsCounter[groupRepA][groupRepB] += kvp.Value; else @@ -1967,6 +1924,6 @@ public override string GetName() public override void Reset() { base.Reset(); - //this.MakeConflictMatrix(this.instance); + //MakeConflictMatrix(instance); } } diff --git a/CbsCacheEntry.cs b/CbsCacheEntry.cs index 9dd3cd7..27df2cd 100644 --- a/CbsCacheEntry.cs +++ b/CbsCacheEntry.cs @@ -12,7 +12,7 @@ public CbsCacheEntry(CbsNode cbsNode, int agentIndex) { this.cbsNode = cbsNode; this.agentIndex = agentIndex; - Trace.Assert(cbsNode.cbs.mergeThreshold == -1, "When agents are merged it affects their paths without explicit constraints"); + Trace.Assert(cbsNode.CBS.MergeThreshold == -1, "When agents are merged it affects their paths without explicit constraints"); } public override int GetHashCode() diff --git a/CbsHeuristicForAStar.cs b/CbsHeuristicForAStar.cs index c4b0a22..0eb39ff 100644 --- a/CbsHeuristicForAStar.cs +++ b/CbsHeuristicForAStar.cs @@ -72,7 +72,7 @@ public uint h(WorldState s) throw new NotImplementedException($"Unsupported cost function {Constants.costFunction}"); if (sicEstimate == 0) // Only the goal has an estimate of zero return 0; - int targetCost = s.g + sicEstimate + this.minAboveSic; // Ariel's idea - using SIC directly here to calc the target + int targetCost = s.G + sicEstimate + this.minAboveSic; // Ariel's idea - using SIC directly here to calc the target // CBS gets an explicitly partially solved state - the agents' g may be greater than zero. // So the cost CBS is going to calc is not of this node but of the initial problem instance, // this is accounted for later too. @@ -108,7 +108,7 @@ protected uint h(WorldState s, int targetCost, int sicEstimate=-1, int lowLevelG s.minGoalTimeStep), // No point in finding shallower goal nodes this.runner, null, null, positiveConstraints); - if (this.cbs.openList.Count > 0 && this.cbs.externalCAT == null) + if (this.cbs.OpenList.Count > 0 && this.cbs.ExternalCAT == null) { if (sicEstimate == -1) { @@ -120,7 +120,7 @@ protected uint h(WorldState s, int targetCost, int sicEstimate=-1, int lowLevelG throw new NotImplementedException($"Unsupported cost function {Constants.costFunction}"); } - Trace.Assert(((CbsNode)this.cbs.openList.Peek()).g - s.g == (int)sicEstimate, + Trace.Assert(((CbsNode)this.cbs.OpenList.Peek()).G - s.G == (int)sicEstimate, "Total cost of CBS root not same as SIC + g"); // Notice we're subtracting s.g, not sAsProblemInstance.g. // Must constraints we put may have forced some moves, @@ -137,9 +137,9 @@ protected uint h(WorldState s, int targetCost, int sicEstimate=-1, int lowLevelG } // Calc the h: - this.cbs.targetF = targetCost; - this.cbs.milliCap = milliCap; - this.cbs.lowLevelGeneratedCap = lowLevelGeneratedCap; + this.cbs.TargetF = targetCost; + this.cbs.MilliCap = milliCap; + this.cbs.LowLevelGeneratedCap = lowLevelGeneratedCap; bool solved = this.cbs.Solve(); @@ -147,7 +147,7 @@ protected uint h(WorldState s, int targetCost, int sicEstimate=-1, int lowLevelG { // We're always going to find a proper goal since we respected the node's minDepth s.SetSolution(this.cbs.GetSinglePlans()); - s.SetGoalCost(this.cbs.solutionCost); // We have to do it explicitly. + s.SetGoalCost(this.cbs.SolutionCost); // We have to do it explicitly. // We can't just change the node's g to g + cbs.g and its h to zero // because approaches like BPMX or maximazing PDBs might "fix" the h back. // So instead h is bumped to its maximum value when this method returns. @@ -162,7 +162,7 @@ protected uint h(WorldState s, int targetCost, int sicEstimate=-1, int lowLevelG this.cbs.AccumulateStatistics(); this.cbs.ClearStatistics(); - if (this.cbs.solutionCost < 0) // A timeout is legitimately possible if very little time was left to begin with, + if (this.cbs.SolutionCost < 0) // A timeout is legitimately possible if very little time was left to begin with, // and a no solution failure may theoretically be possible too. { if (Constants.costFunction == Constants.CostFunction.SUM_OF_COSTS) @@ -173,16 +173,16 @@ protected uint h(WorldState s, int targetCost, int sicEstimate=-1, int lowLevelG throw new NotImplementedException($"Unsupported cost function {Constants.costFunction}"); } - Trace.Assert(this.cbs.solutionCost >= s.g, - $"CBS total cost {this.cbs.solutionCost} is smaller than starting problem's initial cost {s.g}."); // = is allowed since even though this isn't a goal node (otherwise this function won't be called), + Trace.Assert(this.cbs.SolutionCost >= s.G, + $"CBS total cost {this.cbs.SolutionCost} is smaller than starting problem's initial cost {s.G}."); // = is allowed since even though this isn't a goal node (otherwise this function won't be called), // a non-goal node can have h==0 if a minimum depth is specified, and all agents have reached their // goal in this node, but the depth isn't large enough. - uint cbsEstimate = (uint)(this.cbs.solutionCost - s.g); + uint cbsEstimate = (uint)(this.cbs.SolutionCost - s.G); // Discounting the moves the agents did before we started solving // (This is easier than making a copy of each AgentState just to zero its lastMove.time) - this.totalImprovement += (int)(cbsEstimate - s.h); // Not computing difference from SIC to not over-count, since a node can be improved twice. + this.totalImprovement += (int)(cbsEstimate - s.H); // Not computing difference from SIC to not over-count, since a node can be improved twice. // Can be negative if the base heuristic was improved by: // - Partial expansion // - BPMX @@ -203,7 +203,7 @@ protected uint h(WorldState s, int targetCost, int sicEstimate=-1, int lowLevelG epeastarsic.Setup(sAsProblemInstance, s.makespan, runner); bool epeastarsicSolved = epeastarsic.Solve(); if (epeastarsicSolved) - Trace.Assert(epeastarsic.totalCost - s.g >= this.cbs.solutionCost - s.g, "Inadmissible!!"); + Trace.Assert(epeastarsic.totalCost - s.G >= this.cbs.SolutionCost - s.G, "Inadmissible!!"); } return cbsEstimate; @@ -394,7 +394,7 @@ public uint h(WorldState s, int targetH, float effectiveBranchingFactor) // No need to check if SIC is zero because this heuristic is run after SIC was already computed, not instead of it. int lowLevelGeneratedCap = (int) Math.Round(effectiveBranchingFactor * this.instance.agents.Length); // Cap of B_of_AStar * K, // because CBS low level nodes are of one agent only so they're about k times cheaper to work with - return base.h(s, s.g + targetH, -1, lowLevelGeneratedCap); + return base.h(s, s.G + targetH, -1, lowLevelGeneratedCap); } /// @@ -409,7 +409,7 @@ public uint h(WorldState s, int targetH, float effectiveBranchingFactor) public uint h(WorldState s, int targetH, float effectiveBranchingFactor, int millisCap, bool resume) { // No need to check if SIC is zero because this heuristic is run after SIC was already computed, not instead of it. - return this.h(s, s.g + targetH, -1, int.MaxValue, millisCap, resume); + return this.h(s, s.G + targetH, -1, int.MaxValue, millisCap, resume); } public override string ToString() diff --git a/CbsNode.cs b/CbsNode.cs index f225d6d..f6cbe02 100644 --- a/CbsNode.cs +++ b/CbsNode.cs @@ -4,67 +4,69 @@ using System.Diagnostics; using ExtensionMethods; +using System.Collections; namespace mapf; [DebuggerDisplay("hash = {GetHashCode()}, f = {f}, g = {g}, h = {h}")] public class CbsNode : IComparable, IBinaryHeapItem, IHeuristicSearchNode { - public int g { set; get; } // Value depends on Constants.costFunction and Constants.sumOfCostsVariant, Sum of agent makespans until they reach their goal - public int h { get; set; } - public int hBonus { get; set; } + public int G { set; get; } // Value depends on Constants.costFunction and Constants.sumOfCostsVariant, Sum of agent makespans until they reach their goal + + public int H { get; set; } + + public int HBonus { get; set; } + /// /// The size of the minimum vertex cover of the node's cardinal conflict graph. /// Needs to be saved separately from h to allow speeding up the computation of the heuristic /// of the children. /// - public int minimumVertexCover; - public bool[] newPlans; - public SinglePlan[] singleAgentPlans; - public int[] singleAgentCosts; + public int MinimumVertexCover { get; set; } + public BitArray NewPlans { get; } + public SinglePlan[] SingleAgentPlans { get; private set; } + public int[] SingleAgentCosts { get; private set; } /// /// A lower estimate of the number of operations (replanning or merging) needed to solve the node. /// Used for tie-breaking. /// - public int minOpsToSolve; + public int MinOpsToSolve { get; private set; } /// /// For each agent in the problem instance, saves the number of agents from the problem instance that it conflicts with. /// Used for choosing the next conflict to resolve by replanning/merging/shuffling, and for tie-breaking. /// - public int[] countsOfInternalAgentsThatConflict; + private int[] _countsOfInternalAgentsThatConflict; /// /// Counts the number of external agents this node conflicts with. /// Used for tie-breaking. /// - public int totalExternalAgentsThatConflict; + private int _totalExternalAgentsThatConflict; /// /// Used for tie-breaking. /// - public int totalConflictsWithExternalAgents; + public int TotalConflictsWithExternalAgents { get; private set; } /// /// For each agent in the problem instance, maps agent _nums_ it conflicts with, internal or external, /// to the number of conflicts betweem them. /// Used for book-keeping to maintain countsOfInternalAgentsThatConflict, /// totalExternalAgentsThatConflict and minOpsToSolve, and other counts. /// - public Dictionary[] conflictCountsPerAgent; + public Dictionary[] ConflictCountsPerAgent { get; private set; } /// /// For each agent in the problem instance, maps agent _nums_ of agents it collides with to the time of their first collision. /// - public Dictionary>[] conflictTimesPerAgent; - private int binaryHeapIndex; - public CbsConflict conflict; - //public ISet externalConstraints; - //public ISet externalPositiveConstraints; - public CbsConstraint constraint; + public Dictionary>[] ConflictTimesPerAgent { get; private set; } + private int _binaryHeapIndex; + public CbsConflict Conflict { get; private set; } + private CbsConstraint _constraint; /// /// Forcing an agent to be at a certain place at a certain time /// - CbsConstraint mustConstraint; - public CbsNode prev; - public ushort depth; - public ushort[] agentsGroupAssignment; - public ushort replanSize; + private CbsConstraint _mustConstraint; + public CbsNode Prev { get; } + private ushort _depth; + public ushort[] AgentsGroupAssignment { get; } + public ushort ReplanSize { get; private set; } public enum ExpansionState: byte { NOT_EXPANDED = 0, @@ -74,40 +76,40 @@ public enum ExpansionState: byte /// /// For partial expansion /// - public ExpansionState agentAExpansion; + public ExpansionState AgentAExpansion { get; set; } /// /// For partial expansion /// - public ExpansionState agentBExpansion; - protected ICbsSolver solver; - protected ICbsSolver singleAgentSolver; - public CBS cbs; - public Dictionary agentNumToIndex; - public bool parentAlreadyLookedAheadOf; + public ExpansionState AgentBExpansion { get; set; } + private ICbsSolver _solver; + private ICbsSolver _singleAgentSolver; + public CBS CBS { get; private set; } + public Dictionary AgentNumToIndex { get; private set; } + public bool ParentAlreadyLookedAheadOf { get; set; } /// /// For tie-breaking /// - public int totalInternalAgentsThatConflict; + public int TotalInternalAgentsThatConflict { get; private set; } /// /// For tie-breaking /// - public int largerConflictingGroupSize; + private int _largerConflictingGroupSize; // TODO: is it really used? /// /// For tie-breaking /// - public int totalConflictsBetweenInternalAgents; + public int TotalConflictsBetweenInternalAgents { get; private set; } /// /// For each agent, map each level (timestep) of its mdd to a narrowness degree. /// Non-narrow levels are omitted. /// - public Dictionary[] mddNarrownessValues; + public Dictionary[] MDDNarrownessValues { get; } /// /// FIXME: We're currently saving both the MDDs and their much smaller narrowness values in /// order to have a fair comparison with the past /// - public MDD[] mdds; + private MDD[] _mdds; /// /// Root node constructor @@ -120,37 +122,37 @@ public enum ExpansionState: byte public CbsNode(int numberOfAgents, ICbsSolver solver, ICbsSolver singleAgentSolver, CBS cbs, ushort[] agentsGroupAssignment = null, ISet externalConstraints = null, ISet externalPositiveConstraints = null) { - this.cbs = cbs; - singleAgentPlans = new SinglePlan[numberOfAgents]; - newPlans = new bool[numberOfAgents]; - singleAgentCosts = new int[numberOfAgents]; - mddNarrownessValues = new Dictionary[numberOfAgents]; - mdds = new MDD[numberOfAgents]; - countsOfInternalAgentsThatConflict = new int[numberOfAgents]; - conflictCountsPerAgent = new Dictionary[numberOfAgents]; // Populated after Solve() - conflictTimesPerAgent = new Dictionary>[numberOfAgents]; // Populated after Solve() + CBS = cbs; + SingleAgentPlans = new SinglePlan[numberOfAgents]; + NewPlans = new BitArray(numberOfAgents); + SingleAgentCosts = new int[numberOfAgents]; + MDDNarrownessValues = new Dictionary[numberOfAgents]; + _mdds = new MDD[numberOfAgents]; + _countsOfInternalAgentsThatConflict = new int[numberOfAgents]; + ConflictCountsPerAgent = new Dictionary[numberOfAgents]; // Populated after Solve() + ConflictTimesPerAgent = new Dictionary>[numberOfAgents]; // Populated after Solve() if (agentsGroupAssignment == null) { - this.agentsGroupAssignment = new ushort[numberOfAgents]; + AgentsGroupAssignment = new ushort[numberOfAgents]; for (ushort i = 0; i < numberOfAgents; i++) - this.agentsGroupAssignment[i] = i; + AgentsGroupAssignment[i] = i; } else - this.agentsGroupAssignment = agentsGroupAssignment.ToArray(); - agentNumToIndex = new Dictionary(); + AgentsGroupAssignment = [.. agentsGroupAssignment]; + AgentNumToIndex = []; for (int i = 0; i < numberOfAgents; i++) { - agentNumToIndex[this.cbs.GetProblemInstance().agents[i].agent.agentNum] = i; - } - depth = 0; - replanSize = 1; - agentAExpansion = ExpansionState.NOT_EXPANDED; - agentBExpansion = ExpansionState.NOT_EXPANDED; - this.prev = null; - this.constraint = null; - this.solver = solver; - this.singleAgentSolver = singleAgentSolver; - this.minimumVertexCover = (int) ConflictGraph.MinVertexCover.NOT_SET; + AgentNumToIndex[CBS.GetProblemInstance().agents[i].agent.agentNum] = i; + } + _depth = 0; + ReplanSize = 1; + AgentAExpansion = ExpansionState.NOT_EXPANDED; + AgentBExpansion = ExpansionState.NOT_EXPANDED; + Prev = null; + _constraint = null; + _solver = solver; + _singleAgentSolver = singleAgentSolver; + MinimumVertexCover = (int) ConflictGraph.MinVertexCover.NOT_SET; } /// @@ -161,67 +163,66 @@ public CbsNode(int numberOfAgents, ICbsSolver solver, ICbsSolver singleAgentSolv /// public CbsNode(CbsNode parent, CbsConstraint newConstraint, int agentToReplan) { - this.cbs = parent.cbs; - this.singleAgentPlans = parent.singleAgentPlans.ToArray(); - newPlans = new bool[this.singleAgentPlans.Length]; - this.singleAgentCosts = parent.singleAgentCosts.ToArray(); - this.mdds = parent.mdds.ToArray(); - this.mddNarrownessValues = parent.mddNarrownessValues.ToArray(); + CBS = parent.CBS; + SingleAgentPlans = [.. parent.SingleAgentPlans]; + NewPlans = new BitArray(SingleAgentPlans.Length); + SingleAgentCosts = [.. parent.SingleAgentCosts]; + _mdds = [.. parent._mdds]; + MDDNarrownessValues = [.. parent.MDDNarrownessValues]; // Adapt the MDDs for the agent to replan, if possible // The cost may increase, so the old MDD might not be relevant anymore. - if (this.mdds[agentToReplan] != null && - this.mdds[agentToReplan].levels.Length - 1 > newConstraint.time && - (this.mddNarrownessValues[agentToReplan].ContainsKey(newConstraint.time) == false || - (this.mddNarrownessValues[agentToReplan][newConstraint.time] == MDD.LevelNarrowness.ONE_LOCATION_MULTIPLE_DIRECTIONS && + if (_mdds[agentToReplan] != null && + _mdds[agentToReplan].levels.Length - 1 > newConstraint.time && + (MDDNarrownessValues[agentToReplan].ContainsKey(newConstraint.time) == false || + (MDDNarrownessValues[agentToReplan][newConstraint.time] == MDD.LevelNarrowness.ONE_LOCATION_MULTIPLE_DIRECTIONS && newConstraint.move.Direction != Direction.NO_DIRECTION))) { // We have an MDD and same cost can still be achieved - adapt the existing MDD - double startTime = this.cbs.runner.ElapsedMilliseconds(); - this.mdds[agentToReplan] = new MDD(this.mdds[agentToReplan], newConstraint); - this.mddNarrownessValues[agentToReplan] = this.mdds[agentToReplan].getLevelNarrownessValues(); - double endTime = this.cbs.runner.ElapsedMilliseconds(); - this.cbs.mddsAdapted++; - this.cbs.timeBuildingMdds += endTime - startTime; + double startTime = CBS._runner.ElapsedMilliseconds(); + _mdds[agentToReplan] = new MDD(_mdds[agentToReplan], newConstraint); + MDDNarrownessValues[agentToReplan] = _mdds[agentToReplan].getLevelNarrownessValues(); + double endTime = CBS._runner.ElapsedMilliseconds(); + CBS.MDDsAdapted++; + CBS.TimeBuildingMdds += endTime - startTime; } else { - this.mdds[agentToReplan] = null; - this.mddNarrownessValues[agentToReplan] = null; + _mdds[agentToReplan] = null; + MDDNarrownessValues[agentToReplan] = null; } - this.countsOfInternalAgentsThatConflict = parent.countsOfInternalAgentsThatConflict.ToArray(); - this.conflictCountsPerAgent = new Dictionary[parent.conflictCountsPerAgent.Length]; - for (int i = 0; i < this.conflictCountsPerAgent.Length; i++) - this.conflictCountsPerAgent[i] = new Dictionary(parent.conflictCountsPerAgent[i]); // Need a separate copy because unlike plans, the conflict counts for agents that aren't replanned do change. - this.conflictTimesPerAgent = new Dictionary>[parent.conflictTimesPerAgent.Length]; - for (int i = 0; i < this.conflictTimesPerAgent.Length; i++) + _countsOfInternalAgentsThatConflict = parent._countsOfInternalAgentsThatConflict.ToArray(); + ConflictCountsPerAgent = new Dictionary[parent.ConflictCountsPerAgent.Length]; + for (int i = 0; i < ConflictCountsPerAgent.Length; i++) + ConflictCountsPerAgent[i] = new Dictionary(parent.ConflictCountsPerAgent[i]); // Need a separate copy because unlike plans, the conflict counts for agents that aren't replanned do change. + ConflictTimesPerAgent = new Dictionary>[parent.ConflictTimesPerAgent.Length]; + for (int i = 0; i < ConflictTimesPerAgent.Length; i++) { - this.conflictTimesPerAgent[i] = new Dictionary>(); // Need a separate copy because unlike plans, the conflict counts for agents that aren't replanned do change. - foreach (var kvp in parent.conflictTimesPerAgent[i]) - this.conflictTimesPerAgent[i][kvp.Key] = new List(kvp.Value); + ConflictTimesPerAgent[i] = []; // Need a separate copy because unlike plans, the conflict counts for agents that aren't replanned do change. + foreach (var kvp in parent.ConflictTimesPerAgent[i]) + ConflictTimesPerAgent[i][kvp.Key] = [.. kvp.Value]; } - this.agentsGroupAssignment = parent.agentsGroupAssignment.ToArray(); + AgentsGroupAssignment = parent.AgentsGroupAssignment.ToArray(); - for (int i = 0; i < this.singleAgentPlans.Length; i++) + for (int i = 0; i < SingleAgentPlans.Length; i++) { - newPlans[i] = false; + NewPlans[i] = false; } - ISet group = this.GetGroup(agentToReplan); + ISet group = GetGroup(agentToReplan); foreach (int i in group) - newPlans[i] = true; + NewPlans[i] = true; - this.agentNumToIndex = parent.agentNumToIndex; - this.prev = parent; - this.constraint = newConstraint; - this.depth = (ushort)(this.prev.depth + 1); - this.agentAExpansion = ExpansionState.NOT_EXPANDED; - this.agentBExpansion = ExpansionState.NOT_EXPANDED; - this.replanSize = 1; - this.solver = parent.solver; - this.singleAgentSolver = parent.singleAgentSolver; - this.minimumVertexCover = (int) ConflictGraph.MinVertexCover.NOT_SET; - //this.externalConstraints = parent.externalConstraints; + AgentNumToIndex = parent.AgentNumToIndex; + Prev = parent; + _constraint = newConstraint; + _depth = (ushort)(Prev._depth + 1); + AgentAExpansion = ExpansionState.NOT_EXPANDED; + AgentBExpansion = ExpansionState.NOT_EXPANDED; + ReplanSize = 1; + _solver = parent._solver; + _singleAgentSolver = parent._singleAgentSolver; + MinimumVertexCover = (int) ConflictGraph.MinVertexCover.NOT_SET; } /// @@ -232,88 +233,83 @@ public CbsNode(CbsNode parent, CbsConstraint newConstraint, int agentToReplan) /// public CbsNode(CbsNode parent, int mergeGroupA, int mergeGroupB) { - this.singleAgentPlans = parent.singleAgentPlans.ToArray(); - newPlans = new bool[this.singleAgentPlans.Length]; - this.singleAgentCosts = parent.singleAgentCosts.ToArray(); - this.mdds = parent.mdds.ToArray(); - this.mddNarrownessValues = parent.mddNarrownessValues.ToArray(); // No new constraint was added so all of the parent's MDDs are valid - this.countsOfInternalAgentsThatConflict = parent.countsOfInternalAgentsThatConflict.ToArray(); - this.conflictCountsPerAgent = new Dictionary[parent.conflictCountsPerAgent.Length]; - for (int i = 0; i < this.conflictCountsPerAgent.Length; i++) - this.conflictCountsPerAgent[i] = new Dictionary(parent.conflictCountsPerAgent[i]); // Need a separate copy because unlike plans, the conflict counts for agents that aren't replanned do change. - this.conflictTimesPerAgent = new Dictionary>[parent.conflictTimesPerAgent.Length]; - for (int i = 0; i < this.conflictTimesPerAgent.Length; i++) - { - this.conflictTimesPerAgent[i] = new Dictionary>(); // Need a separate copy because unlike plans, the conflict counts for agents that aren't replanned do change. - foreach (var kvp in parent.conflictTimesPerAgent[i]) - this.conflictTimesPerAgent[i][kvp.Key] = new List(kvp.Value); - } - this.agentsGroupAssignment = parent.agentsGroupAssignment.ToArray(); - this.agentNumToIndex = parent.agentNumToIndex; - this.prev = parent; - this.constraint = null; - this.depth = (ushort)(this.prev.depth + 1); - this.agentAExpansion = ExpansionState.NOT_EXPANDED; - this.agentBExpansion = ExpansionState.NOT_EXPANDED; - this.replanSize = 1; - this.solver = parent.solver; - this.singleAgentSolver = parent.singleAgentSolver; - this.cbs = parent.cbs; + SingleAgentPlans = [.. parent.SingleAgentPlans]; + NewPlans = new BitArray(SingleAgentPlans.Length); + SingleAgentCosts = [.. parent.SingleAgentCosts]; + _mdds = [.. parent._mdds]; + MDDNarrownessValues = [.. parent.MDDNarrownessValues]; // No new constraint was added so all of the parent's MDDs are valid + _countsOfInternalAgentsThatConflict = [.. parent._countsOfInternalAgentsThatConflict]; + ConflictCountsPerAgent = new Dictionary[parent.ConflictCountsPerAgent.Length]; + for (int i = 0; i < ConflictCountsPerAgent.Length; i++) + ConflictCountsPerAgent[i] = new Dictionary(parent.ConflictCountsPerAgent[i]); // Need a separate copy because unlike plans, the conflict counts for agents that aren't replanned do change. + ConflictTimesPerAgent = new Dictionary>[parent.ConflictTimesPerAgent.Length]; + for (int i = 0; i < ConflictTimesPerAgent.Length; i++) + { + ConflictTimesPerAgent[i] = []; // Need a separate copy because unlike plans, the conflict counts for agents that aren't replanned do change. + foreach (var kvp in parent.ConflictTimesPerAgent[i]) + ConflictTimesPerAgent[i][kvp.Key] = [.. kvp.Value]; + } + AgentsGroupAssignment = parent.AgentsGroupAssignment.ToArray(); + AgentNumToIndex = parent.AgentNumToIndex; + Prev = parent; + _constraint = null; + _depth = (ushort)(Prev._depth + 1); + AgentAExpansion = ExpansionState.NOT_EXPANDED; + AgentBExpansion = ExpansionState.NOT_EXPANDED; + ReplanSize = 1; + _solver = parent._solver; + _singleAgentSolver = parent._singleAgentSolver; + CBS = parent.CBS; - this.MergeGroups(mergeGroupA, mergeGroupB); + MergeGroups(mergeGroupA, mergeGroupB); - for (int i = 0; i < this.singleAgentPlans.Length; i++) + for (int i = 0; i < SingleAgentPlans.Length; i++) { - newPlans[i] = false; + NewPlans[i] = false; } - ISet mergedGroup = (mergeGroupA < mergeGroupB) ? this.GetGroup(mergeGroupA) : this.GetGroup(mergeGroupB); + ISet mergedGroup = (mergeGroupA < mergeGroupB) ? GetGroup(mergeGroupA) : GetGroup(mergeGroupB); foreach (int i in mergedGroup) - newPlans[i] = true; + NewPlans[i] = true; - this.minimumVertexCover = (int) ConflictGraph.MinVertexCover.NOT_SET; - //this.externalConstraints = parent.externalConstraints; + MinimumVertexCover = (int) ConflictGraph.MinVertexCover.NOT_SET; } /// /// Total cost + heuristic estimate /// - public int f - { - get { return this.g + this.h; } - } + public int F => G + H; - public int GetTargetH(int f) => f - g; + public int GetTargetH(int f) => f - G; /// /// Solves the entire node - finds a plan for every agent group. /// This method is only called for the root of the constraint tree. /// - /// /// Whether solving was successful. Solving fails if a timeout occurs. public bool Solve(int depthToReplan) { - this.g = 0; - ProblemInstance problem = this.cbs.GetProblemInstance(); - for (int i = 0; i < newPlans.Length; i++) + G = 0; + ProblemInstance problem = CBS.GetProblemInstance(); + for (int i = 0; i < NewPlans.Length; i++) { - newPlans[i] = true; + NewPlans[i] = true; } ConflictAvoidanceTable internalCAT = new(); ConflictAvoidanceTable CAT = internalCAT; - if (this.cbs.externalCAT != null) + if (CBS.ExternalCAT != null) { CAT = new CAT_U(); - ((CAT_U)CAT).Join(this.cbs.externalCAT); + ((CAT_U)CAT).Join(CBS.ExternalCAT); ((CAT_U)CAT).Join(internalCAT); } HashSet newConstraints = GetConstraints(); // Probably empty as this is probably the root of the CT. ISet constraints = newConstraints; - if (this.cbs.externalConstraints != null) + if (CBS.ExternalConstraints != null) { constraints = new HashSet_U(); - ((HashSet_U)constraints).Join(this.cbs.externalConstraints); + ((HashSet_U)constraints).Join(CBS.ExternalConstraints); ((HashSet_U)constraints).Join(newConstraints); } @@ -321,17 +317,17 @@ public bool Solve(int depthToReplan) ISet positiveConstraints = null; Dictionary agentsWithPositiveConstraints = null; HashSet newPositiveConstraints = null; - if (this.cbs.doMalte) - newPositiveConstraints = this.GetPositiveConstraints(); - if (this.cbs.externalPositiveConstraints != null && this.cbs.externalPositiveConstraints.Count != 0 && + if (CBS.DoMalte) + newPositiveConstraints = GetPositiveConstraints(); + if (CBS.ExternalPositiveConstraints != null && CBS.ExternalPositiveConstraints.Count != 0 && newPositiveConstraints != null && newPositiveConstraints.Count != 0) { positiveConstraints = new HashSet_U(); - ((HashSet_U)positiveConstraints).Join(this.cbs.externalPositiveConstraints); + ((HashSet_U)positiveConstraints).Join(CBS.ExternalPositiveConstraints); ((HashSet_U)positiveConstraints).Join(newPositiveConstraints); } - else if (this.cbs.externalPositiveConstraints != null && this.cbs.externalPositiveConstraints.Count != 0) - positiveConstraints = this.cbs.externalPositiveConstraints; + else if (CBS.ExternalPositiveConstraints != null && CBS.ExternalPositiveConstraints.Count != 0) + positiveConstraints = CBS.ExternalPositiveConstraints; else if (newPositiveConstraints != null && newPositiveConstraints.Count != 0) positiveConstraints = newPositiveConstraints; @@ -350,12 +346,12 @@ public bool Solve(int depthToReplan) // Find all the agents groups: List[] subGroups = new List[problem.agents.Length]; - for (int i = 0; i < agentsGroupAssignment.Length; i++) + for (int i = 0; i < AgentsGroupAssignment.Length; i++) { - if (subGroups[agentsGroupAssignment[i]] == null) - subGroups[agentsGroupAssignment[i]] = [ problem.agents[i] ]; + if (subGroups[AgentsGroupAssignment[i]] == null) + subGroups[AgentsGroupAssignment[i]] = [ problem.agents[i] ]; else - subGroups[this.agentsGroupAssignment[i]].Add(problem.agents[i]); + subGroups[AgentsGroupAssignment[i]].Add(problem.agents[i]); } bool success = true; @@ -374,36 +370,36 @@ public bool Solve(int depthToReplan) agentGroupHasMustConstraints == false && subGroup.Count == 1) // No constraints on this agent. Shortcut available (that doesn't consider the CAT, though!). { - singleAgentPlans[i] = new SinglePlan(problem.agents[i]); // All moves up to starting pos, if any - singleAgentPlans[i].AgentNum = problem.agents[this.agentsGroupAssignment[i]].agent.agentNum; // Use the group's representative + SingleAgentPlans[i] = new SinglePlan(problem.agents[i]); // All moves up to starting pos, if any + SingleAgentPlans[i].AgentNum = problem.agents[AgentsGroupAssignment[i]].agent.agentNum; // Use the group's representative SinglePlan optimalPlan = problem.GetSingleAgentOptimalPlan(problem.agents[i]); // Count conflicts: - this.conflictCountsPerAgent[i] = new Dictionary(); - this.conflictTimesPerAgent[i] = new Dictionary>(); + ConflictCountsPerAgent[i] = []; + ConflictTimesPerAgent[i] = []; foreach (var move in optimalPlan.LocationAtTimes) { var timedMove = (TimedMove)move; // GetSingleAgentOptimalPlan actually creates a plan with TimedMove instances - timedMove.IncrementConflictCounts(CAT, this.conflictCountsPerAgent[i], this.conflictTimesPerAgent[i]); + timedMove.IncrementConflictCounts(CAT, ConflictCountsPerAgent[i], ConflictTimesPerAgent[i]); } - singleAgentPlans[i].ContinueWith(optimalPlan); - singleAgentCosts[i] = problem.agents[i].g + problem.GetSingleAgentOptimalCost(problem.agents[i]); + SingleAgentPlans[i].ContinueWith(optimalPlan); + SingleAgentCosts[i] = problem.agents[i].g + problem.GetSingleAgentOptimalCost(problem.agents[i]); if (Constants.costFunction == Constants.CostFunction.SUM_OF_COSTS) { - g += (ushort)singleAgentCosts[i]; + G += (ushort)SingleAgentCosts[i]; } else if (Constants.costFunction == Constants.CostFunction.MAKESPAN || Constants.costFunction == Constants.CostFunction.MAKESPAN_THEN_SUM_OF_COSTS) { - g = Math.Max(g, (ushort)singleAgentCosts[i]); + G = Math.Max(G, (ushort)SingleAgentCosts[i]); } else throw new NotImplementedException($"Unsupported cost function {Constants.costFunction}"); - this.UpdateAtGoalConflictCounts(i, CAT); + UpdateAtGoalConflictCounts(i, CAT); } else { - success = this.Replan(i, depthToReplan, subGroup, CAT, constraints, positiveConstraints); + success = Replan(i, depthToReplan, subGroup, CAT, constraints, positiveConstraints); if (!success) // Usually means a timeout occured. break; @@ -412,7 +408,7 @@ public bool Solve(int depthToReplan) // Add the group's plan to the internal CAT. In the case we use ready-made plans from the heuristic, this is still needed to allow us to track conflicts. foreach (AgentState agentState in subGroup) { - internalCAT.AddPlan(singleAgentPlans[this.agentNumToIndex[agentState.agent.agentNum]]); + internalCAT.AddPlan(SingleAgentPlans[AgentNumToIndex[agentState.agent.agentNum]]); } } @@ -420,26 +416,26 @@ public bool Solve(int depthToReplan) return false; // Update conflict counts: All agents but the last saw an incomplete CAT. Update counts backwards. - for (int i = this.conflictCountsPerAgent.Length - 1; i >= 0; i--) + for (int i = ConflictCountsPerAgent.Length - 1; i >= 0; i--) { - foreach (KeyValuePair pair in this.conflictCountsPerAgent[i]) + foreach (KeyValuePair pair in ConflictCountsPerAgent[i]) { - if (this.agentNumToIndex.ContainsKey(pair.Key) && // An internal conflict, rather than external - this.agentNumToIndex[pair.Key] < i) // Just an optimization. Would also be correct without this check. + if (AgentNumToIndex.ContainsKey(pair.Key) && // An internal conflict, rather than external + AgentNumToIndex[pair.Key] < i) // Just an optimization. Would also be correct without this check. { - this.conflictCountsPerAgent[this.agentNumToIndex[pair.Key]] // Yes, index here, num there + ConflictCountsPerAgent[AgentNumToIndex[pair.Key]] // Yes, index here, num there [problem.agents[i].agent.agentNum] = pair.Value; // Collisions are symmetrical, and agent "key" didn't see the route for agent "i" when planning. - this.conflictTimesPerAgent[this.agentNumToIndex[pair.Key]] - [problem.agents[i].agent.agentNum] = this.conflictTimesPerAgent[i][pair.Key]; + ConflictTimesPerAgent[AgentNumToIndex[pair.Key]] + [problem.agents[i].agent.agentNum] = ConflictTimesPerAgent[i][pair.Key]; } } } - this.CountConflicts(); + CountConflicts(); - this.CalcMinOpsToSolve(); + CalcMinOpsToSolve(); - this.isGoal = this.countsOfInternalAgentsThatConflict.All(i => i == 0); + isGoal = _countsOfInternalAgentsThatConflict.All(i => i == 0); return true; } @@ -464,8 +460,8 @@ public bool Replan(int agentToReplan, int minPathTimeStep, { ConflictAvoidanceTable internalCAT = null; // To quiet the compiler - ProblemInstance problem = this.cbs.GetProblemInstance(); - int groupNum = this.agentsGroupAssignment[agentToReplan]; + ProblemInstance problem = CBS.GetProblemInstance(); + int groupNum = AgentsGroupAssignment[agentToReplan]; bool underSolve = true; if (subGroup == null) @@ -474,29 +470,29 @@ public bool Replan(int agentToReplan, int minPathTimeStep, // Construct the subgroup of agents that are of the same group as agentForReplan, // and add the plans of all other agents to CAT internalCAT = new ConflictAvoidanceTable(); - subGroup = new List(); - for (int i = 0; i < agentsGroupAssignment.Length; i++) + subGroup = []; + for (int i = 0; i < AgentsGroupAssignment.Length; i++) { - if (this.agentsGroupAssignment[i] == groupNum) + if (AgentsGroupAssignment[i] == groupNum) subGroup.Add(problem.agents[i]); else - internalCAT.AddPlan(singleAgentPlans[i]); + internalCAT.AddPlan(SingleAgentPlans[i]); } - if (this.cbs.externalCAT != null) + if (CBS.ExternalCAT != null) { CAT = new CAT_U(); - ((CAT_U)CAT).Join(this.cbs.externalCAT); + ((CAT_U)CAT).Join(CBS.ExternalCAT); ((CAT_U)CAT).Join(internalCAT); } else CAT = internalCAT; - HashSet newConstraints = this.GetConstraints(); - if (this.cbs.externalConstraints != null && this.cbs.externalConstraints.Count != 0) + HashSet newConstraints = GetConstraints(); + if (CBS.ExternalConstraints != null && CBS.ExternalConstraints.Count != 0) { constraints = new HashSet_U(); - ((HashSet_U)constraints).Join(this.cbs.externalConstraints); + ((HashSet_U)constraints).Join(CBS.ExternalConstraints); ((HashSet_U)constraints).Join(newConstraints); } else @@ -504,26 +500,26 @@ public bool Replan(int agentToReplan, int minPathTimeStep, HashSet newPositiveConstraints = null; - if (this.cbs.doMalte) - newPositiveConstraints = this.GetPositiveConstraints(); - if (this.cbs.externalPositiveConstraints != null && this.cbs.externalPositiveConstraints.Count != 0 && + if (CBS.DoMalte) + newPositiveConstraints = GetPositiveConstraints(); + if (CBS.ExternalPositiveConstraints != null && CBS.ExternalPositiveConstraints.Count != 0 && newPositiveConstraints != null && newPositiveConstraints.Count != 0) { positiveConstraints = new HashSet_U(); - ((HashSet_U)positiveConstraints).Join(this.cbs.externalPositiveConstraints); + ((HashSet_U)positiveConstraints).Join(CBS.ExternalPositiveConstraints); ((HashSet_U)positiveConstraints).Join(newPositiveConstraints); } - else if (this.cbs.externalPositiveConstraints != null && this.cbs.externalPositiveConstraints.Count != 0) - positiveConstraints = this.cbs.externalPositiveConstraints; + else if (CBS.ExternalPositiveConstraints != null && CBS.ExternalPositiveConstraints.Count != 0) + positiveConstraints = CBS.ExternalPositiveConstraints; else if (newPositiveConstraints != null && newPositiveConstraints.Count != 0) positiveConstraints = newPositiveConstraints; } - this.replanSize = (ushort)subGroup.Count; + ReplanSize = (ushort)subGroup.Count; - ICbsSolver relevantSolver = this.solver; + ICbsSolver relevantSolver = _solver; if (subGroup.Count == 1) - relevantSolver = this.singleAgentSolver; + relevantSolver = _singleAgentSolver; ProblemInstance subProblem = problem.Subproblem(subGroup.ToArray()); @@ -546,15 +542,15 @@ public bool Replan(int agentToReplan, int minPathTimeStep, } MDD mdd = null; - if (this.cbs.replanSameCostWithMdd) - mdd = this.mdds[agentToReplan]; + if (CBS.ReplanSameCostWithMdd) + mdd = _mdds[agentToReplan]; - double startTime = this.cbs.runner.ElapsedMilliseconds(); - relevantSolver.Setup(subProblem, minPathTimeStep, this.cbs.runner, CAT, constraints, positiveConstraints, + double startTime = CBS._runner.ElapsedMilliseconds(); + relevantSolver.Setup(subProblem, minPathTimeStep, CBS._runner, CAT, constraints, positiveConstraints, minPathCost, maxPathCost, mdd); bool solved = relevantSolver.Solve(); - double endTime = this.cbs.runner.ElapsedMilliseconds(); - this.cbs.timePlanningPaths += endTime - startTime; + double endTime = CBS._runner.ElapsedMilliseconds(); + CBS.TimePlanningPaths += endTime - startTime; relevantSolver.AccumulateStatistics(); relevantSolver.ClearStatistics(); @@ -564,7 +560,7 @@ public bool Replan(int agentToReplan, int minPathTimeStep, return false; } - // Copy the SinglePlans for the solved agent group from the solver to the appropriate places in this.allSingleAgentPlans + // Copy the SinglePlans for the solved agent group from the solver to the appropriate places in allSingleAgentPlans SinglePlan[] singlePlans = relevantSolver.GetSinglePlans(); int[] singleCosts = relevantSolver.GetSingleCosts(); Dictionary perAgent = null; // To quiet the compiler @@ -576,8 +572,8 @@ public bool Replan(int agentToReplan, int minPathTimeStep, } else { - perAgent = new Dictionary(); - conflictTimes = new Dictionary>(); + perAgent = []; + conflictTimes = []; foreach (var singlePlan in singlePlans) { foreach (var move in singlePlan.LocationAtTimes) @@ -593,26 +589,26 @@ public bool Replan(int agentToReplan, int minPathTimeStep, for (int i = 0; i < subGroup.Count; i++) { int agentNum = subGroup[i].agent.agentNum; - int agentIndex = this.agentNumToIndex[agentNum]; - this.singleAgentPlans[agentIndex] = singlePlans[i]; - this.singleAgentPlans[agentIndex].AgentNum = problem.agents[groupNum].agent.agentNum; // Use the group's representative - that's how the plans will be inserted into the CAT later too. - this.singleAgentCosts[agentIndex] = singleCosts[i]; + int agentIndex = AgentNumToIndex[agentNum]; + SingleAgentPlans[agentIndex] = singlePlans[i]; + SingleAgentPlans[agentIndex].AgentNum = problem.agents[groupNum].agent.agentNum; // Use the group's representative - that's how the plans will be inserted into the CAT later too. + SingleAgentCosts[agentIndex] = singleCosts[i]; if (i == 0) // This is the group representative { - this.conflictCountsPerAgent[agentIndex] = perAgent; - this.conflictTimesPerAgent[agentIndex] = conflictTimes; + ConflictCountsPerAgent[agentIndex] = perAgent; + ConflictTimesPerAgent[agentIndex] = conflictTimes; } else { if (underSolve == false) { - this.conflictCountsPerAgent[agentIndex].Clear(); // Don't over-count. Leave it to the group's representative. - this.conflictTimesPerAgent[agentIndex].Clear(); + ConflictCountsPerAgent[agentIndex].Clear(); // Don't over-count. Leave it to the group's representative. + ConflictTimesPerAgent[agentIndex].Clear(); } else { - this.conflictCountsPerAgent[agentIndex] = new Dictionary(); - this.conflictTimesPerAgent[agentIndex] = new Dictionary>(); + ConflictCountsPerAgent[agentIndex] = []; + ConflictTimesPerAgent[agentIndex] = []; } } } @@ -620,55 +616,55 @@ public bool Replan(int agentToReplan, int minPathTimeStep, // Update conflict counts with what happens after the plan finishes foreach (var agentNumAndAgentNum in subGroupAgentNums) { - int i = this.agentNumToIndex[agentNumAndAgentNum.Key]; + int i = AgentNumToIndex[agentNumAndAgentNum.Key]; if (CAT != null) - this.UpdateAtGoalConflictCounts(i, CAT); + UpdateAtGoalConflictCounts(i, CAT); // Can't use the null coalescing operator because it requires the operands be of the same type :( else - this.UpdateAtGoalConflictCounts(i, internalCAT); + UpdateAtGoalConflictCounts(i, internalCAT); } if (underSolve == false) { // Update conflictCountsPerAgent and conflictTimes for all agents int representativeAgentNum = subGroup[0].agent.agentNum; - for (int i = 0; i < this.conflictCountsPerAgent.Length; i++) + for (int i = 0; i < ConflictCountsPerAgent.Length; i++) { int agentNum = problem.agents[i].agent.agentNum; if (perAgent.ContainsKey(agentNum)) { - this.conflictCountsPerAgent[i][representativeAgentNum] = perAgent[agentNum]; - this.conflictTimesPerAgent[i][representativeAgentNum] = conflictTimes[agentNum]; + ConflictCountsPerAgent[i][representativeAgentNum] = perAgent[agentNum]; + ConflictTimesPerAgent[i][representativeAgentNum] = conflictTimes[agentNum]; } else { - this.conflictCountsPerAgent[i].Remove(representativeAgentNum); // This part could have been done before replanning - this.conflictTimesPerAgent[i].Remove(representativeAgentNum); // This part could have been done before replanning + ConflictCountsPerAgent[i].Remove(representativeAgentNum); // This part could have been done before replanning + ConflictTimesPerAgent[i].Remove(representativeAgentNum); // This part could have been done before replanning } } - this.CountConflicts(); - this.CalcMinOpsToSolve(); + CountConflicts(); + CalcMinOpsToSolve(); } // Calc g if (Constants.costFunction == Constants.CostFunction.SUM_OF_COSTS) { - this.g = (ushort)Math.Max(this.singleAgentCosts.Sum(), this.g); // Conserve g from partial + G = (ushort)Math.Max(SingleAgentCosts.Sum(), G); // Conserve g from partial // expansion if it's higher // (only happens when shuffling a partially expanded node) } else if (Constants.costFunction == Constants.CostFunction.MAKESPAN || Constants.costFunction == Constants.CostFunction.MAKESPAN_THEN_SUM_OF_COSTS) { - this.g = (ushort)Math.Max(this.singleAgentCosts.Max(), this.g); // Conserve g from partial + G = (ushort)Math.Max(SingleAgentCosts.Max(), G); // Conserve g from partial // expansion if it's higher // (only happens when shuffling a partially expanded node) } else throw new NotImplementedException($"Unsupported cost function {Constants.costFunction}"); - this.isGoal = this.countsOfInternalAgentsThatConflict.All(i => i == 0); + isGoal = _countsOfInternalAgentsThatConflict.All(i => i == 0); return true; } @@ -677,76 +673,76 @@ public void DebugPrint() { Debug.WriteLine(""); Debug.WriteLine(""); - var hashCode = this.GetHashCode(); + var hashCode = GetHashCode(); Debug.WriteLine($"Node hash: {hashCode}"); - var parent = this.prev; + var parent = Prev; Debug.Write("Ancestor hashes (parent to root): "); while (parent != null) { Debug.Write($"{parent.GetHashCode()} "); - parent = parent.prev; + parent = parent.Prev; } Debug.WriteLine(""); - Debug.WriteLine($"g: {this.g}"); - Debug.WriteLine($"h: {this.h}"); - Debug.WriteLine($"Min estimated ops needed: {this.minOpsToSolve}"); - Debug.WriteLine($"Expansion state: {this.agentAExpansion}, {this.agentBExpansion}"); - Debug.WriteLine($"Num of external agents that conflict: {totalExternalAgentsThatConflict}"); - Debug.WriteLine($"Num of internal agents that conflict: {totalInternalAgentsThatConflict}"); - Debug.WriteLine($"Num of conflicts between internal agents: {totalConflictsBetweenInternalAgents}"); - Debug.WriteLine($"Node depth: {this.depth}"); - IList constraints = this.GetConstraintsOrdered(); + Debug.WriteLine($"g: {G}"); + Debug.WriteLine($"h: {H}"); + Debug.WriteLine($"Min estimated ops needed: {MinOpsToSolve}"); + Debug.WriteLine($"Expansion state: {AgentAExpansion}, {AgentBExpansion}"); + Debug.WriteLine($"Num of external agents that conflict: {_totalExternalAgentsThatConflict}"); + Debug.WriteLine($"Num of internal agents that conflict: {TotalInternalAgentsThatConflict}"); + Debug.WriteLine($"Num of conflicts between internal agents: {TotalConflictsBetweenInternalAgents}"); + Debug.WriteLine($"Node depth: {_depth}"); + List constraints = GetConstraintsOrdered(); Debug.WriteLine($"{constraints.Count} relevant internal constraints so far (this node's, then parent's and so on): "); foreach (CbsConstraint constraint in constraints) { Debug.WriteLine(constraint); } - ISet mustConstraints = this.GetPositiveConstraints(); // TODO: Ordered + HashSet mustConstraints = GetPositiveConstraints(); // TODO: Ordered Debug.WriteLine($"{mustConstraints.Count} relevant internal must constraints so far: "); foreach (CbsConstraint mustConstraint in mustConstraints) { Debug.WriteLine(mustConstraint); } - ProblemInstance problem = this.cbs.GetProblemInstance(); - if (this.cbs.externalConstraints != null) + ProblemInstance problem = CBS.GetProblemInstance(); + if (CBS.ExternalConstraints != null) { - Debug.WriteLine($"{this.cbs.externalConstraints.Count} external constraints: "); - foreach (CbsConstraint constraint in this.cbs.externalConstraints) + Debug.WriteLine($"{CBS.ExternalConstraints.Count} external constraints: "); + foreach (CbsConstraint constraint in CBS.ExternalConstraints) { Debug.WriteLine(constraint); } } - Debug.WriteLine($"Conflict: {this.GetConflict()}"); + Debug.WriteLine($"Conflict: {GetConflict()}"); Debug.Write("Agent group assignments: "); - for (int j = 0; j < this.agentsGroupAssignment.Length; j++) + for (int j = 0; j < AgentsGroupAssignment.Length; j++) { - Debug.Write($" {this.agentsGroupAssignment[j],3}"); + Debug.Write($" {AgentsGroupAssignment[j],3}"); } Debug.WriteLine(""); Debug.Write("Single agent costs: "); // Extra spaces to align with the group assignments line - for (int j = 0; j < this.singleAgentCosts.Length; j++) + for (int j = 0; j < SingleAgentCosts.Length; j++) { - Debug.Write($" {this.singleAgentCosts[j],3}"); + Debug.Write($" {SingleAgentCosts[j],3}"); } Debug.WriteLine(""); Debug.Write("Internal agents that conflict with each agent: "); - for (int j = 0; j < this.countsOfInternalAgentsThatConflict.Length; j++) + for (int j = 0; j < _countsOfInternalAgentsThatConflict.Length; j++) { - Debug.Write($" {this.countsOfInternalAgentsThatConflict[j]}"); + Debug.Write($" {_countsOfInternalAgentsThatConflict[j]}"); } Debug.WriteLine(""); Debug.Write("New plans: "); - for (int j = 0; j < this.newPlans.Length; j++) + for (int j = 0; j < NewPlans.Length; j++) { - Debug.Write($" {(this.newPlans[j] ? 1 : 0)}"); + Debug.Write($" {(NewPlans[j] ? 1 : 0)}"); } Debug.WriteLine(""); - for (int j = 0; j < this.conflictCountsPerAgent.Length; j++) + for (int j = 0; j < ConflictCountsPerAgent.Length; j++) { - if (this.conflictCountsPerAgent[j].Count != 0) + if (ConflictCountsPerAgent[j].Count != 0) { Debug.Write($"Agent {problem.agents[j].agent.agentNum} conflict counts: "); - foreach (var pair in this.conflictCountsPerAgent[j]) + foreach (var pair in ConflictCountsPerAgent[j]) { Debug.Write($"{pair.Key}:{pair.Value} "); } @@ -754,12 +750,12 @@ public void DebugPrint() } } - for (int j = 0; j < this.conflictTimesPerAgent.Length; j++) + for (int j = 0; j < ConflictTimesPerAgent.Length; j++) { - if (this.conflictCountsPerAgent[j].Count != 0) + if (ConflictCountsPerAgent[j].Count != 0) { Debug.Write($"Agent {problem.agents[j].agent.agentNum} conflict times: "); - foreach (var pair in this.conflictTimesPerAgent[j]) + foreach (var pair in ConflictTimesPerAgent[j]) { Debug.Write($"{pair.Key}:[{String.Join(",", pair.Value)}], "); } @@ -767,19 +763,19 @@ public void DebugPrint() } } - if (this.cbs.GetType() == typeof(MACBS_WholeTreeThreshold) && this.cbs.mergeThreshold != -1) + if (CBS.GetType() == typeof(MACBS_WholeTreeThreshold) && CBS.MergeThreshold != -1) { - for (int i = 0; i < ((MACBS_WholeTreeThreshold)this.cbs).globalConflictsCounter.Length; i++) + for (int i = 0; i < ((MACBS_WholeTreeThreshold)CBS).globalConflictsCounter.Length; i++) { Debug.Write($"Agent {i} global historic conflict counts: "); for (int j = 0; j < i; j++) { - Debug.Write($"a{j}:{((MACBS_WholeTreeThreshold)this.cbs).globalConflictsCounter[i][j]} "); + Debug.Write($"a{j}:{((MACBS_WholeTreeThreshold)CBS).globalConflictsCounter[i][j]} "); } Debug.WriteLine(""); } } - var plan = this.CalculateJointPlan(); + var plan = CalculateJointPlan(); plan.PrintPlanIfShort(); Debug.WriteLine(""); Debug.WriteLine(""); @@ -794,16 +790,16 @@ public void DebugPrint() /// protected void UpdateAtGoalConflictCounts(int agentIndex, ConflictAvoidanceTable CAT) { - ProblemInstance problem = this.cbs.GetProblemInstance(); + ProblemInstance problem = CBS.GetProblemInstance(); var afterGoal = new TimedMove( problem.agents[agentIndex].agent.Goal.X, problem.agents[agentIndex].agent.Goal.Y, Direction.Wait, time: 0); - for (int time = singleAgentPlans[agentIndex].GetSize(); time < CAT.GetMaxPlanSize(); time++) + for (int time = SingleAgentPlans[agentIndex].GetSize(); time < CAT.GetMaxPlanSize(); time++) { afterGoal.Time = time; afterGoal.IncrementConflictCounts(CAT, - this.conflictCountsPerAgent[this.agentsGroupAssignment[agentIndex]], - this.conflictTimesPerAgent[this.agentsGroupAssignment[agentIndex]]); + ConflictCountsPerAgent[AgentsGroupAssignment[agentIndex]], + ConflictTimesPerAgent[AgentsGroupAssignment[agentIndex]]); } } @@ -831,20 +827,20 @@ protected void UpdateAtGoalConflictCounts(int agentIndex, ConflictAvoidanceTable /// protected void CalcMinOpsToSolve() { - if (this.cbs.disableTieBreakingByMinOpsEstimate == false) + if (!CBS.DisableTieBreakingByMinOpsEstimate) { - var vertexCover = new HashSet(); + HashSet vertexCover = []; - for (int i = 0; i < this.conflictCountsPerAgent.Length; i++) + for (int i = 0; i < ConflictCountsPerAgent.Length; i++) { if (vertexCover.Contains(i)) // This node is already in the cover - all its edges are already covered. continue; - foreach (KeyValuePair otherEndAgentNumAndCount in this.conflictCountsPerAgent[i]) + foreach (KeyValuePair otherEndAgentNumAndCount in ConflictCountsPerAgent[i]) { - if (this.agentNumToIndex.ContainsKey(otherEndAgentNumAndCount.Key)) // It's an internal conflict + if (AgentNumToIndex.ContainsKey(otherEndAgentNumAndCount.Key)) // It's an internal conflict { - int otherEndIndex = this.agentNumToIndex[otherEndAgentNumAndCount.Key]; + int otherEndIndex = AgentNumToIndex[otherEndAgentNumAndCount.Key]; if (vertexCover.Contains(otherEndAgentNumAndCount.Key) == false) // The vertex isn't covered from its other end yet { vertexCover.Add(i); @@ -858,29 +854,29 @@ protected void CalcMinOpsToSolve() int minReplansToSolve = vertexCover.Count / 2; // We have a 2-approximation of the size of the cover - // half that is at least half the value we're trying to approximate. // (The size of the approximation is always even) - //if (this.cbs.debug) + //if (cbs.debug) // Debug.WriteLine("min replans lower estimate: " + minReplansToSolve); - if (this.cbs.mergeThreshold != -1) // Merges possible, account for them + if (CBS.MergeThreshold != -1) // Merges possible, account for them // This assumes the current merging strategy is used. { - if (this.cbs.GetType() == typeof(CBS)) + if (CBS.GetType() == typeof(CBS)) { - if (this.cbs.mergeThreshold > 0) + if (CBS.MergeThreshold > 0) { int maxPotentialMergeSavings = (int)Math.Floor(((double)minReplansToSolve) / 2); - int depthToGoTo = this.depth + minReplansToSolve; - int chainSize = this.cbs.mergeThreshold + 1; // Every series of B+1 downwards consecutive nodes may end with a merge. + int depthToGoTo = _depth + minReplansToSolve; + int chainSize = CBS.MergeThreshold + 1; // Every series of B+1 downwards consecutive nodes may end with a merge. int maxMerges = depthToGoTo / chainSize; // Round down to discount the last unfinished chain. // Count the minimum amount of merges already done and subtract it from maxMerges: - var groupSizes = new Dictionary(); - for (int i = 0; i < this.agentsGroupAssignment.Length; i++) + Dictionary groupSizes = []; + for (int i = 0; i < AgentsGroupAssignment.Length; i++) { - if (groupSizes.ContainsKey(this.agentsGroupAssignment[i]) == false) - groupSizes[this.agentsGroupAssignment[i]] = 0; - groupSizes[this.agentsGroupAssignment[i]]++; + if (groupSizes.ContainsKey(AgentsGroupAssignment[i]) == false) + groupSizes[AgentsGroupAssignment[i]] = 0; + groupSizes[AgentsGroupAssignment[i]]++; } - // Not using this.GetGroupSizes() because what we want is actually + // Not using GetGroupSizes() because what we want is actually // a list of the sizes of the different groups, not the size of each agent's group foreach (int groupSize in groupSizes.Values) @@ -888,18 +884,18 @@ protected void CalcMinOpsToSolve() int maxMergeSavings = Math.Min(maxPotentialMergeSavings, maxMerges); - this.minOpsToSolve = minReplansToSolve - maxMergeSavings; + MinOpsToSolve = minReplansToSolve - maxMergeSavings; } else - this.minOpsToSolve = (int)Math.Ceiling(((double)minReplansToSolve) / 2); + MinOpsToSolve = (int)Math.Ceiling(((double)minReplansToSolve) / 2); } else - this.minOpsToSolve = (int)Math.Ceiling(((double)minReplansToSolve) / 2); // TODO: We could look at the global table and maybe deduce something, but I'm not interested in that right now. + MinOpsToSolve = (int)Math.Ceiling(((double)minReplansToSolve) / 2); // TODO: We could look at the global table and maybe deduce something, but I'm not interested in that right now. } else - this.minOpsToSolve = (int)minReplansToSolve; + MinOpsToSolve = (int)minReplansToSolve; } } @@ -911,53 +907,50 @@ protected void CalcMinOpsToSolve() /// protected void CountConflicts() { - var externalConflictingAgentNums = new HashSet(); - this.totalInternalAgentsThatConflict = 0; - this.totalConflictsBetweenInternalAgents = 0; - this.totalConflictsWithExternalAgents = 0; + HashSet externalConflictingAgentNums = []; + TotalInternalAgentsThatConflict = 0; + TotalConflictsBetweenInternalAgents = 0; + TotalConflictsWithExternalAgents = 0; - for (int i = 0; i < this.conflictCountsPerAgent.Length; i++) + for (int i = 0; i < ConflictCountsPerAgent.Length; i++) { - this.countsOfInternalAgentsThatConflict[i] = 0; + _countsOfInternalAgentsThatConflict[i] = 0; - if (conflictCountsPerAgent[i].Count != 0) - totalInternalAgentsThatConflict++; + if (ConflictCountsPerAgent[i].Count != 0) + TotalInternalAgentsThatConflict++; - foreach (KeyValuePair conflictingAgentNumAndCount in conflictCountsPerAgent[i]) + foreach (KeyValuePair conflictingAgentNumAndCount in ConflictCountsPerAgent[i]) { - if (this.agentNumToIndex.ContainsKey(conflictingAgentNumAndCount.Key)) // It's an internal conflict + if (AgentNumToIndex.ContainsKey(conflictingAgentNumAndCount.Key)) // It's an internal conflict { - this.countsOfInternalAgentsThatConflict[i]++; // Counts one conflict for each agent the i'th agent conflicts with - this.totalConflictsBetweenInternalAgents += conflictingAgentNumAndCount.Value; + _countsOfInternalAgentsThatConflict[i]++; // Counts one conflict for each agent the i'th agent conflicts with + TotalConflictsBetweenInternalAgents += conflictingAgentNumAndCount.Value; } else { externalConflictingAgentNums.Add(conflictingAgentNumAndCount.Key); - this.totalConflictsWithExternalAgents += conflictingAgentNumAndCount.Value; - this.conflictTimesPerAgent[i].Remove(conflictingAgentNumAndCount.Key); // Not needed + TotalConflictsWithExternalAgents += conflictingAgentNumAndCount.Value; + ConflictTimesPerAgent[i].Remove(conflictingAgentNumAndCount.Key); // Not needed } } } - this.totalExternalAgentsThatConflict = externalConflictingAgentNums.Count; + _totalExternalAgentsThatConflict = externalConflictingAgentNums.Count; - this.totalConflictsBetweenInternalAgents /= 2; // Each conflict was counted twice - this.totalConflictsWithExternalAgents /= 2; // Each conflict was counted twice + TotalConflictsBetweenInternalAgents /= 2; // Each conflict was counted twice + TotalConflictsWithExternalAgents /= 2; // Each conflict was counted twice } /// /// Used to preserve state of conflict iteration. /// - private IEnumerator nextConflicts; + private IEnumerator _nextConflicts; /// /// The iterator holds the state of the generator, with all the different queues etc - a lot of memory. /// We also clear the MDD narrowness values that were computed - if no child uses them, they'll be garbage-collected. /// - public void ClearConflictChoiceData() - { - this.nextConflicts = null; - } + public void ClearConflictChoiceData() => _nextConflicts = null; /// /// Use after expanding a node and finding the conflict wasn't cardinal @@ -965,9 +958,9 @@ public void ClearConflictChoiceData() /// Whether we found a new potentially cardinal conflict to work on public bool ChooseNextPotentiallyCardinalConflicts() { - if (this.nextConflictCouldBeCardinal) + if (nextConflictCouldBeCardinal) { - bool cycled = this.ChooseNextConflict(); + bool cycled = ChooseNextConflict(); if (cycled) return true; else @@ -982,9 +975,9 @@ public bool ChooseNextPotentiallyCardinalConflicts() /// Whether another conflict was found public bool ChooseNextConflict() { - bool hadNext = this.nextConflicts.MoveNext(); + bool hadNext = _nextConflicts.MoveNext(); if (hadNext) - this.conflict = this.nextConflicts.Current; + Conflict = _nextConflicts.Current; return hadNext; } @@ -994,63 +987,63 @@ public bool ChooseNextConflict() /// public void ChooseConflict() { - if (this.singleAgentPlans.Length == 1) // A single internal agent can't conflict with anything internally + if (SingleAgentPlans.Length == 1) // A single internal agent can't conflict with anything internally return; - if (this.isGoal) // Goal nodes don't have conflicts + if (isGoal) // Goal nodes don't have conflicts return; - if (this.conflict != null) // Conflict already chosen before + if (Conflict != null) // Conflict already chosen before return; - if (this.cbs.conflictChoice == CBS.ConflictChoice.FIRST) + if (CBS.ConflictChoice == ConflictChoice.FIRST) { - this.ChooseFirstConflict(); + ChooseFirstConflict(); } - else if (this.cbs.conflictChoice == CBS.ConflictChoice.MOST_CONFLICTING_SMALLEST_AGENTS) + else if (CBS.ConflictChoice == ConflictChoice.MOST_CONFLICTING_SMALLEST_AGENTS) { - this.ChooseConflictOfMostConflictingSmallestAgents(); + ChooseConflictOfMostConflictingSmallestAgents(); } - else if (this.cbs.conflictChoice == CBS.ConflictChoice.CARDINAL_MDD) + else if (CBS.ConflictChoice == ConflictChoice.CARDINAL_MDD) { // Choose the first (in order of looking at them), earliest (in time), cardinal // (if not found settle for semi-cardinal, then non-cardinal) conflict. - // Assumes this.mergeThreshold == -1. - this.nextConflicts = this.GetConflictsCardinalFirstUsingMdd().GetEnumerator(); - bool hasConflict = this.nextConflicts.MoveNext(); // This node isn't a goal node so this is expected to return true - + // Assumes mergeThreshold == -1. + _nextConflicts = GetConflictsCardinalFirstUsingMdd().GetEnumerator(); + bool hasConflict = _nextConflicts.MoveNext(); // This node isn't a goal node so this is expected to return true - // a conflict should be found if (hasConflict == false) { - this.DebugPrint(); + DebugPrint(); Trace.Assert(false, "Non-goal node found no conflict"); } - this.conflict = this.nextConflicts.Current; + Conflict = _nextConflicts.Current; } - else if (this.cbs.conflictChoice == CBS.ConflictChoice.CARDINAL_LOOKAHEAD) + else if (CBS.ConflictChoice == ConflictChoice.CARDINAL_LOOKAHEAD) { - this.nextConflicts = this.GetConflictsNoOrder().GetEnumerator(); - bool hasConflict = this.nextConflicts.MoveNext(); // This node isn't a goal node so this is expected to return true - + _nextConflicts = GetConflictsNoOrder().GetEnumerator(); + bool hasConflict = _nextConflicts.MoveNext(); // This node isn't a goal node so this is expected to return true - // a conflict should be found if (hasConflict == false) { - this.DebugPrint(); + DebugPrint(); Trace.Assert(false, "Non-goal node found no conflict"); } - this.conflict = this.nextConflicts.Current; + Conflict = _nextConflicts.Current; //FIXME: code dup with previous option } - else if (this.cbs.conflictChoice == CBS.ConflictChoice.CARDINAL_MDD_THEN_MERGE_EARLY_MOST_CONFLICTING_SMALLEST_GROUP || - this.cbs.conflictChoice == CBS.ConflictChoice.CARDINAL_MDD_THEN_MERGE_EARLY_MOST_CONFLICTING_AND_SMALLEST_GROUP) + else if (CBS.ConflictChoice == ConflictChoice.CARDINAL_MDD_THEN_MERGE_EARLY_MOST_CONFLICTING_SMALLEST_GROUP || + CBS.ConflictChoice == ConflictChoice.CARDINAL_MDD_THEN_MERGE_EARLY_MOST_CONFLICTING_AND_SMALLEST_GROUP) { - this.nextConflicts = this.GetConflictsCardinalFirstUsingMddMergeFirstByNewPolicy().GetEnumerator(); - bool hasConflict = this.nextConflicts.MoveNext(); // This node isn't a goal node so this is expected to return true - + _nextConflicts = GetConflictsCardinalFirstUsingMddMergeFirstByNewPolicy().GetEnumerator(); + bool hasConflict = _nextConflicts.MoveNext(); // This node isn't a goal node so this is expected to return true - // a conflict should be found if (hasConflict == false) { - this.DebugPrint(); + DebugPrint(); Trace.Assert(false, "Non-goal node found no conflict"); } - this.conflict = this.nextConflicts.Current; + Conflict = _nextConflicts.Current; } else throw new Exception("Unknown conflict-choosing method"); @@ -1059,13 +1052,13 @@ public void ChooseConflict() private void ChooseConflictOfMostConflictingSmallestAgents() { (int groupRepA, int groupRepB, int time) = GetDetailsOfConflictOfMostConflictingSmallestAgents(); - this.conflict = FindConflict(groupRepA, groupRepB, time); + Conflict = FindConflict(groupRepA, groupRepB, time); } private void ChooseFirstConflict() { (int groupRepA, int groupRepB, int time) = GetFirstConflictDetails(); - this.conflict = FindConflict(groupRepA, groupRepB, time); + Conflict = FindConflict(groupRepA, groupRepB, time); } /// @@ -1074,18 +1067,18 @@ private void ChooseFirstConflict() /// private IEnumerable GetConflictsNoOrder() { - ISet[] groups = this.GetGroups(); - this.nextConflictCouldBeCardinal = true; // We don't know + ISet[] groups = GetGroups(); + nextConflictCouldBeCardinal = true; // We don't know - for (int agentIndex = 0; agentIndex < this.conflictTimesPerAgent.Length; agentIndex++) + for (int agentIndex = 0; agentIndex < ConflictTimesPerAgent.Length; agentIndex++) { - foreach (int conflictingAgentNum in this.conflictTimesPerAgent[agentIndex].Keys) + foreach (int conflictingAgentNum in ConflictTimesPerAgent[agentIndex].Keys) { - int conflictingAgentIndex = this.agentNumToIndex[conflictingAgentNum]; + int conflictingAgentIndex = AgentNumToIndex[conflictingAgentNum]; if (conflictingAgentIndex < agentIndex) continue; // Return each conflict only once - foreach (int conflictTime in this.conflictTimesPerAgent[agentIndex][conflictingAgentNum]) + foreach (int conflictTime in ConflictTimesPerAgent[agentIndex][conflictingAgentNum]) { yield return FindConflict(agentIndex, conflictingAgentIndex, conflictTime, groups); } @@ -1094,15 +1087,15 @@ private IEnumerable GetConflictsNoOrder() } /// - /// Assumes this.mergeThreshold == -1. + /// Assumes mergeThreshold == -1. /// Builds MDDs for all agents. /// Not currently used. /// /// private IEnumerable GetConflictsExhaustivelySearchingForCardinalsGreedily() { - this.buildAllMDDs(); - return this.GetConflictsCardinalFirstUsingMdd(); + buildAllMDDs(); + return GetConflictsCardinalFirstUsingMdd(); } /// @@ -1110,16 +1103,16 @@ private IEnumerable GetConflictsExhaustivelySearchingForCardinalsGr /// public void buildAllMDDs() { - foreach (var agentIndex in Enumerable.Range(0, this.singleAgentPlans.Length)) + foreach (var agentIndex in Enumerable.Range(0, SingleAgentPlans.Length)) { - if (this.conflictTimesPerAgent[agentIndex].Count == 0) + if (ConflictTimesPerAgent[agentIndex].Count == 0) continue; // Agent has no conflicts - this.buildMddForAgentWithItsCurrentCost(agentIndex); // Does nothing if it's built already + buildMddForAgentWithItsCurrentCost(agentIndex); // Does nothing if it's built already } } /// - /// Assumes this.mergeThreshold == -1. + /// Assumes mergeThreshold == -1. /// Builds MDDs as necessary until a cardinal conflict is found. /// Also sets h to 1 if a cardinal conflict is found. /// Not currently used. @@ -1127,45 +1120,45 @@ public void buildAllMDDs() /// private IEnumerable GetConflictsExhaustivelySearchingForCardinalsLazily() { - ISet[] groups = this.GetGroups(); + ISet[] groups = GetGroups(); - foreach (var agentIndex in Enumerable.Range(0, this.singleAgentPlans.Length)) + foreach (var agentIndex in Enumerable.Range(0, SingleAgentPlans.Length)) { - if (this.conflictTimesPerAgent[agentIndex].Count == 0) + if (ConflictTimesPerAgent[agentIndex].Count == 0) continue; // Agent has no conflicts - bool hasMdd = this.mddNarrownessValues[agentIndex] != null || - this.CopyAppropriateMddFromParent(agentIndex); + bool hasMdd = MDDNarrownessValues[agentIndex] != null || + CopyAppropriateMddFromParent(agentIndex); - foreach (int conflictingAgentNum in this.conflictTimesPerAgent[agentIndex].Keys) + foreach (int conflictingAgentNum in ConflictTimesPerAgent[agentIndex].Keys) { - int conflictingAgentIndex = this.agentNumToIndex[conflictingAgentNum]; - bool otherHasMdd = this.mddNarrownessValues[conflictingAgentIndex] != null || - this.CopyAppropriateMddFromParent(conflictingAgentIndex); + int conflictingAgentIndex = AgentNumToIndex[conflictingAgentNum]; + bool otherHasMdd = MDDNarrownessValues[conflictingAgentIndex] != null || + CopyAppropriateMddFromParent(conflictingAgentIndex); - foreach (int conflictTime in this.conflictTimesPerAgent[agentIndex][conflictingAgentNum]) + foreach (int conflictTime in ConflictTimesPerAgent[agentIndex][conflictingAgentNum]) { - if (otherHasMdd == false || this.DoesAgentHaveNoOtherOption(conflictingAgentIndex, conflictTime, agentIndex, groups)) // Other agent's MDD is narrow at this timestep. + if (otherHasMdd == false || DoesAgentHaveNoOtherOption(conflictingAgentIndex, conflictTime, agentIndex, groups)) // Other agent's MDD is narrow at this timestep. { - this.buildMddForAgentWithItsCurrentCost(agentIndex); + buildMddForAgentWithItsCurrentCost(agentIndex); hasMdd = true; } else continue; - bool iNarrow = this.DoesAgentHaveNoOtherOption(agentIndex, conflictTime, conflictingAgentIndex, groups); + bool iNarrow = DoesAgentHaveNoOtherOption(agentIndex, conflictTime, conflictingAgentIndex, groups); if (iNarrow == false) continue; if (otherHasMdd == false) { - this.buildMddForAgentWithItsCurrentCost(conflictingAgentIndex); + buildMddForAgentWithItsCurrentCost(conflictingAgentIndex); otherHasMdd = true; } - bool otherNarrow = this.DoesAgentHaveNoOtherOption(conflictingAgentIndex, conflictTime, agentIndex, groups); + bool otherNarrow = DoesAgentHaveNoOtherOption(conflictingAgentIndex, conflictTime, agentIndex, groups); if (otherNarrow) // Both narrow! { CbsConflict cardinal = FindConflict(agentIndex, conflictingAgentIndex, conflictTime); cardinal.willCostIncreaseForAgentA = CbsConflict.WillCostIncrease.YES; cardinal.willCostIncreaseForAgentB = CbsConflict.WillCostIncrease.YES; - this.nextConflictCouldBeCardinal = false; // Don't cycle conflicts even if the cost doesn't increase (can happen if this is resolved via a merge operation because it also removes some constraints) + nextConflictCouldBeCardinal = false; // Don't cycle conflicts even if the cost doesn't increase (can happen if this is resolved via a merge operation because it also removes some constraints) yield return cardinal; } } @@ -1173,7 +1166,7 @@ private IEnumerable GetConflictsExhaustivelySearchingForCardinalsLa } // No cardinal conflict was found - var FullSearchIterator = this.GetConflictsCardinalFirstUsingMdd(); + var FullSearchIterator = GetConflictsCardinalFirstUsingMdd(); foreach (var conflict in FullSearchIterator) yield return conflict; } @@ -1194,7 +1187,7 @@ private IEnumerable GetConflictsExhaustivelySearchingForCardinalsLa /// private IEnumerable GetConflictsCardinalFirstUsingMdd() { - if (this.totalConflictsBetweenInternalAgents == 1) + if (TotalConflictsBetweenInternalAgents == 1) { Debug.WriteLine("Single conflict. Just choosing it."); return GetConflictsNoOrder(); @@ -1204,7 +1197,7 @@ private IEnumerable GetConflictsCardinalFirstUsingMdd() private IEnumerable GetConflictsCardinalFirstUsingMddMergeFirstByNewPolicy() { - if (this.totalConflictsBetweenInternalAgents == 1) + if (TotalConflictsBetweenInternalAgents == 1) { Debug.WriteLine("Single conflict. Just choosing it."); return GetConflictsNoOrder(); @@ -1224,16 +1217,16 @@ private IEnumerable GetConflictsCardinalFirstUsingMddMergeFirstByNe /// private IEnumerable GetConflictsCardinalFirstUsingMddInternal() { - ISet[] groups = this.GetGroups(); + ISet[] groups = GetGroups(); // Queue items are - var NotCardinalMaybeSemi = new Queue<(int agentAIndex, int agentBIndex, int conflictTime)>(this.totalConflictsBetweenInternalAgents); // Because first has an MDD - var NotCardinalNotSemi = new Queue<(int agentAIndex, int agentBIndex, int conflictTime)>(this.totalConflictsBetweenInternalAgents); - var SemiCardinal = new Queue<(int agentAIndex, int agentBIndex, int conflictTime)>(this.totalConflictsBetweenInternalAgents); - var PossiblyCardinalFirstHasMddSecondDoesNotButCan = new Queue<(int agentAIndex, int agentBIndex, int conflictTime)>(this.totalConflictsBetweenInternalAgents); - var PossiblyCardinalFirstHasMddSecondCannot = new Queue<(int agentAIndex, int agentBIndex, int conflictTime)>(this.totalConflictsBetweenInternalAgents); - var PossiblyCardinalBothCannotBuildMdd = new Queue<(int agentAIndex, int agentBIndex, int conflictTime)>(this.totalConflictsBetweenInternalAgents); - var PossiblyCardinalFirstCanBuildMdd = new Queue<(int agentAIndex, int agentBIndex, int conflictTime)>(this.totalConflictsBetweenInternalAgents); // Going over these just get the first element, build its MDD and - var AgentIndexesWaitingToCheckTheirConflictsForCardinality = new Queue(Enumerable.Range(0, this.singleAgentPlans.Length)); // Initially go over all conflicting agents. + Queue<(int agentAIndex, int agentBIndex, int conflictTime)> NotCardinalMaybeSemi = new(TotalConflictsBetweenInternalAgents); // Because first has an MDD + Queue<(int agentAIndex, int agentBIndex, int conflictTime)> NotCardinalNotSemi = new(TotalConflictsBetweenInternalAgents); + Queue<(int agentAIndex, int agentBIndex, int conflictTime)> SemiCardinal = new(TotalConflictsBetweenInternalAgents); + Queue<(int agentAIndex, int agentBIndex, int conflictTime)> PossiblyCardinalFirstHasMddSecondDoesNotButCan = new(TotalConflictsBetweenInternalAgents); + Queue<(int agentAIndex, int agentBIndex, int conflictTime)> PossiblyCardinalFirstHasMddSecondCannot = new(TotalConflictsBetweenInternalAgents); + Queue<(int agentAIndex, int agentBIndex, int conflictTime)> PossiblyCardinalBothCannotBuildMdd = new(TotalConflictsBetweenInternalAgents); + Queue<(int agentAIndex, int agentBIndex, int conflictTime)> PossiblyCardinalFirstCanBuildMdd = new(TotalConflictsBetweenInternalAgents); // Going over these just get the first element, build its MDD and + Queue AgentIndexesWaitingToCheckTheirConflictsForCardinality = new(Enumerable.Range(0, SingleAgentPlans.Length)); // Initially go over all conflicting agents. // TODO: this will also go over non-conflicting agents harmlessly. Is there an easy way to get a list of agents that have conflicts? // Positively cardinal conflicts are just yielded immediately // Conflicting agents are only entered into a queue once. Only if the conflicting agent with the larger index @@ -1251,14 +1244,14 @@ private IEnumerable GetConflictsCardinalFirstUsingMddInternal() while (AgentIndexesWaitingToCheckTheirConflictsForCardinality.Count != 0) // Can't use foreach, we actually want to drain the queue { var i = AgentIndexesWaitingToCheckTheirConflictsForCardinality.Dequeue(); - bool hasMDD = this.mddNarrownessValues[i] != null || // No need to check if its levels is null, we don't sync MDDs and we know there's a path with the current cost for the agent - this.CopyAppropriateMddFromParent(i); + bool hasMDD = MDDNarrownessValues[i] != null || // No need to check if its levels is null, we don't sync MDDs and we know there's a path with the current cost for the agent + CopyAppropriateMddFromParent(i); bool canBuildMDD = groups[i].Count == 1; - foreach (int conflictingAgentNum in this.conflictTimesPerAgent[i].Keys) + foreach (int conflictingAgentNum in ConflictTimesPerAgent[i].Keys) { - int conflictingAgentIndex = this.agentNumToIndex[conflictingAgentNum]; - bool otherCanBuildMdd = groups[conflictingAgentIndex].Count == 1 && this.mddNarrownessValues[conflictingAgentIndex] == null; + int conflictingAgentIndex = AgentNumToIndex[conflictingAgentNum]; + bool otherCanBuildMdd = groups[conflictingAgentIndex].Count == 1 && MDDNarrownessValues[conflictingAgentIndex] == null; if (allowAgentOrderFlip) { if (i < conflictingAgentIndex && // We'll see this pair again in the other order @@ -1270,16 +1263,16 @@ private IEnumerable GetConflictsCardinalFirstUsingMddInternal() ((canBuildMDD && otherCanBuildMdd == false) == false)) // We didn't skip to this order earlier continue; // Already taken care of } - bool otherHasMDD = this.mddNarrownessValues[conflictingAgentIndex] != null || - this.CopyAppropriateMddFromParent(conflictingAgentIndex); // FIXME: If no ancestor has an appropriate MDD, this might be checked multiple times :( + bool otherHasMDD = MDDNarrownessValues[conflictingAgentIndex] != null || + CopyAppropriateMddFromParent(conflictingAgentIndex); // FIXME: If no ancestor has an appropriate MDD, this might be checked multiple times :( // Reaching here means either i < conflictingAgentIndex, // or the i'th agent can build an MDD and the conflictingAgentIndex'th can't. - foreach (int conflictTime in this.conflictTimesPerAgent[i][conflictingAgentNum]) + foreach (int conflictTime in ConflictTimesPerAgent[i][conflictingAgentNum]) { if (hasMDD) // Check if not cardinal { - bool iNarrow = this.DoesAgentHaveNoOtherOption(i, conflictTime, conflictingAgentIndex, groups); + bool iNarrow = DoesAgentHaveNoOtherOption(i, conflictTime, conflictingAgentIndex, groups); if (iNarrow == false) // Then it isn't cardinal. May still be semi cardinal. { if (otherHasMDD == false) // Skip building the second MDD even if it's possible @@ -1289,7 +1282,7 @@ private IEnumerable GetConflictsCardinalFirstUsingMddInternal() } else // Other has MDD { - bool otherNarrow = this.DoesAgentHaveNoOtherOption(conflictingAgentIndex, conflictTime, i, groups); + bool otherNarrow = DoesAgentHaveNoOtherOption(conflictingAgentIndex, conflictTime, i, groups); if (otherNarrow == false) { NotCardinalNotSemi.Enqueue((i, conflictingAgentIndex, conflictTime)); @@ -1319,7 +1312,7 @@ private IEnumerable GetConflictsCardinalFirstUsingMddInternal() } else // Other has MDD { - bool otherNarrow = this.DoesAgentHaveNoOtherOption(conflictingAgentIndex, conflictTime, i, groups); + bool otherNarrow = DoesAgentHaveNoOtherOption(conflictingAgentIndex, conflictTime, i, groups); if (otherNarrow == false) // iNarrow but other not narrow { SemiCardinal.Enqueue((i, conflictingAgentIndex, conflictTime)); @@ -1331,8 +1324,8 @@ private IEnumerable GetConflictsCardinalFirstUsingMddInternal() CbsConflict cardinal = FindConflict(i, conflictingAgentIndex, conflictTime, groups); cardinal.willCostIncreaseForAgentA = CbsConflict.WillCostIncrease.YES; cardinal.willCostIncreaseForAgentB = CbsConflict.WillCostIncrease.YES; - this.h = Math.Max(this.h, 1); // The children's cost will be at least 1 more than this node's cost - this.nextConflictCouldBeCardinal = false; // We don't want CBS to cycle conflicts after this one. + H = Math.Max(H, 1); // The children's cost will be at least 1 more than this node's cost + nextConflictCouldBeCardinal = false; // We don't want CBS to cycle conflicts after this one. // This could happen if the conflict is resolved via a merge // and the conflicting agents already have some constraints // to avoid each other that have already increased the cost @@ -1369,7 +1362,7 @@ private IEnumerable GetConflictsCardinalFirstUsingMddInternal() // a. Get one conflict from PossiblyCardinalFirstHasMddSecondDoesNotButCan and build the second agent's // MDD. int agentToBuildAnMddFor = PossiblyCardinalFirstHasMddSecondDoesNotButCan.Dequeue().agentBIndex; - this.buildMddForAgentWithItsCurrentCost(agentToBuildAnMddFor); + buildMddForAgentWithItsCurrentCost(agentToBuildAnMddFor); // b. Remove other conflicts from PossiblyCardinalFirstHasMddSecondDoesNotButCan where the second // agent is the one we built an MDD for (in all of those, the first agent's index is lower than the second's, // since we could build an MDD for it). @@ -1397,7 +1390,7 @@ private IEnumerable GetConflictsCardinalFirstUsingMddInternal() // a. Get one conflict from PossiblyCardinalFirstCanBuildMdd and build the first agent's // MDD. int agentToBuildAnMddFor = PossiblyCardinalFirstCanBuildMdd.Dequeue().agentAIndex; - this.buildMddForAgentWithItsCurrentCost(agentToBuildAnMddFor); + buildMddForAgentWithItsCurrentCost(agentToBuildAnMddFor); // b. Remove other conflicts from PossiblyCardinalFirstHasMddSecondDoesNotButCan where the second // agent is the one we built an MDD for (in all of those, the first agent's index is lower than the second's, // since we could build an MDD for it). @@ -1427,7 +1420,7 @@ private IEnumerable GetConflictsCardinalFirstUsingMddInternal() { Debug.WriteLine("Checking for cardinality via a lookahead..."); var tuple = PossiblyCardinalFirstHasMddSecondCannot.Dequeue(); - this.nextConflictCouldBeCardinal = (PossiblyCardinalFirstHasMddSecondCannot.Count != 0) || + nextConflictCouldBeCardinal = (PossiblyCardinalFirstHasMddSecondCannot.Count != 0) || (PossiblyCardinalBothCannotBuildMdd.Count != 0); var possiblyCardinal = FindConflict(tuple.agentAIndex, tuple.agentBIndex, tuple.conflictTime, groups); possiblyCardinal.willCostIncreaseForAgentA = CbsConflict.WillCostIncrease.YES; @@ -1440,14 +1433,14 @@ private IEnumerable GetConflictsCardinalFirstUsingMddInternal() { Debug.WriteLine("Checking for cardinality via a lookahead..."); var tuple = PossiblyCardinalBothCannotBuildMdd.Dequeue(); - this.nextConflictCouldBeCardinal = PossiblyCardinalBothCannotBuildMdd.Count != 0; + nextConflictCouldBeCardinal = PossiblyCardinalBothCannotBuildMdd.Count != 0; var possiblyCardinal = FindConflict(tuple.agentAIndex, tuple.agentBIndex, tuple.conflictTime, groups); possiblyCardinal.willCostIncreaseForAgentA = CbsConflict.WillCostIncrease.MAYBE; possiblyCardinal.willCostIncreaseForAgentB = CbsConflict.WillCostIncrease.MAYBE; yield return possiblyCardinal; } - this.nextConflictCouldBeCardinal = false; + nextConflictCouldBeCardinal = false; // Yield semi cardinal conflicts while (SemiCardinal.Count != 0) @@ -1506,26 +1499,26 @@ private IEnumerable GetConflictsCardinalFirstUsingMddInternal() /// private IEnumerable GetConflictsCardinalFirstUsindMddMergeFirstByNewPolicyInternal() { - ISet[] groups = this.GetGroups(); + ISet[] groups = GetGroups(); // Queue items are - var CardinalShouldMerge = new List<(int agentAIndex, int agentBIndex, int conflictTime)>(this.totalConflictsBetweenInternalAgents); - var Cardinal = new Queue<(int agentAIndex, int agentBIndex, int conflictTime)>(this.totalConflictsBetweenInternalAgents); - var NotCardinalMaybeSemi = new Queue<(int agentAIndex, int agentBIndex, int conflictTime)>(this.totalConflictsBetweenInternalAgents); // Because first has an MDD - var NonCardinalShouldMerge = new Queue<(int agentAIndex, int agentBIndex, int conflictTime)>(this.totalConflictsBetweenInternalAgents); - var NotCardinalNotSemi = new Queue<(int agentAIndex, int agentBIndex, int conflictTime)>(this.totalConflictsBetweenInternalAgents); - var SemiCardinal = new Queue<(int agentAIndex, int agentBIndex, int conflictTime)>(this.totalConflictsBetweenInternalAgents); - var PossiblyCardinalFirstHasMddSecondCannotShouldMerge = new Queue<(int agentAIndex, int agentBIndex, int conflictTime)>(this.totalConflictsBetweenInternalAgents); - var PossiblyCardinalBothCannotBuildMddShouldMerge = new Queue<(int agentAIndex, int agentBIndex, int conflictTime)>(this.totalConflictsBetweenInternalAgents); - var PossiblyCardinalFirstHasMddSecondDoesNotButCan = new Queue<(int agentAIndex, int agentBIndex, int conflictTime)>(this.totalConflictsBetweenInternalAgents); - var PossiblyCardinalFirstHasMddSecondCannot = new Queue<(int agentAIndex, int agentBIndex, int conflictTime)>(this.totalConflictsBetweenInternalAgents); - var PossiblyCardinalBothCannotBuildMdd = new Queue<(int agentAIndex, int agentBIndex, int conflictTime)>(this.totalConflictsBetweenInternalAgents); - var PossiblyCardinalFirstCanBuildMdd = new Queue<(int agentAIndex, int agentBIndex, int conflictTime)>(this.totalConflictsBetweenInternalAgents); // Going over these just get the first element, build its MDD and - var AgentIndexesWaitingToCheckTheirConflictsForCardinality = new Queue(Enumerable.Range(0, this.singleAgentPlans.Length)); // Initially go over all conflicting agents. - // TODO: this will also go over non-conflicting agents harmlessly. Is there an easy way to get a list of agents that have conflicts? - // Positively cardinal conflicts are just yielded immediately - // Conflicting agents are only entered into a queue once. Only if the conflicting agent with the larger index - // can have an MDD built and the one with the lower can't, a pair of conflicting agents is entered in reverse. - var shouldMergePair = new Dictionary<(int agentAIndex, int agentBIndex), bool>(); + List<(int agentAIndex, int agentBIndex, int conflictTime)> CardinalShouldMerge = new(TotalConflictsBetweenInternalAgents); + Queue<(int agentAIndex, int agentBIndex, int conflictTime)> Cardinal = new(TotalConflictsBetweenInternalAgents); + Queue<(int agentAIndex, int agentBIndex, int conflictTime)> NotCardinalMaybeSemi = new(TotalConflictsBetweenInternalAgents); // Because first has an MDD + Queue<(int agentAIndex, int agentBIndex, int conflictTime)> NonCardinalShouldMerge = new(TotalConflictsBetweenInternalAgents); + Queue<(int agentAIndex, int agentBIndex, int conflictTime)> NotCardinalNotSemi = new(TotalConflictsBetweenInternalAgents); + Queue<(int agentAIndex, int agentBIndex, int conflictTime)> SemiCardinal = new(TotalConflictsBetweenInternalAgents); + Queue<(int agentAIndex, int agentBIndex, int conflictTime)> PossiblyCardinalFirstHasMddSecondCannotShouldMerge = new(TotalConflictsBetweenInternalAgents); + Queue<(int agentAIndex, int agentBIndex, int conflictTime)> PossiblyCardinalBothCannotBuildMddShouldMerge = new(TotalConflictsBetweenInternalAgents); + Queue<(int agentAIndex, int agentBIndex, int conflictTime)> PossiblyCardinalFirstHasMddSecondDoesNotButCan = new(TotalConflictsBetweenInternalAgents); + Queue<(int agentAIndex, int agentBIndex, int conflictTime)> PossiblyCardinalFirstHasMddSecondCannot = new(TotalConflictsBetweenInternalAgents); + Queue<(int agentAIndex, int agentBIndex, int conflictTime)> PossiblyCardinalBothCannotBuildMdd = new(TotalConflictsBetweenInternalAgents); + Queue<(int agentAIndex, int agentBIndex, int conflictTime)> PossiblyCardinalFirstCanBuildMdd = new(TotalConflictsBetweenInternalAgents); // Going over these just get the first element, build its MDD and + Queue AgentIndexesWaitingToCheckTheirConflictsForCardinality = new(Enumerable.Range(0, SingleAgentPlans.Length)); // Initially go over all conflicting agents. + // TODO: this will also go over non-conflicting agents harmlessly. Is there an easy way to get a list of agents that have conflicts? + // Positively cardinal conflicts are just yielded immediately + // Conflicting agents are only entered into a queue once. Only if the conflicting agent with the larger index + // can have an MDD built and the one with the lower can't, a pair of conflicting agents is entered in reverse. + Dictionary<(int agentAIndex, int agentBIndex), bool> shouldMergePair = []; bool allowAgentOrderFlip = true; // Needed when rechecking agents to signal that we shouldn't // rely on the other end to check a conflict @@ -1538,14 +1531,14 @@ private IEnumerable GetConflictsCardinalFirstUsindMddMergeFirstByNe while (AgentIndexesWaitingToCheckTheirConflictsForCardinality.Count != 0) // Can't use foreach, we actually want to drain the queue { var i = AgentIndexesWaitingToCheckTheirConflictsForCardinality.Dequeue(); - bool hasMDD = this.mddNarrownessValues[i] != null || // No need to check if its levels is null, we don't sync MDDs and we know there's a path with the current cost for the agent - this.CopyAppropriateMddFromParent(i); + bool hasMDD = MDDNarrownessValues[i] != null || // No need to check if its levels is null, we don't sync MDDs and we know there's a path with the current cost for the agent + CopyAppropriateMddFromParent(i); bool canBuildMDD = groups[i].Count == 1; - foreach (int conflictingAgentNum in this.conflictTimesPerAgent[i].Keys) + foreach (int conflictingAgentNum in ConflictTimesPerAgent[i].Keys) { - int conflictingAgentIndex = this.agentNumToIndex[conflictingAgentNum]; - bool otherCanBuildMdd = groups[conflictingAgentIndex].Count == 1 && this.mddNarrownessValues[conflictingAgentIndex] == null; + int conflictingAgentIndex = AgentNumToIndex[conflictingAgentNum]; + bool otherCanBuildMdd = groups[conflictingAgentIndex].Count == 1 && MDDNarrownessValues[conflictingAgentIndex] == null; if (allowAgentOrderFlip) { if (i < conflictingAgentIndex && // We'll see this pair again in the other order @@ -1557,12 +1550,12 @@ private IEnumerable GetConflictsCardinalFirstUsindMddMergeFirstByNe ((canBuildMDD && otherCanBuildMdd == false) == false)) // We didn't skip to this order earlier continue; // Already taken care of } - bool otherHasMDD = this.mddNarrownessValues[conflictingAgentIndex] != null || - this.CopyAppropriateMddFromParent(conflictingAgentIndex); // FIXME: If no ancestor has an appropriate MDD, this might be checked multiple times :( + bool otherHasMDD = MDDNarrownessValues[conflictingAgentIndex] != null || + CopyAppropriateMddFromParent(conflictingAgentIndex); // FIXME: If no ancestor has an appropriate MDD, this might be checked multiple times :( bool shouldMergeThisPair; if (shouldMergePair.ContainsKey((i, conflictingAgentIndex)) == false) { - shouldMergeThisPair = this.ShouldMerge(this.cbs.mergeThreshold, i, conflictingAgentIndex); + shouldMergeThisPair = ShouldMerge(CBS.MergeThreshold, i, conflictingAgentIndex); shouldMergePair[(i, conflictingAgentIndex)] = shouldMergeThisPair; shouldMergePair[(conflictingAgentIndex, i)] = shouldMergeThisPair; } @@ -1572,12 +1565,12 @@ private IEnumerable GetConflictsCardinalFirstUsindMddMergeFirstByNe { if (!hasMDD && canBuildMDD) { - this.buildMddForAgentWithItsCurrentCost(i); + buildMddForAgentWithItsCurrentCost(i); hasMDD = true; } if (!otherHasMDD && otherCanBuildMdd) { - this.buildMddForAgentWithItsCurrentCost(conflictingAgentIndex); + buildMddForAgentWithItsCurrentCost(conflictingAgentIndex); otherHasMDD = true; } } @@ -1587,11 +1580,11 @@ private IEnumerable GetConflictsCardinalFirstUsindMddMergeFirstByNe // Reaching here means either i < conflictingAgentIndex, // or the i'th agent can build an MDD and the conflictingAgentIndex'th can't. - foreach (int conflictTime in this.conflictTimesPerAgent[i][conflictingAgentNum]) + foreach (int conflictTime in ConflictTimesPerAgent[i][conflictingAgentNum]) { if (hasMDD) // Check if not cardinal { - bool iNarrow = this.DoesAgentHaveNoOtherOption(i, conflictTime, conflictingAgentIndex, groups); + bool iNarrow = DoesAgentHaveNoOtherOption(i, conflictTime, conflictingAgentIndex, groups); if (iNarrow == false) // Then it isn't cardinal. May still be semi cardinal. { if (otherHasMDD == false) // Skip building the second MDD even if it's possible @@ -1605,7 +1598,7 @@ private IEnumerable GetConflictsCardinalFirstUsindMddMergeFirstByNe } else // Other has MDD { - bool otherNarrow = this.DoesAgentHaveNoOtherOption(conflictingAgentIndex, conflictTime, i, groups); + bool otherNarrow = DoesAgentHaveNoOtherOption(conflictingAgentIndex, conflictTime, i, groups); if (otherNarrow == false) { if (shouldMergeThisPair) @@ -1645,7 +1638,7 @@ private IEnumerable GetConflictsCardinalFirstUsindMddMergeFirstByNe } else // Other has MDD { - bool otherNarrow = this.DoesAgentHaveNoOtherOption(conflictingAgentIndex, conflictTime, i, groups); + bool otherNarrow = DoesAgentHaveNoOtherOption(conflictingAgentIndex, conflictTime, i, groups); if (otherNarrow == false) // iNarrow but other not narrow { if (shouldMergeThisPair) @@ -1703,8 +1696,8 @@ private IEnumerable GetConflictsCardinalFirstUsindMddMergeFirstByNe // on the agents will be discarded, so the cost of the merged agent might actually go down! // But this is the way to tell if this conflict is avoidable or not. // FIXME: Lots of code duplication here - int groupRepA = agentsGroupAssignment[tuple.agentAIndex]; - int groupRepB = agentsGroupAssignment[tuple.agentBIndex]; + int groupRepA = AgentsGroupAssignment[tuple.agentAIndex]; + int groupRepB = AgentsGroupAssignment[tuple.agentBIndex]; int aCost = GetGroupCost(groupRepA); int bCost = GetGroupCost(groupRepB); int minAndMaxNewCost; @@ -1716,7 +1709,7 @@ private IEnumerable GetConflictsCardinalFirstUsindMddMergeFirstByNe else throw new Exception("Unexpected cost function"); var tempNode = new CbsNode(this, groupRepA, groupRepB); - bool success = tempNode.Replan(groupRepA, this.cbs.minSolutionTimeStep, + bool success = tempNode.Replan(groupRepA, CBS.MinSolutionTimeStep, minPathCost: minAndMaxNewCost, maxPathCost: minAndMaxNewCost); if (success) // then not cardinal { @@ -1744,8 +1737,8 @@ private IEnumerable GetConflictsCardinalFirstUsindMddMergeFirstByNe // on the agents will be discarded, so the cost of the merged agent might actually go down! // But this is the way to tell if this conflict is avoidable or not. // FIXME: Lots of code duplication here - int groupRepA = agentsGroupAssignment[tuple.agentAIndex]; - int groupRepB = agentsGroupAssignment[tuple.agentBIndex]; + int groupRepA = AgentsGroupAssignment[tuple.agentAIndex]; + int groupRepB = AgentsGroupAssignment[tuple.agentBIndex]; int aCost = GetGroupCost(groupRepA); int bCost = GetGroupCost(groupRepB); int minAndMaxNewCost; @@ -1757,7 +1750,7 @@ private IEnumerable GetConflictsCardinalFirstUsindMddMergeFirstByNe else throw new Exception("Unexpected cost function"); var tempNode = new CbsNode(this, groupRepA, groupRepB); - bool success = tempNode.Replan(groupRepA, this.cbs.minSolutionTimeStep, + bool success = tempNode.Replan(groupRepA, CBS.MinSolutionTimeStep, minPathCost: minAndMaxNewCost, maxPathCost: minAndMaxNewCost); if (success) // then not cardinal { @@ -1778,16 +1771,16 @@ private IEnumerable GetConflictsCardinalFirstUsindMddMergeFirstByNe while (CardinalShouldMerge.Count != 0) { int i; - if (this.cbs.conflictChoice == CBS.ConflictChoice.CARDINAL_MDD_THEN_MERGE_EARLY_MOST_CONFLICTING_SMALLEST_GROUP) + if (CBS.ConflictChoice == ConflictChoice.CARDINAL_MDD_THEN_MERGE_EARLY_MOST_CONFLICTING_SMALLEST_GROUP) { - i = CardinalShouldMerge.ToArray().IndexOfMax(tuple => -1 * (this.GetGroupSize(tuple.agentAIndex) + this.GetGroupSize(tuple.agentBIndex))); + i = CardinalShouldMerge.ToArray().IndexOfMax(tuple => -1 * (GetGroupSize(tuple.agentAIndex) + GetGroupSize(tuple.agentBIndex))); //var array = CardinalShouldMerge.ToArray(); - //var indices_of_smallest = array.IndicesOfMax(tuple => -1 * (this.GetGroupSize(tuple.agentAIndex) + this.GetGroupSize(tuple.agentBIndex))); // Max of -1*value instead of adding min variants + //var indices_of_smallest = array.IndicesOfMax(tuple => -1 * (GetGroupSize(tuple.agentAIndex) + GetGroupSize(tuple.agentBIndex))); // Max of -1*value instead of adding min variants //var i = indices_of_smallest.MaxByKeyFunc(index => conflicts(array[i].agentAIndex) + conflicts(array[i].agentAIndex)) } - else if (this.cbs.conflictChoice == CBS.ConflictChoice.CARDINAL_MDD_THEN_MERGE_EARLY_MOST_CONFLICTING_AND_SMALLEST_GROUP) + else if (CBS.ConflictChoice == ConflictChoice.CARDINAL_MDD_THEN_MERGE_EARLY_MOST_CONFLICTING_AND_SMALLEST_GROUP) { - //i = CardinalShouldMerge.ToArray().IndexOfMax(tuple => conflicts / (1 << (this.GetGroupSize(tuple.agentAIndex) + this.GetGroupSize(tuple.agentBIndex) - 1))); + //i = CardinalShouldMerge.ToArray().IndexOfMax(tuple => conflicts / (1 << (GetGroupSize(tuple.agentAIndex) + GetGroupSize(tuple.agentBIndex) - 1))); //... throw new Exception("Unexpected"); } @@ -1796,7 +1789,7 @@ private IEnumerable GetConflictsCardinalFirstUsindMddMergeFirstByNe var tuple = CardinalShouldMerge[i]; CardinalShouldMerge.RemoveAt(i); Debug.WriteLine("Chose a cardinal conflict between agents that should be merged"); - this.nextConflictCouldBeCardinal = (Cardinal.Count != 0) || + nextConflictCouldBeCardinal = (Cardinal.Count != 0) || (PossiblyCardinalFirstHasMddSecondCannot.Count != 0) || (PossiblyCardinalBothCannotBuildMdd.Count != 0); var cardinal = FindConflict(tuple.agentAIndex, tuple.agentBIndex, tuple.conflictTime, groups); @@ -1810,7 +1803,7 @@ private IEnumerable GetConflictsCardinalFirstUsindMddMergeFirstByNe { Debug.WriteLine("Chose a cardinal conflict"); var tuple = Cardinal.Dequeue(); - this.nextConflictCouldBeCardinal = (Cardinal.Count != 0) || + nextConflictCouldBeCardinal = (Cardinal.Count != 0) || (PossiblyCardinalFirstHasMddSecondCannot.Count != 0) || (PossiblyCardinalBothCannotBuildMdd.Count != 0); var cardinal = FindConflict(tuple.agentAIndex, tuple.agentBIndex, tuple.conflictTime, groups); @@ -1826,7 +1819,7 @@ private IEnumerable GetConflictsCardinalFirstUsindMddMergeFirstByNe // a. Get one conflict from PossiblyCardinalFirstHasMddSecondDoesNotButCan and build the second agent's // MDD. int agentToBuildAnMddFor = PossiblyCardinalFirstHasMddSecondDoesNotButCan.Dequeue().agentBIndex; - this.buildMddForAgentWithItsCurrentCost(agentToBuildAnMddFor); + buildMddForAgentWithItsCurrentCost(agentToBuildAnMddFor); // b. Remove other conflicts from PossiblyCardinalFirstHasMddSecondDoesNotButCan where the second // agent is the one we built an MDD for (in all of those, the first agent's index is lower than the second's, // since we could build an MDD for it). @@ -1854,7 +1847,7 @@ private IEnumerable GetConflictsCardinalFirstUsindMddMergeFirstByNe // a. Get one conflict from PossiblyCardinalFirstCanBuildMdd and build the first agent's // MDD. int agentToBuildAnMddFor = PossiblyCardinalFirstCanBuildMdd.Dequeue().agentAIndex; - this.buildMddForAgentWithItsCurrentCost(agentToBuildAnMddFor); + buildMddForAgentWithItsCurrentCost(agentToBuildAnMddFor); // b. Remove other conflicts from PossiblyCardinalFirstHasMddSecondDoesNotButCan where the second // agent is the one we built an MDD for (in all of those, the first agent's index is lower than the second's, // since we could build an MDD for it). @@ -1884,7 +1877,7 @@ private IEnumerable GetConflictsCardinalFirstUsindMddMergeFirstByNe { Debug.WriteLine("Checking for cardinality via a lookahead..."); var tuple = PossiblyCardinalFirstHasMddSecondCannot.Dequeue(); - this.nextConflictCouldBeCardinal = (PossiblyCardinalFirstHasMddSecondCannot.Count != 0) || + nextConflictCouldBeCardinal = (PossiblyCardinalFirstHasMddSecondCannot.Count != 0) || (PossiblyCardinalBothCannotBuildMdd.Count != 0); var possiblyCardinal = FindConflict(tuple.agentAIndex, tuple.agentBIndex, tuple.conflictTime, groups); possiblyCardinal.willCostIncreaseForAgentA = CbsConflict.WillCostIncrease.YES; @@ -1897,14 +1890,14 @@ private IEnumerable GetConflictsCardinalFirstUsindMddMergeFirstByNe { Debug.WriteLine("Checking for cardinality via a lookahead..."); var tuple = PossiblyCardinalBothCannotBuildMdd.Dequeue(); - this.nextConflictCouldBeCardinal = PossiblyCardinalBothCannotBuildMdd.Count != 0; + nextConflictCouldBeCardinal = PossiblyCardinalBothCannotBuildMdd.Count != 0; var possiblyCardinal = FindConflict(tuple.agentAIndex, tuple.agentBIndex, tuple.conflictTime, groups); possiblyCardinal.willCostIncreaseForAgentA = CbsConflict.WillCostIncrease.MAYBE; possiblyCardinal.willCostIncreaseForAgentB = CbsConflict.WillCostIncrease.MAYBE; yield return possiblyCardinal; } - this.nextConflictCouldBeCardinal = false; + nextConflictCouldBeCardinal = false; // Yield semi cardinal conflicts while (SemiCardinal.Count != 0) @@ -1970,14 +1963,14 @@ private CbsConflict FindConflict(int aConflictingGroupMemberIndex, int bConflictingGroupMemberIndex, int time, ISet[] groups = null) { int specificConflictingAgentA, specificConflictingAgentB; - this.FindConflicting(aConflictingGroupMemberIndex, bConflictingGroupMemberIndex, time, + FindConflicting(aConflictingGroupMemberIndex, bConflictingGroupMemberIndex, time, out specificConflictingAgentA, out specificConflictingAgentB, groups); - ProblemInstance problem = this.cbs.GetProblemInstance(); + ProblemInstance problem = CBS.GetProblemInstance(); int initialTimeStep = problem.agents[0].lastMove.Time; // To account for solving partially solved problems. // This assumes the makespan of all the agents is the same. - Move first = singleAgentPlans[specificConflictingAgentA].GetLocationAt(time); - Move second = singleAgentPlans[specificConflictingAgentB].GetLocationAt(time); + Move first = SingleAgentPlans[specificConflictingAgentA].GetLocationAt(time); + Move second = SingleAgentPlans[specificConflictingAgentB].GetLocationAt(time); return new CbsConflict(specificConflictingAgentA, specificConflictingAgentB, first, second, time + initialTimeStep); } @@ -1996,7 +1989,7 @@ private void FindConflicting(int aConflictingGroupMemberIndex, int bConflictingG int time, out int a, out int b, ISet[] groups = null) { - if (this.cbs.mergeThreshold == -1) // An optimization for CBS. We assume they collide. + if (CBS.MergeThreshold == -1) // An optimization for CBS. We assume they collide. { a = aConflictingGroupMemberIndex; b = bConflictingGroupMemberIndex; @@ -2008,8 +2001,8 @@ private void FindConflicting(int aConflictingGroupMemberIndex, int bConflictingG if (groups == null) { - groupA = this.GetGroup(aConflictingGroupMemberIndex); - groupB = this.GetGroup(bConflictingGroupMemberIndex); + groupA = GetGroup(aConflictingGroupMemberIndex); + groupB = GetGroup(bConflictingGroupMemberIndex); } else { @@ -2017,7 +2010,7 @@ private void FindConflicting(int aConflictingGroupMemberIndex, int bConflictingG groupB = groups[bConflictingGroupMemberIndex]; } - this.largerConflictingGroupSize = Math.Max(groupA.Count, groupB.Count); // TODO: explain why + _largerConflictingGroupSize = Math.Max(groupA.Count, groupB.Count); // TODO: explain why if (groupA.Count == 1 && groupB.Count == 1) // We assume they collide. { @@ -2030,7 +2023,7 @@ private void FindConflicting(int aConflictingGroupMemberIndex, int bConflictingG { foreach (var varB in groupB) { - if (singleAgentPlans[varA].IsColliding(time, singleAgentPlans[varB])) + if (SingleAgentPlans[varA].IsColliding(time, SingleAgentPlans[varB])) { a = varA; b = varB; @@ -2040,7 +2033,7 @@ private void FindConflicting(int aConflictingGroupMemberIndex, int bConflictingG } // A conflict should have been found - this.DebugPrint(); + DebugPrint(); throw new Exception("Conflict not found"); } @@ -2052,15 +2045,15 @@ private void FindConflicting(int aConflictingGroupMemberIndex, int bConflictingG /// or if an MDD of the same cost was adapted private bool CopyAppropriateMddFromParent(int agentIndex) { - int targetCost = this.singleAgentCosts[agentIndex]; + int targetCost = SingleAgentCosts[agentIndex]; CbsNode node = this; CbsNode ancestorWithMddOfSameCost = null; - Stack stack = new Stack(); + Stack stack = new(); while (node != null) { - if (node.mdds[agentIndex] != null) + if (node._mdds[agentIndex] != null) { - if (node.mdds[agentIndex].cost == targetCost) + if (node._mdds[agentIndex].cost == targetCost) { ancestorWithMddOfSameCost = node; break; @@ -2072,35 +2065,35 @@ private bool CopyAppropriateMddFromParent(int agentIndex) } } stack.Push(node); - node = node.prev; + node = node.Prev; } if (ancestorWithMddOfSameCost != null) { // Copy the MDD down the CT branch, deleting nodes as necessary when constraints // make them invalid. - MDD mdd = ancestorWithMddOfSameCost.mdds[agentIndex]; - Dictionary mddValues = ancestorWithMddOfSameCost.mddNarrownessValues[agentIndex]; + MDD mdd = ancestorWithMddOfSameCost._mdds[agentIndex]; + Dictionary mddValues = ancestorWithMddOfSameCost.MDDNarrownessValues[agentIndex]; while (stack.Count > 0) { CbsNode nodeToGiveAnMdd = stack.Pop(); - if (nodeToGiveAnMdd.constraint != null && - this.agentNumToIndex[nodeToGiveAnMdd.constraint.agentNum] == agentIndex) + if (nodeToGiveAnMdd._constraint != null && + AgentNumToIndex[nodeToGiveAnMdd._constraint.agentNum] == agentIndex) { - double startTime = this.cbs.runner.ElapsedMilliseconds(); - mdd = new MDD(mdd, nodeToGiveAnMdd.constraint); + double startTime = CBS._runner.ElapsedMilliseconds(); + mdd = new MDD(mdd, nodeToGiveAnMdd._constraint); mddValues = mdd.getLevelNarrownessValues(); - double endTime = this.cbs.runner.ElapsedMilliseconds(); - this.cbs.timeBuildingMdds += endTime - startTime; - this.cbs.mddsAdapted++; - if (this.cbs.cacheMdds) + double endTime = CBS._runner.ElapsedMilliseconds(); + CBS.TimeBuildingMdds += endTime - startTime; + CBS.MDDsAdapted++; + if (CBS.CacheMdds) { CbsCacheEntry entry = new CbsCacheEntry(nodeToGiveAnMdd, agentIndex); - this.cbs.mddCache[agentIndex][entry] = mdd; - this.cbs.mddNarrownessValuesCache[agentIndex][entry] = mddValues; + CBS.MDDCache[agentIndex][entry] = mdd; + CBS.MDDNarrownessValuesCache[agentIndex][entry] = mddValues; } } - nodeToGiveAnMdd.mdds[agentIndex] = mdd; - nodeToGiveAnMdd.mddNarrownessValues[agentIndex] = mddValues; + nodeToGiveAnMdd._mdds[agentIndex] = mdd; + nodeToGiveAnMdd.MDDNarrownessValues[agentIndex] = mddValues; } return true; } @@ -2116,10 +2109,10 @@ private bool CopyAppropriateMddFromParent(int agentIndex) /// Whether an MDD was built public bool buildMddForAgentWithItsCurrentCost(int agentIndex) { - if (this.mddNarrownessValues[agentIndex] != null) // Already have an MDD with the current cost (they're nulled when the cost increases) + if (MDDNarrownessValues[agentIndex] != null) // Already have an MDD with the current cost (they're nulled when the cost increases) return false; - if (this.cbs.cacheMdds == false || this.cbs.mddCache[agentIndex].ContainsKey(new CbsCacheEntry(this, agentIndex)) == false) + if (CBS.CacheMdds == false || CBS.MDDCache[agentIndex].ContainsKey(new CbsCacheEntry(this, agentIndex)) == false) { // Caching not enabled or no cache hit if (CopyAppropriateMddFromParent(agentIndex)) @@ -2127,13 +2120,13 @@ public bool buildMddForAgentWithItsCurrentCost(int agentIndex) // Build the MDD // TODO: Code dup with Replan, Solve - ProblemInstance problem = this.cbs.GetProblemInstance(); - HashSet newConstraints = this.GetConstraints(); + ProblemInstance problem = CBS.GetProblemInstance(); + HashSet newConstraints = GetConstraints(); ISet constraints = null; - if (this.cbs.externalConstraints != null) + if (CBS.ExternalConstraints != null) { constraints = new HashSet_U(); - ((HashSet_U)constraints).Join(this.cbs.externalConstraints); + ((HashSet_U)constraints).Join(CBS.ExternalConstraints); ((HashSet_U)constraints).Join(newConstraints); } else @@ -2141,21 +2134,21 @@ public bool buildMddForAgentWithItsCurrentCost(int agentIndex) ISet positiveConstraints = null; HashSet newPositiveConstraints = null; - if (this.cbs.doMalte) - newPositiveConstraints = this.GetPositiveConstraints(); - if (this.cbs.externalPositiveConstraints != null && this.cbs.externalPositiveConstraints.Count != 0 && + if (CBS.DoMalte) + newPositiveConstraints = GetPositiveConstraints(); + if (CBS.ExternalPositiveConstraints != null && CBS.ExternalPositiveConstraints.Count != 0 && newPositiveConstraints != null && newPositiveConstraints.Count != 0) { positiveConstraints = new HashSet_U(); - ((HashSet_U)positiveConstraints).Join(this.cbs.externalPositiveConstraints); + ((HashSet_U)positiveConstraints).Join(CBS.ExternalPositiveConstraints); ((HashSet_U)positiveConstraints).Join(newPositiveConstraints); } - else if (this.cbs.externalPositiveConstraints != null && this.cbs.externalPositiveConstraints.Count != 0) - positiveConstraints = this.cbs.externalPositiveConstraints; + else if (CBS.ExternalPositiveConstraints != null && CBS.ExternalPositiveConstraints.Count != 0) + positiveConstraints = CBS.ExternalPositiveConstraints; else if (newPositiveConstraints != null && newPositiveConstraints.Count != 0) positiveConstraints = newPositiveConstraints; - int depth = this.singleAgentCosts.Max(); + int depth = SingleAgentCosts.Max(); IEnumerable myConstraints = constraints.Where( constraint => constraint.agentNum == problem.agents[agentIndex].agent.agentNum); @@ -2175,44 +2168,44 @@ public bool buildMddForAgentWithItsCurrentCost(int agentIndex) } } - Debug.WriteLine($"Building MDD for agent index {agentIndex} of cost {this.singleAgentCosts[agentIndex]} and depth {depth}"); + Debug.WriteLine($"Building MDD for agent index {agentIndex} of cost {SingleAgentCosts[agentIndex]} and depth {depth}"); - double startTime = this.cbs.runner.ElapsedMilliseconds(); - this.mdds[agentIndex] = new MDD(agentIndex, problem.agents[agentIndex].agent.agentNum, - problem.agents[agentIndex].GetMove(), this.singleAgentCosts[agentIndex], + double startTime = CBS._runner.ElapsedMilliseconds(); + _mdds[agentIndex] = new MDD(agentIndex, problem.agents[agentIndex].agent.agentNum, + problem.agents[agentIndex].GetMove(), SingleAgentCosts[agentIndex], depth, problem.GetNumOfAgents(), problem, ignoreConstraints: false, supportPruning: false, constraints: constraints, positiveConstraints: positiveConstraints); - this.mddNarrownessValues[agentIndex] = this.mdds[agentIndex].getLevelNarrownessValues(); - double endTime = this.cbs.runner.ElapsedMilliseconds(); - this.cbs.timeBuildingMdds += endTime - startTime; - if (this.cbs.cacheMdds) + MDDNarrownessValues[agentIndex] = _mdds[agentIndex].getLevelNarrownessValues(); + double endTime = CBS._runner.ElapsedMilliseconds(); + CBS.TimeBuildingMdds += endTime - startTime; + if (CBS.CacheMdds) { CbsCacheEntry entry = new CbsCacheEntry(this, agentIndex); - this.cbs.mddCache[agentIndex][entry] = this.mdds[agentIndex]; - this.cbs.mddNarrownessValuesCache[agentIndex][entry] = this.mddNarrownessValues[agentIndex]; + CBS.MDDCache[agentIndex][entry] = _mdds[agentIndex]; + CBS.MDDNarrownessValuesCache[agentIndex][entry] = MDDNarrownessValues[agentIndex]; } - this.cbs.mddsBuilt++; + CBS.MDDsBuilt++; } else { // The MDD is in the cache! CbsCacheEntry entry = new CbsCacheEntry(this, agentIndex); - this.mdds[agentIndex] = this.cbs.mddCache[agentIndex][entry]; - this.mddNarrownessValues[agentIndex] = this.cbs.mddNarrownessValuesCache[agentIndex][entry]; - this.cbs.mddCacheHits++; + _mdds[agentIndex] = CBS.MDDCache[agentIndex][entry]; + MDDNarrownessValues[agentIndex] = CBS.MDDNarrownessValuesCache[agentIndex][entry]; + CBS.MDDCacheHits++; } // Copy the MDD up to ancestors where appropriate CbsNode node = this; while (node != null) { - node.mddNarrownessValues[agentIndex] = this.mddNarrownessValues[agentIndex]; - if (node.constraint != null && - this.agentNumToIndex[node.constraint.agentNum] == agentIndex) + node.MDDNarrownessValues[agentIndex] = MDDNarrownessValues[agentIndex]; + if (node._constraint != null && + AgentNumToIndex[node._constraint.agentNum] == agentIndex) break; // This is where the last contraint on the agent was added. // Ancestors will have inappropriate MDDs for this agent - no need to check them. - node = node.prev; + node = node.Prev; } return true; @@ -2223,15 +2216,15 @@ public bool buildMddForAgentWithItsCurrentCost(int agentIndex) int groupRepA = -1; // To quiet the compiler int groupRepB = -1; // To quiet the compiler int time = int.MaxValue; - for (int i = 0; i < this.conflictTimesPerAgent.Length; i++) + for (int i = 0; i < ConflictTimesPerAgent.Length; i++) { - foreach (var otherAgentNumAndConflictTimes in this.conflictTimesPerAgent[i]) + foreach (var otherAgentNumAndConflictTimes in ConflictTimesPerAgent[i]) { if (otherAgentNumAndConflictTimes.Value[0] < time) { time = otherAgentNumAndConflictTimes.Value[0]; groupRepA = i; - groupRepB = this.agentNumToIndex[otherAgentNumAndConflictTimes.Key]; + groupRepB = AgentNumToIndex[otherAgentNumAndConflictTimes.Key]; } } } @@ -2256,29 +2249,29 @@ public bool buildMddForAgentWithItsCurrentCost(int agentIndex) int groupRepA = -1; // To quiet the compiler int groupRepB = -1; // To quiet the compiler int time = int.MaxValue; - Func formula = i => this.countsOfInternalAgentsThatConflict[i] / ((double)(1 << (this.GetGroupSize(i) - 1))); + Func formula = i => _countsOfInternalAgentsThatConflict[i] / ((double)(1 << (GetGroupSize(i) - 1))); - int chosenAgentIndex = Enumerable.Range(0, this.singleAgentPlans.Length).MaxByKeyFunc(formula); + int chosenAgentIndex = Enumerable.Range(0, SingleAgentPlans.Length).MaxByKeyFunc(formula); // We could just look for any of this agent's conflicts, // but the best choice among the agents it conflicts with is the one which maximizes the formula itself. - IEnumerable conflictsWithAgentNums = this.conflictCountsPerAgent[chosenAgentIndex].Keys; - IEnumerable conflictsWithInternallyAgentNums = conflictsWithAgentNums.Where(agentNum => this.agentNumToIndex.ContainsKey(agentNum)); - IEnumerable conflictsWithInternallyAgentIndices = conflictsWithInternallyAgentNums.Select(agentNum => this.agentNumToIndex[agentNum]); + IEnumerable conflictsWithAgentNums = ConflictCountsPerAgent[chosenAgentIndex].Keys; + IEnumerable conflictsWithInternallyAgentNums = conflictsWithAgentNums.Where(agentNum => AgentNumToIndex.ContainsKey(agentNum)); + IEnumerable conflictsWithInternallyAgentIndices = conflictsWithInternallyAgentNums.Select(agentNum => AgentNumToIndex[agentNum]); int chosenConflictingAgentIndex = conflictsWithInternallyAgentIndices.MaxByKeyFunc(formula); groupRepA = chosenAgentIndex; groupRepB = chosenConflictingAgentIndex; - ProblemInstance problem = this.cbs.GetProblemInstance(); - time = this.conflictTimesPerAgent[chosenAgentIndex] // Yes, the index of the first and the num of the second + ProblemInstance problem = CBS.GetProblemInstance(); + time = ConflictTimesPerAgent[chosenAgentIndex] // Yes, the index of the first and the num of the second [problem.agents[chosenConflictingAgentIndex].agent.agentNum][0]; return (groupRepA, groupRepB, time); } public CbsConflict GetConflict() { - return this.conflict; + return Conflict; } /// @@ -2290,31 +2283,31 @@ public CbsConflict GetConflict() /// public void AdoptSolutionOf(CbsNode child) { - Trace.Assert(this.g == child.g, "Tried to adopt node of a different cost"); - this.agentAExpansion = CbsNode.ExpansionState.NOT_EXPANDED; - this.agentBExpansion = CbsNode.ExpansionState.NOT_EXPANDED; - this.singleAgentCosts = child.singleAgentCosts; - this.singleAgentPlans = child.singleAgentPlans; - this.conflict = child.conflict; // Probably null, see below - this.isGoal = child.isGoal; - this.countsOfInternalAgentsThatConflict = child.countsOfInternalAgentsThatConflict; - this.conflictCountsPerAgent = child.conflictCountsPerAgent; - this.conflictTimesPerAgent = child.conflictTimesPerAgent; - this.totalExternalAgentsThatConflict = child.totalExternalAgentsThatConflict; - this.minOpsToSolve = child.minOpsToSolve; - this.totalInternalAgentsThatConflict = child.totalInternalAgentsThatConflict; - this.totalConflictsWithExternalAgents = child.totalConflictsWithExternalAgents; - this.totalConflictsBetweenInternalAgents = child.totalConflictsBetweenInternalAgents; - this.largerConflictingGroupSize = child.largerConflictingGroupSize; - for (int i = 0; i < child.newPlans.Length; i++) - { - this.newPlans[i] = this.newPlans[i] || child.newPlans[i]; + Trace.Assert(G == child.G, "Tried to adopt node of a different cost"); + AgentAExpansion = CbsNode.ExpansionState.NOT_EXPANDED; + AgentBExpansion = CbsNode.ExpansionState.NOT_EXPANDED; + SingleAgentCosts = child.SingleAgentCosts; + SingleAgentPlans = child.SingleAgentPlans; + Conflict = child.Conflict; // Probably null, see below + isGoal = child.isGoal; + _countsOfInternalAgentsThatConflict = child._countsOfInternalAgentsThatConflict; + ConflictCountsPerAgent = child.ConflictCountsPerAgent; + ConflictTimesPerAgent = child.ConflictTimesPerAgent; + _totalExternalAgentsThatConflict = child._totalExternalAgentsThatConflict; + MinOpsToSolve = child.MinOpsToSolve; + TotalInternalAgentsThatConflict = child.TotalInternalAgentsThatConflict; + TotalConflictsWithExternalAgents = child.TotalConflictsWithExternalAgents; + TotalConflictsBetweenInternalAgents = child.TotalConflictsBetweenInternalAgents; + _largerConflictingGroupSize = child._largerConflictingGroupSize; + for (int i = 0; i < child.NewPlans.Length; i++) + { + NewPlans[i] = NewPlans[i] || child.NewPlans[i]; } // We don't adopt the child's constraints, nor its agents groups assignment - // this.mdds is kept unchanged too since the cost of the replanned (meta-)agent + // mdds is kept unchanged too since the cost of the replanned (meta-)agent // didn't change and we added no constraints. - this.ChooseConflict(); // child probably hasn't chosen a conflict (and will never get a chance to), + ChooseConflict(); // child probably hasn't chosen a conflict (and will never get a chance to), // need to choose the new conflict to work on. // (if child somehow had a conflict already, ChooseConflict does nothing) // We can't just continue the node's conflict iteration since @@ -2331,12 +2324,12 @@ public override int GetHashCode() unchecked { int ans = 0; - for (int i = 0; i < agentsGroupAssignment.Length; i++) + for (int i = 0; i < AgentsGroupAssignment.Length; i++) { - ans += Constants.PRIMES_FOR_HASHING[i % Constants.PRIMES_FOR_HASHING.Length] * agentsGroupAssignment[i]; + ans += Constants.PRIMES_FOR_HASHING[i % Constants.PRIMES_FOR_HASHING.Length] * AgentsGroupAssignment[i]; } - HashSet constraints = this.GetConstraints(); + HashSet constraints = GetConstraints(); // Add the hash codes for the contraints, ignoring their order foreach (CbsConstraint constraint in constraints) @@ -2359,18 +2352,18 @@ public override bool Equals(object obj) return false; CbsNode other = (CbsNode)obj; - if (this.agentsGroupAssignment.SequenceEqual(other.agentsGroupAssignment) == false) + if (AgentsGroupAssignment.SequenceEqual(other.AgentsGroupAssignment) == false) return false; CbsNode current = this; HashSet other_constraints = other.GetConstraints(); - HashSet constraints = this.GetConstraints(); + HashSet constraints = GetConstraints(); foreach (CbsConstraint constraint in constraints) { if (other_constraints.Contains(constraint) == false) return false; - current = current.prev; + current = current.Prev; } // TODO: Consider replacing the above foreach with constraints.IsSubsetOf(other_constraints) @@ -2382,24 +2375,24 @@ public override bool Equals(object obj) /// public void Clear() { - this.singleAgentPlans = null; - this.singleAgentCosts = null; - this.countsOfInternalAgentsThatConflict = null; - this.conflictCountsPerAgent = null; - this.conflictTimesPerAgent = null; - this.agentNumToIndex = null; + SingleAgentPlans = null; + SingleAgentCosts = null; + _countsOfInternalAgentsThatConflict = null; + ConflictCountsPerAgent = null; + ConflictTimesPerAgent = null; + AgentNumToIndex = null; } public int CompareTo(IBinaryHeapItem item) { CbsNode other = (CbsNode)item; - if (this.f < other.f) + if (F < other.F) return -1; - if (this.f > other.f) + if (F > other.F) return 1; - return this.TieBreak(other); + return TieBreak(other); } public int TieBreak(CbsNode other, bool ignorePartialExpansion = false, bool ignoreDepth = false) @@ -2409,53 +2402,53 @@ public int TieBreak(CbsNode other, bool ignorePartialExpansion = false, bool ign // Prefer fewer external conflicts, even over goal nodes, as goal nodes with less external conflicts are better. // External conflicts are also taken into account by the low level solver to prefer fewer conflicts between fewer agents. // This only helps when this CBS is used as a low level solver, of course. - if (this.totalExternalAgentsThatConflict < other.totalExternalAgentsThatConflict) + if (_totalExternalAgentsThatConflict < other._totalExternalAgentsThatConflict) return -1; - if (this.totalExternalAgentsThatConflict > other.totalExternalAgentsThatConflict) + if (_totalExternalAgentsThatConflict > other._totalExternalAgentsThatConflict) return 1; - if (this.totalConflictsWithExternalAgents < other.totalConflictsWithExternalAgents) + if (TotalConflictsWithExternalAgents < other.TotalConflictsWithExternalAgents) return -1; - if (this.totalConflictsWithExternalAgents > other.totalConflictsWithExternalAgents) + if (TotalConflictsWithExternalAgents > other.TotalConflictsWithExternalAgents) return 1; // Prefer goal nodes. The elaborate form is to keep the comparison consistent. Without it goalA other.g) + //if (g > other.g) // return -1; - //if (this.g < other.g) + //if (g < other.g) // return 1; // Prefer nodes which would possibly require less work. // Remember replans and merges don't necessarily enlarge the total cost, so the number of operations needed to solve // sadly can't be added to the node's total cost. - if (this.cbs.disableTieBreakingByMinOpsEstimate == false) + if (!CBS.DisableTieBreakingByMinOpsEstimate) { - if (this.minOpsToSolve < other.minOpsToSolve) + if (MinOpsToSolve < other.MinOpsToSolve) return -1; - if (this.minOpsToSolve > other.minOpsToSolve) + if (MinOpsToSolve > other.MinOpsToSolve) return 1; } else { - if (this.totalInternalAgentsThatConflict < other.totalInternalAgentsThatConflict) + if (TotalInternalAgentsThatConflict < other.TotalInternalAgentsThatConflict) return -1; - if (this.totalInternalAgentsThatConflict > other.totalInternalAgentsThatConflict) + if (TotalInternalAgentsThatConflict > other.TotalInternalAgentsThatConflict) return 1; } // Prefer fewer internal conflicts if the minOpsToSolve is the same (or turned off) // More conflicts - bigger chance some of them are cardinal (in case they weren't checked already). - if (this.totalConflictsBetweenInternalAgents < other.totalConflictsBetweenInternalAgents) + if (TotalConflictsBetweenInternalAgents < other.TotalConflictsBetweenInternalAgents) return -1; - if (this.totalConflictsBetweenInternalAgents > other.totalConflictsBetweenInternalAgents) + if (TotalConflictsBetweenInternalAgents > other.TotalConflictsBetweenInternalAgents) return 1; // If same number of internal conflicts and agents that conflict - prefer more depth. @@ -2463,9 +2456,9 @@ public int TieBreak(CbsNode other, bool ignorePartialExpansion = false, bool ign // a dependency between agents or find a cardinal conflict if (ignoreDepth == false) { - if (this.depth > other.depth) + if (_depth > other._depth) return -1; - if (this.depth < other.depth) + if (_depth < other._depth) return 1; } @@ -2474,11 +2467,11 @@ public int TieBreak(CbsNode other, bool ignorePartialExpansion = false, bool ign // Prefer partially expanded nodes, in addition to their H bonus. // They're less work because they have less constraints and only one child to generate. // The elaborate form, again, is to keep the comparison consistent. Without it partiallyExpandedApartiallyExpandedB - if ((this.agentAExpansion == CbsNode.ExpansionState.DEFERRED || this.agentBExpansion == CbsNode.ExpansionState.DEFERRED) && - other.agentAExpansion == CbsNode.ExpansionState.NOT_EXPANDED && other.agentBExpansion == CbsNode.ExpansionState.NOT_EXPANDED) + if ((AgentAExpansion == CbsNode.ExpansionState.DEFERRED || AgentBExpansion == CbsNode.ExpansionState.DEFERRED) && + other.AgentAExpansion == CbsNode.ExpansionState.NOT_EXPANDED && other.AgentBExpansion == CbsNode.ExpansionState.NOT_EXPANDED) return -1; - if ((other.agentAExpansion == CbsNode.ExpansionState.DEFERRED || other.agentBExpansion == CbsNode.ExpansionState.DEFERRED) && - this.agentAExpansion == CbsNode.ExpansionState.NOT_EXPANDED && this.agentBExpansion == CbsNode.ExpansionState.NOT_EXPANDED) + if ((other.AgentAExpansion == CbsNode.ExpansionState.DEFERRED || other.AgentBExpansion == CbsNode.ExpansionState.DEFERRED) && + AgentAExpansion == CbsNode.ExpansionState.NOT_EXPANDED && AgentBExpansion == CbsNode.ExpansionState.NOT_EXPANDED) return 1; } @@ -2487,9 +2480,9 @@ public int TieBreak(CbsNode other, bool ignorePartialExpansion = false, bool ign // it comes *out* of OPEN, as choosing the conflict may be expensive. // TODO: enable this when both nodes have a chosen conflict. Nodes can be re-entered // into OPEN. - //if (this.largerConflictingGroupSize < other.largerConflictingGroupSize) + //if (largerConflictingGroupSize < other.largerConflictingGroupSize) // return -1; - //if (this.largerConflictingGroupSize > other.largerConflictingGroupSize) + //if (largerConflictingGroupSize > other.largerConflictingGroupSize) // return 1; return 0; @@ -2499,29 +2492,26 @@ public int TieBreak(CbsNode other, bool ignorePartialExpansion = false, bool ign /// Not used. /// /// - public CbsConstraint GetLastConstraint() - { - return this.constraint; - } + public CbsConstraint GetLastConstraint() => _constraint; public HashSet GetConstraints() { - var constraints = new HashSet(); + HashSet constraints = []; CbsNode current = this; - while (current.depth > 0) // The root has no constraints + while (current._depth > 0) // The root has no constraints { - if (current.constraint != null && // Last check not enough if "surprise merges" happen (merges taken from adopted child) - current.prev.conflict != null && // Can only happen for temporary lookahead nodes that were created and then + if (current._constraint != null && // Last check not enough if "surprise merges" happen (merges taken from adopted child) + current.Prev.Conflict != null && // Can only happen for temporary lookahead nodes that were created and then // later the parent adopted a goal node - this.agentsGroupAssignment[current.prev.conflict.agentAIndex] != - this.agentsGroupAssignment[current.prev.conflict.agentBIndex]) // Ignore constraints that deal with conflicts between + AgentsGroupAssignment[current.Prev.Conflict.agentAIndex] != + AgentsGroupAssignment[current.Prev.Conflict.agentBIndex]) // Ignore constraints that deal with conflicts between // agents that were later merged. They're irrelevant // since merging fixes all conflicts between merged agents. // Nodes that only differ in such irrelevant conflicts will have the same single agent paths. // Dereferencing current.prev is safe because current isn't the root. // Also, merging creates a non-root node with a null constraint, and this helps avoid adding the null to the answer. - constraints.Add(current.constraint); - current = current.prev; + constraints.Add(current._constraint); + current = current.Prev; } return constraints; } @@ -2532,34 +2522,34 @@ public HashSet GetConstraints() /// public List GetConstraintsOrdered() { - var constraints = new List(); + List constraints = []; CbsNode current = this; - while (current.depth > 0) // The root has no constraints + while (current._depth > 0) // The root has no constraints { - if (current.constraint != null && // Next check not enough if "surprise merges" happen (merges taken from adopted child) - current.prev.conflict != null && // Can only happen for temporary lookahead nodes the were created and then later the parent adopted a goal node - this.agentsGroupAssignment[current.prev.conflict.agentAIndex] != - this.agentsGroupAssignment[current.prev.conflict.agentBIndex]) // Ignore constraints that deal with conflicts between + if (current._constraint != null && // Next check not enough if "surprise merges" happen (merges taken from adopted child) + current.Prev.Conflict != null && // Can only happen for temporary lookahead nodes the were created and then later the parent adopted a goal node + AgentsGroupAssignment[current.Prev.Conflict.agentAIndex] != + AgentsGroupAssignment[current.Prev.Conflict.agentBIndex]) // Ignore constraints that deal with conflicts between // agents that were later merged. They're irrelevant // since merging fixes all conflicts between merged agents. // Nodes that only differ in such irrelevant conflicts will have the same single agent paths. // Dereferencing current.prev is safe because current isn't the root. // Also, merging creates a non-root node with a null constraint, and this helps avoid adding the null to the answer. - constraints.Add(current.constraint); - current = current.prev; + constraints.Add(current._constraint); + current = current.Prev; } return constraints; } public HashSet GetPositiveConstraints() { - var constraints = new HashSet(); + HashSet constraints = []; CbsNode current = this; - while (current.depth > 0) + while (current._depth > 0) { - if (current.mustConstraint != null) // TODO: Ignore positive constraints from merged agents - constraints.Add(current.mustConstraint); - current = current.prev; + if (current._mustConstraint != null) // TODO: Ignore positive constraints from merged agents + constraints.Add(current._mustConstraint); + current = current.Prev; } return constraints; } @@ -2568,18 +2558,15 @@ public HashSet GetPositiveConstraints() /// IBinaryHeapItem implementation /// /// - public int GetIndexInHeap() { return this.binaryHeapIndex; } + public int GetIndexInHeap() => _binaryHeapIndex; /// /// IBinaryHeapItem implementation /// /// - public void SetIndexInHeap(int index) { this.binaryHeapIndex = index; } + public void SetIndexInHeap(int index) => _binaryHeapIndex = index; - public Plan CalculateJointPlan() - { - return new Plan(singleAgentPlans); - } + public Plan CalculateJointPlan() => new Plan(SingleAgentPlans); /// /// Check if the agent groups that participate in the conflict of this node should be merged. @@ -2591,18 +2578,18 @@ public Plan CalculateJointPlan() public bool ShouldMerge(int mergeThreshold) { int conflictsCount = 1; // The agentA and agentB conflict in this node. - ISet firstGroup = this.GetGroup(this.conflict.agentAIndex); - ISet secondGroup = this.GetGroup(this.conflict.agentBIndex); + ISet firstGroup = GetGroup(Conflict.agentAIndex); + ISet secondGroup = GetGroup(Conflict.agentBIndex); - CbsNode current = this.prev; + CbsNode current = Prev; int a, b; while (current != null) { - a = current.conflict.agentAIndex; - b = current.conflict.agentBIndex; + a = current.Conflict.agentAIndex; + b = current.Conflict.agentBIndex; if ((firstGroup.Contains(a) && secondGroup.Contains(b)) || (firstGroup.Contains(b) && secondGroup.Contains(a))) conflictsCount++; - current = current.prev; + current = current.Prev; } return conflictsCount > mergeThreshold; @@ -2618,29 +2605,29 @@ public bool ShouldMerge(int mergeThreshold) public bool ShouldMerge(int mergeThreshold, int agentAIndex, int agentBIndex) { int conflictsCount = 0; - ISet firstGroup = this.GetGroup(agentAIndex); - ISet secondGroup = this.GetGroup(agentBIndex); - ProblemInstance problem = this.cbs.GetProblemInstance(); + ISet firstGroup = GetGroup(agentAIndex); + ISet secondGroup = GetGroup(agentBIndex); + ProblemInstance problem = CBS.GetProblemInstance(); CbsNode current = this; while (current != null) { - for (int i = 0; i < this.conflictCountsPerAgent.Length; i++) + for (int i = 0; i < ConflictCountsPerAgent.Length; i++) { if (firstGroup.Contains(i) == false && secondGroup.Contains(i) == false) continue; - foreach (var kvp in this.conflictCountsPerAgent[i]) + foreach (var kvp in ConflictCountsPerAgent[i]) { if (i > kvp.Key) continue; // Count each conflict once - if (newPlans[i] == false && newPlans[kvp.Key] == false) + if (NewPlans[i] == false && NewPlans[kvp.Key] == false) continue; // Only count conflicts of new plans if (firstGroup.Contains(i) || secondGroup.Contains(i)) conflictsCount += kvp.Value; } } - current = current.prev; + current = current.Prev; } @@ -2661,8 +2648,8 @@ public bool ShouldMerge(int mergeThreshold, int agentAIndex, int agentBIndex) public bool ShouldMerge(int mergeThreshold, int[][] globalConflictCounter) { int conflictCounter = 0; - ISet firstGroup = this.GetGroup(this.conflict.agentAIndex); - ISet secondGroup = this.GetGroup(this.conflict.agentBIndex); + ISet firstGroup = GetGroup(Conflict.agentAIndex); + ISet secondGroup = GetGroup(Conflict.agentBIndex); foreach (int a in firstGroup) { @@ -2685,19 +2672,19 @@ public bool ShouldMerge(int mergeThreshold, int[][] globalConflictCounter) public bool ShouldMerge(int mergeThreshold, int[][] globalConflictCounter, int agentAIndex, int agentBIndex) { int conflictsCount = 0; - ISet firstGroup = this.GetGroup(agentAIndex); - ISet secondGroup = this.GetGroup(agentBIndex); - ProblemInstance problem = this.cbs.GetProblemInstance(); + ISet firstGroup = GetGroup(agentAIndex); + ISet secondGroup = GetGroup(agentBIndex); + ProblemInstance problem = CBS.GetProblemInstance(); - for (int i = 0; i < this.conflictCountsPerAgent.Length; i++) + for (int i = 0; i < ConflictCountsPerAgent.Length; i++) { if (firstGroup.Contains(i) == false && secondGroup.Contains(i) == false) continue; - foreach (var kvp in this.conflictCountsPerAgent[i]) + foreach (var kvp in ConflictCountsPerAgent[i]) { if (i > kvp.Key) continue; // Count each conflict once - if (newPlans[i] == false && newPlans[kvp.Key] == false) + if (NewPlans[i] == false && NewPlans[kvp.Key] == false) continue; // Only count conflicts of new plans if (firstGroup.Contains(i) || secondGroup.Contains(i)) { @@ -2716,12 +2703,12 @@ public bool ShouldMerge(int mergeThreshold, int[][] globalConflictCounter, int a /// public ISet GetGroup(int agentIndex) { - int groupNumber = this.agentsGroupAssignment[agentIndex]; - ISet group = new SortedSet(); + int groupNumber = AgentsGroupAssignment[agentIndex]; + SortedSet group = []; - for (int i = 0; i < agentsGroupAssignment.Length; i++) + for (int i = 0; i < AgentsGroupAssignment.Length; i++) { - if (agentsGroupAssignment[i] == groupNumber) + if (AgentsGroupAssignment[i] == groupNumber) group.Add(i); } return group; @@ -2737,10 +2724,10 @@ public int GetGroupCost(int groupNumber) if (Constants.costFunction == Constants.CostFunction.SUM_OF_COSTS) { int cost = 0; - for (int i = 0; i < agentsGroupAssignment.Length; i++) + for (int i = 0; i < AgentsGroupAssignment.Length; i++) { - if (agentsGroupAssignment[i] == groupNumber) - cost += this.singleAgentCosts[i]; + if (AgentsGroupAssignment[i] == groupNumber) + cost += SingleAgentCosts[i]; } return cost; } @@ -2748,11 +2735,11 @@ public int GetGroupCost(int groupNumber) Constants.costFunction == Constants.CostFunction.MAKESPAN_THEN_SUM_OF_COSTS) { int cost = 0; - for (int i = 0; i < agentsGroupAssignment.Length; i++) + for (int i = 0; i < AgentsGroupAssignment.Length; i++) { - if (agentsGroupAssignment[i] == groupNumber) - if (this.singleAgentCosts[i] > cost) - cost = this.singleAgentCosts[i]; + if (AgentsGroupAssignment[i] == groupNumber) + if (SingleAgentCosts[i] > cost) + cost = SingleAgentCosts[i]; } return cost; } @@ -2767,12 +2754,12 @@ public int GetGroupCost(int groupNumber) /// public int GetGroupSize(int agentIndex) { - int groupNumber = this.agentsGroupAssignment[agentIndex]; + int groupNumber = AgentsGroupAssignment[agentIndex]; int count = 0; - for (int i = 0; i < agentsGroupAssignment.Length; i++) + for (int i = 0; i < AgentsGroupAssignment.Length; i++) { - if (agentsGroupAssignment[i] == groupNumber) + if (AgentsGroupAssignment[i] == groupNumber) count += 1; } return count; @@ -2784,15 +2771,15 @@ public int GetGroupSize(int agentIndex) /// public int[] GetGroupSizes() { - int[] counts = new int[this.agentsGroupAssignment.Length]; + Span counts = stackalloc int[AgentsGroupAssignment.Length]; - for (int i = 0; i < agentsGroupAssignment.Length; i++) - counts[this.agentsGroupAssignment[i]]++; + for (int i = 0; i < AgentsGroupAssignment.Length; i++) + counts[AgentsGroupAssignment[i]]++; - int[] groupSizes = new int[this.agentsGroupAssignment.Length]; + int[] groupSizes = new int[AgentsGroupAssignment.Length]; - for (int i = 0; i < agentsGroupAssignment.Length; i++) - groupSizes[i] = counts[this.agentsGroupAssignment[i]]; + for (int i = 0; i < AgentsGroupAssignment.Length; i++) + groupSizes[i] = counts[AgentsGroupAssignment[i]]; return groupSizes; } @@ -2803,25 +2790,25 @@ public int[] GetGroupSizes() /// public ISet[] GetGroups() { - Dictionary> repsToGroups = new Dictionary>(); + Dictionary> repsToGroups = []; - for (int i = 0; i < agentsGroupAssignment.Length; i++) + for (int i = 0; i < AgentsGroupAssignment.Length; i++) { - int groupRep = this.agentsGroupAssignment[i]; + int groupRep = AgentsGroupAssignment[i]; if (repsToGroups.ContainsKey(groupRep)) repsToGroups[groupRep].Add(i); else { - var newGroup = new HashSet(); + HashSet newGroup = []; newGroup.Add(i); repsToGroups[groupRep] = newGroup; } } - ISet[] res = new HashSet[this.agentsGroupAssignment.Length]; + ISet[] res = new HashSet[AgentsGroupAssignment.Length]; for (int i = 0; i < res.Length; i++) - res[i] = repsToGroups[this.agentsGroupAssignment[i]]; + res[i] = repsToGroups[AgentsGroupAssignment[i]]; return res; } @@ -2837,38 +2824,38 @@ public void MergeGroups(int a, int b, bool fixCounts = true) if (b < a) (a, b) = (b, a); - ProblemInstance problem = this.cbs.GetProblemInstance(); + ProblemInstance problem = CBS.GetProblemInstance(); int aAgentNum = problem.agents[a].agent.agentNum; int bAgentNum = problem.agents[b].agent.agentNum; - for (int i = 0; i < agentsGroupAssignment.Length; i++) + for (int i = 0; i < AgentsGroupAssignment.Length; i++) { - if (agentsGroupAssignment[i] == b) + if (AgentsGroupAssignment[i] == b) { - agentsGroupAssignment[i] = (ushort)a; + AgentsGroupAssignment[i] = (ushort)a; } } if (fixCounts) { - this.conflictCountsPerAgent[a].Remove(bAgentNum); // It isn't really necessary to update the conflictCountPerAgent dictionaries of the merged agents - + ConflictCountsPerAgent[a].Remove(bAgentNum); // It isn't really necessary to update the conflictCountPerAgent dictionaries of the merged agents - // they're about to be replanned and have their dictionaries updated anyway - this.conflictTimesPerAgent[a].Remove(bAgentNum); - this.conflictCountsPerAgent[b].Clear(); - this.conflictTimesPerAgent[b].Clear(); - for (int i = 0; i < this.conflictCountsPerAgent.Length; i++) + ConflictTimesPerAgent[a].Remove(bAgentNum); + ConflictCountsPerAgent[b].Clear(); + ConflictTimesPerAgent[b].Clear(); + for (int i = 0; i < ConflictCountsPerAgent.Length; i++) { - if (this.conflictCountsPerAgent[i].ContainsKey(bAgentNum)) + if (ConflictCountsPerAgent[i].ContainsKey(bAgentNum)) { - if (this.conflictCountsPerAgent[i].ContainsKey(aAgentNum) == false) - this.conflictCountsPerAgent[i][aAgentNum] = 0; - this.conflictCountsPerAgent[i][aAgentNum] += this.conflictCountsPerAgent[i][bAgentNum]; - this.conflictCountsPerAgent[i].Remove(bAgentNum); - - if (this.conflictTimesPerAgent[i].ContainsKey(aAgentNum) == false) - this.conflictTimesPerAgent[i][aAgentNum] = new List(this.conflictTimesPerAgent[i][bAgentNum].Count); - this.conflictTimesPerAgent[i][aAgentNum].AddRange(this.conflictTimesPerAgent[i][bAgentNum]); - this.conflictTimesPerAgent[i].Remove(bAgentNum); + if (ConflictCountsPerAgent[i].ContainsKey(aAgentNum) == false) + ConflictCountsPerAgent[i][aAgentNum] = 0; + ConflictCountsPerAgent[i][aAgentNum] += ConflictCountsPerAgent[i][bAgentNum]; + ConflictCountsPerAgent[i].Remove(bAgentNum); + + if (ConflictTimesPerAgent[i].ContainsKey(aAgentNum) == false) + ConflictTimesPerAgent[i][aAgentNum] = new List(ConflictTimesPerAgent[i][bAgentNum].Count); + ConflictTimesPerAgent[i][aAgentNum].AddRange(ConflictTimesPerAgent[i][bAgentNum]); + ConflictTimesPerAgent[i].Remove(bAgentNum); } } } @@ -2876,12 +2863,12 @@ public void MergeGroups(int a, int b, bool fixCounts = true) public void PrintConflict() { - if (conflict != null) + if (Conflict != null) { Debug.WriteLine("Conflict:"); - Debug.WriteLine("Agents:({0},{1})", conflict.agentAIndex, conflict.agentBIndex); - Debug.WriteLine("Location:({0},{1})", conflict.agentAmove.X, conflict.agentAmove.Y); - Debug.WriteLine("Time:{0}", conflict.timeStep); + Debug.WriteLine("Agents:({0},{1})", Conflict.agentAIndex, Conflict.agentBIndex); + Debug.WriteLine("Location:({0},{1})", Conflict.agentAmove.X, Conflict.agentAmove.Y); + Debug.WriteLine("Time:{0}", Conflict.timeStep); } Debug.WriteLine(""); } @@ -2894,7 +2881,7 @@ public void PrintConflict() /// public int PathLength(int agent) { - List moves = singleAgentPlans[agent].LocationAtTimes; + List moves = SingleAgentPlans[agent].LocationAtTimes; Move goal = moves[moves.Count - 1]; for (int i = moves.Count - 2; i >= 0; i--) { @@ -2909,22 +2896,22 @@ public bool DoesMustConstraintAllow(CbsConstraint check) CbsNode current = this; while (current != null) { - if (current.mustConstraint != null && !current.mustConstraint.Allows(check)) + if (current._mustConstraint != null && !current._mustConstraint.Allows(check)) return false; - current = current.prev; + current = current.Prev; } return true; } public void SetMustConstraint(CbsConstraint set) { - this.mustConstraint = set; + _mustConstraint = set; } private bool isGoal = false; public bool GoalTest() { - if (this.g < this.cbs.minSolutionCost) + if (G < CBS.MinSolutionCost) return false; return isGoal; } @@ -2933,33 +2920,30 @@ public bool GoalTest() { /// For CBS IDA* only. /// TODO: Consider inheriting from CbsNode and overriding the Replan method instead. /// - /// - /// - /// /// Whether a path was successfully found public bool Replan3b(int agentToReplan, int depthToReplan, int minPathCost = -1, int maxPathCost = int.MaxValue) { - ProblemInstance problem = this.cbs.GetProblemInstance(); + ProblemInstance problem = CBS.GetProblemInstance(); var internalCAT = new ConflictAvoidanceTable(); ConflictAvoidanceTable CAT; - if (this.cbs.externalCAT != null) + if (CBS.ExternalCAT != null) { CAT = new CAT_U(); - ((CAT_U)CAT).Join(this.cbs.externalCAT); + ((CAT_U)CAT).Join(CBS.ExternalCAT); ((CAT_U)CAT).Join(internalCAT); } else CAT = internalCAT; - HashSet newConstraints = this.GetConstraints(); + HashSet newConstraints = GetConstraints(); ISet constraints; - if (this.cbs.externalConstraints != null) + if (CBS.ExternalConstraints != null) { constraints = new HashSet_U(); - ((HashSet_U)constraints).Join(this.cbs.externalConstraints); + ((HashSet_U)constraints).Join(CBS.ExternalConstraints); ((HashSet_U)constraints).Join(newConstraints); } else @@ -2968,17 +2952,17 @@ public bool Replan3b(int agentToReplan, int depthToReplan, int minPathCost = -1, ISet positiveConstraints = null; HashSet newPositiveConstraints = null; - if (this.cbs.doMalte) - newPositiveConstraints = this.GetPositiveConstraints(); - if (this.cbs.externalPositiveConstraints != null && this.cbs.externalPositiveConstraints.Count != 0 && + if (CBS.DoMalte) + newPositiveConstraints = GetPositiveConstraints(); + if (CBS.ExternalPositiveConstraints != null && CBS.ExternalPositiveConstraints.Count != 0 && newPositiveConstraints != null && newPositiveConstraints.Count != 0) { positiveConstraints = new HashSet_U(); - ((HashSet_U)positiveConstraints).Join(this.cbs.externalPositiveConstraints); + ((HashSet_U)positiveConstraints).Join(CBS.ExternalPositiveConstraints); ((HashSet_U)positiveConstraints).Join(newPositiveConstraints); } - else if (this.cbs.externalPositiveConstraints != null && this.cbs.externalPositiveConstraints.Count != 0) - positiveConstraints = this.cbs.externalPositiveConstraints; + else if (CBS.ExternalPositiveConstraints != null && CBS.ExternalPositiveConstraints.Count != 0) + positiveConstraints = CBS.ExternalPositiveConstraints; else if (newPositiveConstraints != null && newPositiveConstraints.Count != 0) positiveConstraints = newPositiveConstraints; @@ -2988,34 +2972,34 @@ public bool Replan3b(int agentToReplan, int depthToReplan, int minPathCost = -1, depthToReplan = Math.Max(depthToReplan, maxConstraintTimeStep); // Give all constraints a chance to affect the plan } - List subGroup = new List(); - int groupNum = this.agentsGroupAssignment[agentToReplan]; - for (int i = 0; i < agentsGroupAssignment.Length; i++) + List subGroup = []; + int groupNum = AgentsGroupAssignment[agentToReplan]; + for (int i = 0; i < AgentsGroupAssignment.Length; i++) { - if (this.agentsGroupAssignment[i] == groupNum) + if (AgentsGroupAssignment[i] == groupNum) subGroup.Add(problem.agents[i]); else - internalCAT.AddPlan(singleAgentPlans[i]); + internalCAT.AddPlan(SingleAgentPlans[i]); } - this.replanSize = (ushort)subGroup.Count; + ReplanSize = (ushort)subGroup.Count; - ICbsSolver relevantSolver = this.solver; + ICbsSolver relevantSolver = _solver; if (subGroup.Count == 1) - relevantSolver = this.singleAgentSolver; + relevantSolver = _singleAgentSolver; ProblemInstance subProblem = problem.Subproblem(subGroup.ToArray()); subProblem.parameters = problem.parameters; MDD mdd = null; - if (this.cbs.replanSameCostWithMdd) - mdd = this.mdds[agentToReplan]; - double startTime = this.cbs.runner.ElapsedMilliseconds(); - relevantSolver.Setup(subProblem, depthToReplan, this.cbs.runner, CAT, constraints, positiveConstraints, + if (CBS.ReplanSameCostWithMdd) + mdd = _mdds[agentToReplan]; + double startTime = CBS._runner.ElapsedMilliseconds(); + relevantSolver.Setup(subProblem, depthToReplan, CBS._runner, CAT, constraints, positiveConstraints, minPathCost, maxPathCost, mdd); bool solved = relevantSolver.Solve(); - double endTime = this.cbs.runner.ElapsedMilliseconds(); - this.cbs.timePlanningPaths += endTime - startTime; + double endTime = CBS._runner.ElapsedMilliseconds(); + CBS.TimePlanningPaths += endTime - startTime; relevantSolver.AccumulateStatistics(); relevantSolver.ClearStatistics(); @@ -3026,35 +3010,35 @@ public bool Replan3b(int agentToReplan, int depthToReplan, int minPathCost = -1, int j = 0; SinglePlan[] singlePlans = relevantSolver.GetSinglePlans(); int[] singleCosts = relevantSolver.GetSingleCosts(); - for (int i = 0; i < agentsGroupAssignment.Length; i++) + for (int i = 0; i < AgentsGroupAssignment.Length; i++) { - if (this.agentsGroupAssignment[i] == groupNum) + if (AgentsGroupAssignment[i] == groupNum) { - this.singleAgentPlans[i] = singlePlans[j]; - this.singleAgentPlans[i].AgentNum = problem.agents[groupNum].agent.agentNum; // Use the group's representative - this.singleAgentCosts[i] = singleCosts[j]; + SingleAgentPlans[i] = singlePlans[j]; + SingleAgentPlans[i].AgentNum = problem.agents[groupNum].agent.agentNum; // Use the group's representative + SingleAgentCosts[i] = singleCosts[j]; j++; } } - Trace.Assert(j == replanSize); + Trace.Assert(j == ReplanSize); // Calc g if (Constants.costFunction == Constants.CostFunction.SUM_OF_COSTS) { - this.g = (ushort)this.singleAgentCosts.Sum(); + G = (ushort)SingleAgentCosts.Sum(); } else if (Constants.costFunction == Constants.CostFunction.MAKESPAN || Constants.costFunction == Constants.CostFunction.MAKESPAN_THEN_SUM_OF_COSTS) { - this.g = (ushort)this.singleAgentCosts.Max(); + G = (ushort)SingleAgentCosts.Max(); } else throw new NotImplementedException($"Unsupported cost function {Constants.costFunction}"); // PrintPlan(); - this.isGoal = this.countsOfInternalAgentsThatConflict.All(i => i == 0); - //this.ChooseConflict(); + isGoal = _countsOfInternalAgentsThatConflict.All(i => i == 0); + //ChooseConflict(); // PrintConflict(); return true; @@ -3063,19 +3047,14 @@ public bool Replan3b(int agentToReplan, int depthToReplan, int minPathCost = -1, /// /// Assumes agents conflict at the given time, and an MDD has been built for the agent /// - /// - /// - /// - /// - /// public bool DoesAgentHaveNoOtherOption(int agentIndex, int conflictTime, int conflictingAgentIndex, ISet[] groups) { - bool stayingAtGoalConflict = conflictTime > this.mddNarrownessValues[agentIndex].Keys.Max(); // The time step of reaching the goal must be present here, possibly together with extra later time steps the MDD was built with + bool stayingAtGoalConflict = conflictTime > MDDNarrownessValues[agentIndex].Keys.Max(); // The time step of reaching the goal must be present here, possibly together with extra later time steps the MDD was built with if (stayingAtGoalConflict) // Then it must be a vertex conflict, and the agent can't have another option at same cost return true; - else if (!this.mddNarrownessValues[agentIndex].ContainsKey(conflictTime)) + else if (!MDDNarrownessValues[agentIndex].ContainsKey(conflictTime)) return false; - else if (mddNarrownessValues[agentIndex][conflictTime] == MDD.LevelNarrowness.WIDTH_1) + else if (MDDNarrownessValues[agentIndex][conflictTime] == MDD.LevelNarrowness.WIDTH_1) return true; else // mddNarrownessValues[agentIndex][conflictTime] == ONE_LOCATION_MULTIPLE_DIRECTIONS { @@ -3099,41 +3078,41 @@ public bool DoesAgentHaveNoOtherOption(int agentIndex, int conflictTime, int con /// public class AgentToCheckForCardinalConflicts : IBinaryHeapItem { - int groupSize; - int degree; - int planCost; - public int index; + private readonly int _groupSize; + private readonly int _degree; + private readonly int _planCost; + private readonly int _index; public AgentToCheckForCardinalConflicts(int groupSize, int degree, int planCost, int index) { - this.groupSize = groupSize; - this.degree = degree; - this.planCost = planCost; - this.index = index; + _groupSize = groupSize; + _degree = degree; + _planCost = planCost; + _index = index; } public int CompareTo(IBinaryHeapItem item) { AgentToCheckForCardinalConflicts other = (AgentToCheckForCardinalConflicts)item; - if (this.groupSize < other.groupSize) + if (_groupSize < other._groupSize) return -1; - else if (this.groupSize > other.groupSize) + else if (_groupSize > other._groupSize) return 1; - if (this.degree < other.degree) + if (_degree < other._degree) return -1; - else if (this.degree > other.degree) + else if (_degree > other._degree) return 1; - if (this.planCost < other.planCost) + if (_planCost < other._planCost) return -1; - else if (this.planCost > other.planCost) + else if (_planCost > other._planCost) return 1; - if (this.index < other.index) + if (_index < other._index) return -1; - else if (this.index > other.index) + else if (_index > other._index) return 1; else return 0; diff --git a/DynamicLazyOpenList.cs b/DynamicLazyOpenList.cs index fa220ba..68843ce 100644 --- a/DynamicLazyOpenList.cs +++ b/DynamicLazyOpenList.cs @@ -50,7 +50,7 @@ public override Item Remove() // No need to run the expensive heuristic - it can't push back a node over another. Debug.WriteLine("Fewer than 2 nodes in the open list - not applying the heuristic"); node = base.Remove(); // Throws if Count == 0 - this.lastF = node.f; + this.lastF = node.F; return node; } // There are alternatives to the lowest cost node in the open list, try to postpone expansion of it: @@ -60,17 +60,17 @@ public override Item Remove() node = base.Remove(); if (node.GoalTest() == true || // Can't improve the h of the goal - node.hBonus > 0 || // Already computed the expensive heuristic + node.HBonus > 0 || // Already computed the expensive heuristic this.runner.ElapsedMilliseconds() > Constants.MAX_TIME) // No time to continue improving H. break; var next = base.Peek(); - int targetH = node.GetTargetH(next.f + 1); // Don't assume f = g + h (but do assume integer costs) + int targetH = node.GetTargetH(next.F + 1); // Don't assume f = g + h (but do assume integer costs) int expensiveEstimate = (int)this.expensive.h(node, targetH); - if (node.h < expensiveEstimate) // Node may have inherited a better estimate from its parent + if (node.H < expensiveEstimate) // Node may have inherited a better estimate from its parent { - node.hBonus += expensiveEstimate - node.h; - node.h = expensiveEstimate; + node.HBonus += expensiveEstimate - node.H; + node.H = expensiveEstimate; } if (node.CompareTo(next) == 1) // node is not the smallest F anymore - re-insert into open list @@ -91,7 +91,7 @@ public override Item Remove() break; } } - this.lastF = node.f; + this.lastF = node.F; return node; } diff --git a/DynamicRationalLazyOpenList.cs b/DynamicRationalLazyOpenList.cs index 9785adb..e9561e9 100644 --- a/DynamicRationalLazyOpenList.cs +++ b/DynamicRationalLazyOpenList.cs @@ -93,16 +93,16 @@ public override WorldState Remove() if (node.GoalTest() == true || // Can't improve the h of the goal this.runner.ElapsedMilliseconds() > Constants.MAX_TIME) // No time to continue improving H. { - if (node.g + node.h < this.lastF) // This can happen if the last removed node had many runs of the expensive heuristic, which this node didn't yet have. + if (node.G + node.H < this.lastF) // This can happen if the last removed node had many runs of the expensive heuristic, which this node didn't yet have. { - int newH = this.lastF - node.g; - node.hBonus += newH - node.h; - node.h = newH; // Just so we don't throw an inconsistency exception + int newH = this.lastF - node.G; + node.HBonus += newH - node.H; + node.H = newH; // Just so we don't throw an inconsistency exception } break; } - if (node.hBonus > 0) // Improving the h will be more difficult than usual - don't try + if (node.HBonus > 0) // Improving the h will be more difficult than usual - don't try { this.skips++; // TODO: Separate statistic? break; @@ -216,14 +216,14 @@ public override WorldState Remove() //for (int i = 0; i < NUM_CAPS; ++i) //Console.WriteLine("Expected regret for cap {0}:{1}", ((double)(1 << i)) / 1000, expectedRegret[i]/1000.0); - if (node.g + node.h < lastF) // Must improve the heuristic estimate to be consistent + if (node.G + node.H < lastF) // Must improve the heuristic estimate to be consistent millisCap = Double.MaxValue; bool success = false; if (millisCap > overhead) // Worth running the expensive heuristic { next = this.Peek(); - int targetH = node.GetTargetH(next.f + 1); + int targetH = node.GetTargetH(next.F + 1); double expensiveCallStartTime = this.runner.ElapsedMilliseconds(); int expensiveEstimate = (int)this.expensive.h(node, targetH, -1, (int)(expensiveCallStartTime + millisCap), false); @@ -231,11 +231,11 @@ public override WorldState Remove() bool nodeSolved = node.GoalTest(); - if (expensiveEstimate > node.h) // Node may have inherited a better estimate from + if (expensiveEstimate > node.H) // Node may have inherited a better estimate from // its parent so this check is necessary for failures { - node.hBonus += expensiveEstimate - node.h; - node.h = expensiveEstimate; // If this wasn't a success, the assignment here serves to force the next heuristic call + node.HBonus += expensiveEstimate - node.H; + node.H = expensiveEstimate; // If this wasn't a success, the assignment here serves to force the next heuristic call // to search deeper, since we're always consistent. } @@ -276,7 +276,7 @@ public override WorldState Remove() else this.skips++; - if (success || node.g + node.h < lastF) // Never be inconsistent - don't return nodes with lower F than before. Try searching the node again. + if (success || node.G + node.H < lastF) // Never be inconsistent - don't return nodes with lower F than before. Try searching the node again. { this.Add(node); } @@ -292,7 +292,7 @@ public override WorldState Remove() } finish: - this.lastF = node.g + node.h; + this.lastF = node.G + node.H; this.expandStartTime = this.runner.ElapsedMilliseconds(); return node; } diff --git a/EPEA_Star.cs b/EPEA_Star.cs index acd5f2a..d4b1392 100644 --- a/EPEA_Star.cs +++ b/EPEA_Star.cs @@ -105,9 +105,9 @@ public override void Expand(WorldState nodeP) node.targetDeltaF++; node.remainingDeltaF = node.targetDeltaF; // Just for the following hasChildrenForCurrentDeltaF call. } while (node.hasMoreChildren() && node.hasChildrenForCurrentDeltaF() == false); - } while (node.hasMoreChildren() && node.g + node.sic + node.targetDeltaF <= node.minGoalCost); // Generate more children immediately if we have a lower bound on the solution depth + } while (node.hasMoreChildren() && node.G + node.sic + node.targetDeltaF <= node.minGoalCost); // Generate more children immediately if we have a lower bound on the solution depth - if (node.hasMoreChildren() && node.hasChildrenForCurrentDeltaF() && node.g + node.sic + node.targetDeltaF <= this.maxSolutionCost) + if (node.hasMoreChildren() && node.hasChildrenForCurrentDeltaF() && node.G + node.sic + node.targetDeltaF <= this.maxSolutionCost) { // Assuming the heuristic used doesn't give a lower estimate than SIC for each and every one of the node's children, // (an ok assumption since SIC is quite basic, no heuristic we use is ever worse than it) diff --git a/EnumeratedPDB.cs b/EnumeratedPDB.cs index 2273eb5..71736ca 100644 --- a/EnumeratedPDB.cs +++ b/EnumeratedPDB.cs @@ -226,15 +226,15 @@ public override void build() int nSingleAgentShortestPath = 0; foreach (var a in i.allAgentsState) nSingleAgentShortestPath += this.problem.GetSingleAgentOptimalCost(a); - int nDifference = i.g - nSingleAgentShortestPath; + int nDifference = i.G - nSingleAgentShortestPath; Trace.Assert(nDifference >= 0); Trace.Assert(nDifference < Byte.MaxValue); nCandidateValue = (Byte)nDifference; } else { - Trace.Assert(i.g < Byte.MaxValue); - nCandidateValue = (Byte)i.g; + Trace.Assert(i.G < Byte.MaxValue); + nCandidateValue = (Byte)i.G; } if (nCandidateValue < table[nHash]) { diff --git a/IHeuristicSearchNode.cs b/IHeuristicSearchNode.cs index d0da9b4..9b77942 100644 --- a/IHeuristicSearchNode.cs +++ b/IHeuristicSearchNode.cs @@ -8,13 +8,13 @@ namespace mapf; public interface IHeuristicSearchNode { - int g { get; set; } - int h { get; set; } + int G { get; set; } + int H { get; set; } /// /// Used to mark that heuristic estimate was improved already /// - int hBonus { get; set; } - int f { get; } + int HBonus { get; set; } + int F { get; } bool GoalTest(); diff --git a/MddPruningHeuristicForCbs.cs b/MddPruningHeuristicForCbs.cs index 4330d33..35e9fed 100644 --- a/MddPruningHeuristicForCbs.cs +++ b/MddPruningHeuristicForCbs.cs @@ -86,8 +86,8 @@ public void ClearStatistics() /// public uint h(CbsNode s) { - var agentIndicesAndCosts = (s.conflict.agentAIndex, s.conflict.agentBIndex, - s.singleAgentCosts[s.conflict.agentAIndex], s.singleAgentCosts[s.conflict.agentBIndex]); + var agentIndicesAndCosts = (s.Conflict.agentAIndex, s.Conflict.agentBIndex, + s.SingleAgentCosts[s.Conflict.agentAIndex], s.SingleAgentCosts[s.Conflict.agentBIndex]); if (this.ignoreConstraints) { if (this.cache.ContainsKey(agentIndicesAndCosts)) @@ -103,29 +103,29 @@ public uint h(CbsNode s) return 0; } - if (s.h > 1) + if (s.H > 1) { return 1; // We can't raise the heuristic more than that } - if (s.GetGroupSize(s.conflict.agentAIndex) > 1 || s.GetGroupSize(s.conflict.agentBIndex) > 1) + if (s.GetGroupSize(s.Conflict.agentAIndex) > 1 || s.GetGroupSize(s.Conflict.agentBIndex) > 1) { return 0; // Without saving the result, as it's just a cop-out } - int maxCost = Math.Max(s.singleAgentCosts[s.conflict.agentAIndex], - s.singleAgentCosts[s.conflict.agentBIndex]); + int maxCost = Math.Max(s.SingleAgentCosts[s.Conflict.agentAIndex], + s.SingleAgentCosts[s.Conflict.agentBIndex]); // Building MDDs for the conflicting agents. We can't keep them because we're // destructively syncing them later (the first one, at least). - var mddA = new MDD(s.conflict.agentAIndex, this.instance.agents[s.conflict.agentAIndex].agent.agentNum, - this.instance.agents[s.conflict.agentAIndex].lastMove, - s.singleAgentCosts[s.conflict.agentAIndex], maxCost, + var mddA = new MDD(s.Conflict.agentAIndex, this.instance.agents[s.Conflict.agentAIndex].agent.agentNum, + this.instance.agents[s.Conflict.agentAIndex].lastMove, + s.SingleAgentCosts[s.Conflict.agentAIndex], maxCost, this.instance.GetNumOfAgents(), this.instance, this.ignoreConstraints); - var mddB = new MDD(s.conflict.agentBIndex, this.instance.agents[s.conflict.agentBIndex].agent.agentNum, - this.instance.agents[s.conflict.agentBIndex].lastMove, - s.singleAgentCosts[s.conflict.agentBIndex], maxCost, + var mddB = new MDD(s.Conflict.agentBIndex, this.instance.agents[s.Conflict.agentBIndex].agent.agentNum, + this.instance.agents[s.Conflict.agentBIndex].lastMove, + s.SingleAgentCosts[s.Conflict.agentBIndex], maxCost, this.instance.GetNumOfAgents(), this.instance, this.ignoreConstraints); - s.cbs.mddsBuilt += 2; + s.CBS.MDDsBuilt += 2; (MDD.PruningDone ans, int stat) = mddA.SyncMDDs(mddB, checkTriples: false); if (ans == MDD.PruningDone.EVERYTHING) { @@ -151,7 +151,7 @@ public uint h(CbsNode s) /// public uint h(CbsNode s, int target) { - if (s.g + 1 < target) + if (s.G + 1 < target) { this.targetTooHigh++; return 0; // Currently we can only give an estimate of 1 diff --git a/MvcHeuristicForCbs.cs b/MvcHeuristicForCbs.cs index 0372629..c313528 100644 --- a/MvcHeuristicForCbs.cs +++ b/MvcHeuristicForCbs.cs @@ -70,9 +70,9 @@ public uint h(CbsNode s, int target) Debug.WriteLine($"Computing heuristic estimate for node hash {s.GetHashCode()}"); if (target != int.MaxValue && ( - (s.prev != null && s.prev.minimumVertexCover != (int) ConflictGraph.MinVertexCover.NOT_SET && - target > s.prev.minimumVertexCover + 1) || - (target > s.totalInternalAgentsThatConflict)) + (s.Prev != null && s.Prev.MinimumVertexCover != (int) ConflictGraph.MinVertexCover.NOT_SET && + target > s.Prev.MinimumVertexCover + 1) || + (target > s.TotalInternalAgentsThatConflict)) ) { Debug.WriteLine($"Target estimate {target} was too high!"); @@ -83,30 +83,30 @@ public uint h(CbsNode s, int target) // 0 just signals we couldn't raise the h enough. } - ConflictGraph CardinallyConflictingAgents = new ConflictGraph(s.singleAgentPlans.Length); + ConflictGraph CardinallyConflictingAgents = new ConflictGraph(s.SingleAgentPlans.Length); ISet[] groups = s.GetGroups(); // Populate the cardinal conflict graph - foreach (var agentIndex in Enumerable.Range(0, s.singleAgentPlans.Length)) + foreach (var agentIndex in Enumerable.Range(0, s.SingleAgentPlans.Length)) { - if (s.conflictTimesPerAgent[agentIndex].Count == 0) + if (s.ConflictTimesPerAgent[agentIndex].Count == 0) continue; // Agent has no conflicts - bool hasMdd = s.mddNarrownessValues[agentIndex] != null; + bool hasMdd = s.MDDNarrownessValues[agentIndex] != null; bool canBuildMDD = groups[agentIndex].Count == 1; if (canBuildMDD == false) continue; // We aren't going to lookahead just for the heuristic - foreach (int conflictingAgentNum in s.conflictTimesPerAgent[agentIndex].Keys) + foreach (int conflictingAgentNum in s.ConflictTimesPerAgent[agentIndex].Keys) { - int conflictingAgentIndex = s.agentNumToIndex[conflictingAgentNum]; + int conflictingAgentIndex = s.AgentNumToIndex[conflictingAgentNum]; if (conflictingAgentIndex < agentIndex) // check later continue; - bool otherHasMdd = s.mddNarrownessValues[conflictingAgentIndex] != null; + bool otherHasMdd = s.MDDNarrownessValues[conflictingAgentIndex] != null; bool otherCanBuildMdd = groups[conflictingAgentIndex].Count == 1; if (otherCanBuildMdd == false) continue; // We won't lookahead just for the heuristic - foreach (int conflictTime in s.conflictTimesPerAgent[agentIndex][conflictingAgentNum]) + foreach (int conflictTime in s.ConflictTimesPerAgent[agentIndex][conflictingAgentNum]) { bool otherNarrow; if (otherHasMdd) @@ -135,15 +135,15 @@ public uint h(CbsNode s, int target) } } - if (s.prev == null || s.prev.minimumVertexCover == (int) ConflictGraph.MinVertexCover.NOT_SET) - s.minimumVertexCover = CardinallyConflictingAgents.MinimumVertexCover(); + if (s.Prev == null || s.Prev.MinimumVertexCover == (int) ConflictGraph.MinVertexCover.NOT_SET) + s.MinimumVertexCover = CardinallyConflictingAgents.MinimumVertexCover(); else - s.minimumVertexCover = CardinallyConflictingAgents.MinimumVertexCover(s.prev.minimumVertexCover); + s.MinimumVertexCover = CardinallyConflictingAgents.MinimumVertexCover(s.Prev.MinimumVertexCover); // FIXME: The value might be incorrect after a merge operation which wasn't followed by a restart if (target != int.MaxValue) { - if (s.minimumVertexCover >= target) + if (s.MinimumVertexCover >= target) { Debug.WriteLine($"Target estimate {target} reached"); this.targetReached++; @@ -155,7 +155,7 @@ public uint h(CbsNode s, int target) } } - return (uint)s.minimumVertexCover; + return (uint)s.MinimumVertexCover; } public void Init(ProblemInstance pi, List agentsToConsider) diff --git a/PEA_Star.cs b/PEA_Star.cs index 8ccd964..bbc8520 100644 --- a/PEA_Star.cs +++ b/PEA_Star.cs @@ -60,13 +60,13 @@ override public void Expand(WorldState simpleLookingNode) hasMoreSuccessors = false; this.nextFvalue = int.MaxValue; - this.currentFTarget = node.g + node.h; + this.currentFTarget = node.G + node.H; base.Expand(node); if (hasMoreSuccessors && this.nextFvalue <= this.maxSolutionCost) { - node.h = this.nextFvalue - node.g; // Just to update this node's f value to the desired value. + node.H = this.nextFvalue - node.G; // Just to update this node's f value to the desired value. // Although you could say that since we exhausted the current F value, if we get to this node again it means the heuristic was off by at least 1 this.openList.Add(node); // Re-insert to open list with updated F } @@ -79,14 +79,14 @@ override public void Expand(WorldState simpleLookingNode) /// protected override bool ProcessGeneratedNode(WorldState currentNode) { - if (currentNode.h + currentNode.g == this.currentFTarget) + if (currentNode.H + currentNode.G == this.currentFTarget) return base.ProcessGeneratedNode(currentNode); else generatedAndDiscarded++; // Notice we don't count the discarded nodes in the genereted count, only here - if (currentNode.h + currentNode.g > this.currentFTarget) + if (currentNode.H + currentNode.G > this.currentFTarget) { this.hasMoreSuccessors = true; - this.nextFvalue = (byte)Math.Min(this.nextFvalue, currentNode.h + currentNode.g); + this.nextFvalue = (byte)Math.Min(this.nextFvalue, currentNode.H + currentNode.G); } return false; } diff --git a/Run.cs b/Run.cs index 0db2bff..1a33199 100644 --- a/Run.cs +++ b/Run.cs @@ -839,12 +839,12 @@ public bool SolveGivenProblem(ProblemInstance instance) // ((CBS)((IndependenceDetection)solvers[i]).groupSolver).debug = true; if (solvers[i].GetType() == typeof(CBS) || solvers[i].GetType() == typeof(MACBS_WholeTreeThreshold)) { - if (((CBS)solvers[i]).mergeThreshold == 314159) // MAGIC NUMBER WHICH MAKES US ADJUST B according to map + if (((CBS)solvers[i]).MergeThreshold == 314159) // MAGIC NUMBER WHICH MAKES US ADJUST B according to map { if (instance.gridName.StartsWith("den")) - ((CBS)solvers[i]).mergeThreshold = 10; + ((CBS)solvers[i]).MergeThreshold = 10; else if (instance.gridName.StartsWith("brc") || instance.gridName.StartsWith("ost")) - ((CBS)solvers[i]).mergeThreshold = 100; + ((CBS)solvers[i]).MergeThreshold = 100; } } @@ -856,12 +856,12 @@ public bool SolveGivenProblem(ProblemInstance instance) ((IndependenceDetection)solvers[i]).groupSolver.GetType() == typeof(MACBS_WholeTreeThreshold)) ) { - if (((CBS)((IndependenceDetection)solvers[i]).groupSolver).mergeThreshold == 314159) // MAGIC NUMBER SEE ABOVE + if (((CBS)solvers[i]).MergeThreshold == 314159) // MAGIC NUMBER SEE ABOVE { if (instance.gridName.StartsWith("den")) - ((CBS)((IndependenceDetection)solvers[i]).groupSolver).mergeThreshold = 10; + ((CBS)solvers[i]).MergeThreshold = 10; else if (instance.gridName.StartsWith("brc") || instance.gridName.StartsWith("ost")) - ((CBS)((IndependenceDetection)solvers[i]).groupSolver).mergeThreshold = 100; + ((CBS)solvers[i]).MergeThreshold = 100; } } diff --git a/WorldState.cs b/WorldState.cs index 099a00c..7b7bb54 100644 --- a/WorldState.cs +++ b/WorldState.cs @@ -11,9 +11,9 @@ namespace mapf; public class WorldState : IComparable, IBinaryHeapItem, IHeuristicSearchNode { public int makespan; // Total time steps passed, max(agent makespans) - public int g { get; set; } // Value depends on Constants.costFunction and Constants.sumOfCostsVariant, Sum of agent makespans until they reach their goal - public int h { get; set; } - public int hBonus { get; set; } + public int G { get; set; } // Value depends on Constants.costFunction and Constants.sumOfCostsVariant, Sum of agent makespans until they reach their goal + public int H { get; set; } + public int HBonus { get; set; } public AgentState[] allAgentsState; public WorldState prevStep; private int binaryHeapIndex; @@ -89,7 +89,7 @@ public WorldState(AgentState[] allAgentsState, int minDepth = -1, int minCost = this.goalCost = NOT_SET; this.goalSingleCosts = null; this.singlePlans = null; - this.hBonus = 0; + this.HBonus = 0; this.mddNode = mddNode; } @@ -100,8 +100,8 @@ public WorldState(AgentState[] allAgentsState, int minDepth = -1, int minCost = public WorldState(WorldState cpy) { this.makespan = cpy.makespan; - this.g = cpy.g; - this.h = cpy.h; + this.G = cpy.G; + this.H = cpy.H; // The conflictTimes, conflictCounts and sumConflictCounts are only copied later if necessary. this.minGoalTimeStep = cpy.minGoalTimeStep; this.minGoalCost = cpy.minGoalCost; @@ -120,7 +120,7 @@ public WorldState(WorldState cpy) this.goalCost = NOT_SET; this.goalSingleCosts = null; this.singlePlans = null; - this.hBonus = 0; + this.HBonus = 0; this.mddNode = cpy.mddNode; this.prevStep = cpy; } @@ -166,13 +166,13 @@ public bool GoalTest() } } - if (this.g < this.minGoalCost) + if (this.G < this.minGoalCost) return false; if (this.makespan < this.minGoalTimeStep) return false; - return this.h == 0; // This assumes the heuristic is consistent, + return this.H == 0; // This assumes the heuristic is consistent, // or at least has the property of consistent heuristics that only the goal has h==0. // SIC really is a consistent heuristic, so this is fine for now. // TODO: Implement a proper goal test and use it when h==0. @@ -231,7 +231,7 @@ public int GetGoalCost() { if (Constants.costFunction == Constants.CostFunction.SUM_OF_COSTS) { - return this.g; + return this.G; } else if (Constants.costFunction == Constants.CostFunction.MAKESPAN || Constants.costFunction == Constants.CostFunction.MAKESPAN_THEN_SUM_OF_COSTS) @@ -289,8 +289,8 @@ public void SetSingleCosts(int[] costs) public virtual int CompareTo(IBinaryHeapItem other) { WorldState that = (WorldState)other; - int thisF = this.f; - int thatF = that.f; + int thisF = this.F; + int thatF = that.F; if (thisF < thatF) return -1; if (thisF > thatF) @@ -350,9 +350,9 @@ public int TieBreak(WorldState that) // f, collision sets, conflicts and internal conflicts being equal, prefer nodes with a larger g // - they're closer to the goal so less nodes would probably be generated by them on the way to it. - if (this.g < that.g) + if (this.G < that.G) return 1; - if (this.g > that.g) + if (this.G > that.G) return -1; return 0; @@ -365,12 +365,12 @@ public virtual void CalculateG() { if (Constants.costFunction == Constants.CostFunction.SUM_OF_COSTS) { - g = allAgentsState.Sum(agent => agent.g); + G = allAgentsState.Sum(agent => agent.g); } else if (Constants.costFunction == Constants.CostFunction.MAKESPAN || Constants.costFunction == Constants.CostFunction.MAKESPAN_THEN_SUM_OF_COSTS) { - g = makespan; // Let's hope makespan var is correct + G = makespan; // Let's hope makespan var is correct } else { @@ -383,19 +383,19 @@ public virtual void CalculateG() /// public virtual void Clear() { } - public virtual int f + public virtual int F { get { - return this.g + this.h; + return this.G + this.H; } } - public int GetTargetH(int f) => f - g; + public int GetTargetH(int f) => f - G; public override string ToString() { - var builder = new System.Text.StringBuilder($"{generated} f:{f} makespan:{makespan} h:{h} g:{g} "); + var builder = new System.Text.StringBuilder($"{generated} f:{F} makespan:{makespan} h:{H} g:{G} "); foreach (AgentState temp in allAgentsState) { builder.Append("|"); diff --git a/WorldStateForPartialExpansion.cs b/WorldStateForPartialExpansion.cs index 9ddee8b..d4bdc89 100644 --- a/WorldStateForPartialExpansion.cs +++ b/WorldStateForPartialExpansion.cs @@ -269,12 +269,12 @@ public override void Clear() //this.hBonus = 0; } - public override int f + public override int F { get { - return Math.Max(this.g + this.h, - this.g + this.sic + this.targetDeltaF); + return Math.Max(this.G + this.H, + this.G + this.sic + this.targetDeltaF); } } } From cc6c8377d2e548c4dc1a507b6ce033f3123cf6c0 Mon Sep 17 00:00:00 2001 From: Artem Kliatchkine Date: Mon, 5 May 2025 17:05:31 +0200 Subject: [PATCH 05/14] Removed unnecessary LinkedList usage from SinglePalan (performance reasons). --- A_Star_MDDs.cs | 9 ++++++--- Plan.cs | 22 +++++++++++----------- ProblemInstance.cs | 4 ++-- 3 files changed, 19 insertions(+), 16 deletions(-) diff --git a/A_Star_MDDs.cs b/A_Star_MDDs.cs index 93ca8db..fce1ff6 100644 --- a/A_Star_MDDs.cs +++ b/A_Star_MDDs.cs @@ -270,23 +270,26 @@ private bool GoalTest(A_Star_MDDs_Node toCheck) private SinglePlan[] GetAnswer(A_Star_MDDs_Node finish) { // TODO: Move the construction of the SinglePlans to a static method in SinglePlan - var routes = new LinkedList[problem.Length]; + List[] routes = new List[problem.Length]; for (int i = 0; i < routes.Length; i++) - routes[i] = new LinkedList(); + routes[i] = []; A_Star_MDDs_Node current = finish; while (current != null) { for (int i = 0; i < problem.Length; i++) { - routes[i].AddFirst(new Move(current.allSteps[i].move)); + routes[i].Add(new Move(current.allSteps[i].move)); } current = current.prev; } var ans = new SinglePlan[problem.Length]; for (int i = 0; i < ans.Length; i++) + { + routes[i].Reverse(); ans[i] = new SinglePlan(routes[i], i); + } return ans; } diff --git a/Plan.cs b/Plan.cs index 9394204..6cad191 100644 --- a/Plan.cs +++ b/Plan.cs @@ -317,19 +317,20 @@ public SinglePlan(AgentState goalState) { AgentNum = goalState.agent.agentNum; AgentState currentNode = goalState; - LinkedList locations = []; + List locations = []; while (currentNode != null) { - locations.AddFirst(currentNode.GetMove()); + locations.Add(currentNode.GetMove()); currentNode = currentNode.prev; } - LocationAtTimes = locations.ToList(); + locations.Reverse(); + LocationAtTimes = locations; } - public SinglePlan(LinkedList route, int agentNum) + public SinglePlan(List route, int agentNum) { AgentNum = agentNum; - LocationAtTimes = [.. route]; + LocationAtTimes = route; } public SinglePlan(SinglePlan cpy) @@ -341,8 +342,6 @@ public SinglePlan(SinglePlan cpy) /// /// TODO: Get rid of the else /// - /// - /// public Move GetLocationAt(int time) { if (time < LocationAtTimes.Count) @@ -496,21 +495,22 @@ public override string ToString() public static SinglePlan[] GetSinglePlans(WorldState goalState) // FIXME: Duplication with other methods. { - LinkedList[] allroutes = new LinkedList[goalState.allAgentsState.Length]; + List[] allroutes = new List[goalState.allAgentsState.Length]; for (int i = 0; i < allroutes.Length; i++) - allroutes[i] = new LinkedList(); + allroutes[i] = []; WorldState currentNode = goalState; while (currentNode != null) { for (int i = 0; i < allroutes.Length; i++) - allroutes[i].AddFirst(currentNode.GetSingleAgentMove(i)); + allroutes[i].Add(currentNode.GetSingleAgentMove(i)); currentNode = currentNode.prevStep; } SinglePlan[] ans = new SinglePlan[goalState.allAgentsState.Length]; for (int i = 0; i < ans.Length; i++) { + allroutes[i].Reverse(); ans[i] = new SinglePlan(allroutes[i], goalState.allAgentsState[i].agent.agentNum); } return ans; @@ -521,7 +521,7 @@ public static SinglePlan[] GetSinglePlans(WorldState goalState) // FIXME: Duplic /// /// /// - public static SinglePlan[] GetSinglePlans(LinkedList[] allRoutes) + public static SinglePlan[] GetSinglePlans(List[] allRoutes) { SinglePlan[] ans = new SinglePlan[allRoutes.Length]; for (int i = 0; i < ans.Length; i++) diff --git a/ProblemInstance.cs b/ProblemInstance.cs index 5f619b7..807e232 100644 --- a/ProblemInstance.cs +++ b/ProblemInstance.cs @@ -262,14 +262,14 @@ public Move GetSingleAgentOptimalMove(AgentState agentState) /// An optimal plan for the agent, ignoring all others public SinglePlan GetSingleAgentOptimalPlan(AgentState agentState) { - LinkedList moves = new LinkedList(); + List moves = []; int agentNum = agentState.agent.agentNum; TimedMove current = agentState.lastMove; // The starting position int time = current.Time; while (true) { - moves.AddLast(current); + moves.Add(current); if (agentState.agent.Goal.Equals(current)) break; From 385175d197d92c8f0f7f3dd1c54107702b9d2e49 Mon Sep 17 00:00:00 2001 From: Artem Kliatchkine Date: Mon, 5 May 2025 17:18:59 +0200 Subject: [PATCH 06/14] Removed unnecessary LinkedList usage from Plan class (performance considerations). --- Plan.cs | 35 +++++++++++++++++++---------------- 1 file changed, 19 insertions(+), 16 deletions(-) diff --git a/Plan.cs b/Plan.cs index 6cad191..1fe075f 100644 --- a/Plan.cs +++ b/Plan.cs @@ -12,7 +12,7 @@ namespace mapf; /// public class Plan { - private LinkedList> _locationsAtTimes = []; + private readonly List> _locationsAtTimes = []; /// /// Reconstructs the plan by goind backwards from the goal. @@ -25,9 +25,10 @@ public Plan(WorldState goalState) while (currentNode != null) { List agentMoves = currentNode.GetAgentsMoves(); - _locationsAtTimes.AddFirst(agentMoves); + _locationsAtTimes.Add(agentMoves); currentNode = currentNode.prevStep; } + _locationsAtTimes.Reverse(); } /// @@ -42,9 +43,10 @@ public Plan(AgentState goalState) { List l = []; l.Add(currentNode.GetMove()); - _locationsAtTimes.AddFirst(l); + _locationsAtTimes.Add(l); currentNode = currentNode.prev; } + _locationsAtTimes.Reverse(); } /// @@ -54,16 +56,17 @@ public Plan(LinkedList[] routePerAgent) { for (int i = 0; i < routePerAgent[0].Count; i++) { - _locationsAtTimes.AddLast([]); + _locationsAtTimes.Add([]); } - + + int index = 0; foreach (LinkedList agentRoute in routePerAgent) { - LinkedListNode> locationsAtTime = _locationsAtTimes.First; + List locationsAtTime = _locationsAtTimes[index]; foreach (Move agentLocation in agentRoute) { - locationsAtTime.Value.Add(agentLocation); - locationsAtTime = locationsAtTime.Next; + locationsAtTime.Add(agentLocation); + index++; } } } @@ -83,7 +86,7 @@ public Plan(IEnumerable subplans) foreach (Move move in plan.GetLocationsAt(time)) allMoves.Add(move); - _locationsAtTimes.AddLast(allMoves); + _locationsAtTimes.Add(allMoves); } } @@ -99,7 +102,7 @@ public Plan(IEnumerable subplans) // FIXME: Almost fully duplicates allMoves.Add(plan.GetLocationAt(time)); } - _locationsAtTimes.AddLast(allMoves); + _locationsAtTimes.Add(allMoves); } } @@ -110,7 +113,7 @@ public Plan(Plan cpy) { foreach (List cpyStep in cpy._locationsAtTimes) { - _locationsAtTimes.AddLast([.. cpyStep]); + _locationsAtTimes.Add([.. cpyStep]); } } @@ -127,12 +130,12 @@ public void ContinueWith(Plan other) if (first) { first = false; - if (newLocationsAtTime.SequenceEqual(_locationsAtTimes.Last.Value)) + if (newLocationsAtTime.SequenceEqual(_locationsAtTimes.Last())) continue; else Trace.Assert(false, "Continuing a plan doesn't start from the same state"); } - _locationsAtTimes.AddLast(newLocationsAtTime); + _locationsAtTimes.Add(newLocationsAtTime); } } @@ -176,7 +179,7 @@ public List GetLocationsAt(int time) return _locationsAtTimes.ElementAt(time); // FIXME: Expensive! else { - List toCopy = _locationsAtTimes.Last.Value; + List toCopy = _locationsAtTimes.Last(); List atRest = [.. toCopy]; for (int i = 0; i < atRest.Count; i++) { @@ -186,7 +189,7 @@ public List GetLocationsAt(int time) } } - public LinkedList> GetLocations() => _locationsAtTimes; + public List> GetLocations() => _locationsAtTimes; /// /// NOT the cost, which: @@ -223,7 +226,7 @@ public bool IsColliding(int time, Plan otherPlan) public void PrintPlanIfShort() { var planSize = GetSize(); - var numAgents = _locationsAtTimes.First.Value.Count; + var numAgents = _locationsAtTimes.First().Count; if (planSize < 200 && numAgents < 30) PrintPlan(); else if (planSize >= 200) From 0e4e4d159fe219dd050e309cdb54d65c8a10dc8e Mon Sep 17 00:00:00 2001 From: Artem Kliatchkine Date: Tue, 6 May 2025 09:46:37 +0200 Subject: [PATCH 07/14] Using stack allocation for a minor local array (performance). --- ConflictGraph.cs | 87 ++++++++++++++++++++++++------------------------ 1 file changed, 44 insertions(+), 43 deletions(-) diff --git a/ConflictGraph.cs b/ConflictGraph.cs index f3bf80f..cc1b7f7 100644 --- a/ConflictGraph.cs +++ b/ConflictGraph.cs @@ -5,9 +5,9 @@ namespace mapf; public class ConflictGraph { - public bool[,] G; - public int numOfNodes; - public int numOfEdges; + private bool[,] _g; + private int _numOfNodes; + private int _numOfEdges; public enum MinVertexCover : sbyte { @@ -16,30 +16,31 @@ public enum MinVertexCover : sbyte public ConflictGraph(int numOfAgents) { - this.numOfEdges = 0; - this.numOfNodes = 0; - this.G = new bool[numOfAgents, numOfAgents]; + _numOfEdges = 0; + _numOfNodes = 0; + _g = new bool[numOfAgents, numOfAgents]; for (int i = 0; i< numOfAgents; i++) // FIXME: Not necessary. { for (int j = 0; j < numOfAgents; j++) - G[i, j] = false; + _g[i, j] = false; } } + public ConflictGraph(ConflictGraph other) { - this.numOfEdges = other.numOfEdges; - this.numOfNodes = other.numOfNodes; - this.G = new bool[other.G.GetLength(0), other.G.GetLength(1)]; - Array.Copy(other.G, G, G.Length); + _numOfEdges = other._numOfEdges; + _numOfNodes = other._numOfNodes; + _g = new bool[other._g.GetLength(0), other._g.GetLength(1)]; + Array.Copy(other._g, _g, _g.Length); } public void Add(int agentAId, int agentBId) { - if (!G[agentAId, agentBId]) + if (!_g[agentAId, agentBId]) { - G[agentAId, agentBId] = true; - G[agentBId, agentAId] = true; - numOfEdges++; + _g[agentAId, agentBId] = true; + _g[agentBId, agentAId] = true; + _numOfEdges++; } } @@ -50,18 +51,18 @@ public void Add(int agentAId, int agentBId) /// The size of the 2-approximate minimum vertex cover public int ApproximateMinimumVertexCover(int prevMVC = (int)MinVertexCover.NOT_SET) { - if (this.numOfEdges < 2) - return this.numOfEdges; + if (_numOfEdges < 2) + return _numOfEdges; - var approximateMinCover = new HashSet(); - for (int i = 0; i < this.G.GetLength(0) - 1; i++) + HashSet approximateMinCover = []; + for (int i = 0; i < _g.GetLength(0) - 1; i++) { if (approximateMinCover.Contains(i)) // Node i already in the cover - all its edges are already covered. continue; - for (int j = i + 1; j < this.G.GetLength(1); j++) + for (int j = i + 1; j < _g.GetLength(1); j++) { - if (G[i, j]) + if (_g[i, j]) { if (approximateMinCover.Contains(j) == false) { @@ -83,25 +84,25 @@ public int ApproximateMinimumVertexCover(int prevMVC = (int)MinVertexCover.NOT_S /// The size of the minimum vertex cover public int MinimumVertexCover(int prevMVC = (int) MinVertexCover.NOT_SET) { - if (this.numOfEdges < 2) - return this.numOfEdges; + if (_numOfEdges < 2) + return _numOfEdges; // compute number of nodes that have edges - this.numOfNodes = 0; - for (int i = 0; i < this.G.GetLength(0); i++) + _numOfNodes = 0; + for (int i = 0; i < _g.GetLength(0); i++) { - for (int j = 0; j < this.G.GetLength(1); j++) + for (int j = 0; j < _g.GetLength(1); j++) { - if (G[i,j]) + if (_g[i,j]) { - this.numOfNodes++; + _numOfNodes++; break; } } } if (prevMVC == (int) MinVertexCover.NOT_SET) // root node of CBS tree, or any node on whose parent we decided not to compute this heuristic - for (int i = 1; i < this.numOfNodes; i++) + for (int i = 1; i < _numOfNodes; i++) if (KVertexCover(this, i)) return i; @@ -138,9 +139,9 @@ public int MinimumVertexCover(int prevMVC = (int) MinVertexCover.NOT_SET) /// private static bool KVertexCover(ConflictGraph CG, int k, int lastEdgeX = 0) { - if (CG.numOfEdges == 0) + if (CG._numOfEdges == 0) return true; - else if (CG.numOfEdges > k * CG.numOfNodes - k) // |E| > K*(|V|-1), there are more edges + else if (CG._numOfEdges > k * CG._numOfNodes - k) // |E| > K*(|V|-1), there are more edges // to cover than the maximum number of edges that could be covered with K vertices // (if every vertex chosen for the cover is connected to all other vertices in the graph), // so a K vertex cover is impossible @@ -148,13 +149,13 @@ private static bool KVertexCover(ConflictGraph CG, int k, int lastEdgeX = 0) // Choose an edge (u,v) - (this step is actually O(n^2) but the algorithm assumes is done in constant time) // TODO: Measure if this part is significant - int[] edge = new int[2]; + Span edge = stackalloc int[2]; bool found = false; - for (int i = lastEdgeX; i < CG.G.GetLength(0) - 1 && !found; i++) + for (int i = lastEdgeX; i < CG._g.GetLength(0) - 1 && !found; i++) { - for (int j = i + 1; j < CG.G.GetLength(1) && !found; j++) + for (int j = i + 1; j < CG._g.GetLength(1) && !found; j++) { - if (CG.G[i, j]) + if (CG._g[i, j]) { edge[0] = i; edge[1] = j; @@ -167,20 +168,20 @@ private static bool KVertexCover(ConflictGraph CG, int k, int lastEdgeX = 0) // If any are true, return true. Else return false. for (int i = 0; i < 2; i++) { - ConflictGraph CG_copy = new ConflictGraph(CG); // TODO: This part also costs n^2 - for (int j = 0; j < CG.G.GetLength(0); j++) + ConflictGraph CG_copy = new(CG); // TODO: This part also costs n^2 + for (int j = 0; j < CG._g.GetLength(0); j++) { - if (CG_copy.G[edge[i], j]) + if (CG_copy._g[edge[i], j]) { - CG_copy.G[edge[i], j] = false; - CG_copy.G[j, edge[i]] = false; - CG_copy.numOfEdges--; + CG_copy._g[edge[i], j] = false; + CG_copy._g[j, edge[i]] = false; + CG_copy._numOfEdges--; } } - CG_copy.numOfNodes--; + CG_copy._numOfNodes--; if (KVertexCover(CG_copy, k - 1, edge[0])) return true; } return false; - } + } } From e5c289d22f59702f28459b55fa4331835bd8ff2c Mon Sep 17 00:00:00 2001 From: Artem Kliatchkine Date: Tue, 6 May 2025 10:56:15 +0200 Subject: [PATCH 08/14] Minor code cleanup. --- MddPruningHeuristicForCbs.cs | 139 ++++++++++++++++------------------- 1 file changed, 65 insertions(+), 74 deletions(-) diff --git a/MddPruningHeuristicForCbs.cs b/MddPruningHeuristicForCbs.cs index 35e9fed..e1cd404 100644 --- a/MddPruningHeuristicForCbs.cs +++ b/MddPruningHeuristicForCbs.cs @@ -1,7 +1,6 @@ using System; using System.Collections.Generic; using System.IO; -using System.Linq; namespace mapf; @@ -9,34 +8,26 @@ class MddPruningHeuristicForCbs : ILazyHeuristic { public MddPruningHeuristicForCbs(bool ignoreConstraints = false) { - this.ignoreConstraints = ignoreConstraints; + _ignoreConstraints = ignoreConstraints; } - protected int pruningSuccesses; - protected int pruningFailures; - protected int cacheHits; - protected int targetTooHigh; - protected int accPruningSuccesses; - protected int accPruningFailures; - protected int accCacheHits; - protected int accTargetTooHigh; + private int _pruningSuccesses; + private int _pruningFailures; + private int _cacheHits; + private int _targetTooHigh; + private int _accPruningSuccesses; + private int _accPruningFailures; + private int _accCacheHits; + private int _accTargetTooHigh; - protected bool ignoreConstraints; - protected ProblemInstance instance; + private bool _ignoreConstraints; + private ProblemInstance _instance; /// /// Maps a pair of agents and their costs to whether there's a solution with those costs /// public Dictionary<(int agentAIndex, int agentBIndex, int agentACost, int agentBCost), ushort> cache; - - public int NumStatsColumns - { - get - { - return 4; - } - } - + public int NumStatsColumns => 4; public string GetName() { return "MDD Pruning Heuristic"; @@ -44,26 +35,26 @@ public string GetName() public void AccumulateStatistics() { - this.accPruningSuccesses += this.pruningSuccesses; - this.accPruningFailures += this.pruningFailures; - this.accCacheHits += this.cacheHits; - this.accTargetTooHigh += this.targetTooHigh; + _accPruningSuccesses += _pruningSuccesses; + _accPruningFailures += _pruningFailures; + _accCacheHits += _cacheHits; + _accTargetTooHigh += _targetTooHigh; } public void ClearAccumulatedStatistics() { - this.accPruningSuccesses = 0; - this.accPruningFailures = 0; - this.accCacheHits = 0; - this.accTargetTooHigh = 0; + _accPruningSuccesses = 0; + _accPruningFailures = 0; + _accCacheHits = 0; + _accTargetTooHigh = 0; } public void ClearStatistics() { - this.pruningSuccesses = 0; - this.pruningFailures = 0; - this.cacheHits = 0; - this.targetTooHigh = 0; + _pruningSuccesses = 0; + _pruningFailures = 0; + _cacheHits = 0; + _targetTooHigh = 0; } /// @@ -88,12 +79,12 @@ public uint h(CbsNode s) { var agentIndicesAndCosts = (s.Conflict.agentAIndex, s.Conflict.agentBIndex, s.SingleAgentCosts[s.Conflict.agentAIndex], s.SingleAgentCosts[s.Conflict.agentBIndex]); - if (this.ignoreConstraints) + if (_ignoreConstraints) { - if (this.cache.ContainsKey(agentIndicesAndCosts)) + if (cache.ContainsKey(agentIndicesAndCosts)) { - this.cacheHits++; - return this.cache[agentIndicesAndCosts]; + _cacheHits++; + return cache[agentIndicesAndCosts]; } } // TODO @@ -117,28 +108,28 @@ public uint h(CbsNode s) s.SingleAgentCosts[s.Conflict.agentBIndex]); // Building MDDs for the conflicting agents. We can't keep them because we're // destructively syncing them later (the first one, at least). - var mddA = new MDD(s.Conflict.agentAIndex, this.instance.agents[s.Conflict.agentAIndex].agent.agentNum, - this.instance.agents[s.Conflict.agentAIndex].lastMove, + var mddA = new MDD(s.Conflict.agentAIndex, _instance.agents[s.Conflict.agentAIndex].agent.agentNum, + _instance.agents[s.Conflict.agentAIndex].lastMove, s.SingleAgentCosts[s.Conflict.agentAIndex], maxCost, - this.instance.GetNumOfAgents(), this.instance, this.ignoreConstraints); - var mddB = new MDD(s.Conflict.agentBIndex, this.instance.agents[s.Conflict.agentBIndex].agent.agentNum, - this.instance.agents[s.Conflict.agentBIndex].lastMove, + _instance.GetNumOfAgents(), _instance, _ignoreConstraints); + var mddB = new MDD(s.Conflict.agentBIndex, _instance.agents[s.Conflict.agentBIndex].agent.agentNum, + _instance.agents[s.Conflict.agentBIndex].lastMove, s.SingleAgentCosts[s.Conflict.agentBIndex], maxCost, - this.instance.GetNumOfAgents(), this.instance, this.ignoreConstraints); + _instance.GetNumOfAgents(), _instance, _ignoreConstraints); s.CBS.MDDsBuilt += 2; - (MDD.PruningDone ans, int stat) = mddA.SyncMDDs(mddB, checkTriples: false); + MDD.PruningDone ans = mddA.SyncMDDs(mddB, checkTriples: false).Item1; if (ans == MDD.PruningDone.EVERYTHING) { - if (this.ignoreConstraints) - this.cache.Add(agentIndicesAndCosts, 1); - this.pruningSuccesses++; + if (_ignoreConstraints) + cache.Add(agentIndicesAndCosts, 1); + _pruningSuccesses++; return 1; } else { - if (this.ignoreConstraints) - this.cache.Add(agentIndicesAndCosts, 0); - this.pruningFailures++; + if (_ignoreConstraints) + cache.Add(agentIndicesAndCosts, 0); + _pruningFailures++; return 0; } } @@ -153,48 +144,48 @@ public uint h(CbsNode s, int target) { if (s.G + 1 < target) { - this.targetTooHigh++; + _targetTooHigh++; return 0; // Currently we can only give an estimate of 1 } - return this.h(s); + return h(s); } public void Init(ProblemInstance pi, List agentsToConsider) { - this.instance = pi; + _instance = pi; } public void OutputAccumulatedStatistics(TextWriter output) { - string name = this.GetName(); - Console.WriteLine($"{name} Accumulated Pruning Successes (High-Level): {this.accPruningSuccesses}"); - Console.WriteLine($"{name} Accumulated Pruning Failures (High-Level): {this.accPruningFailures}"); - Console.WriteLine($"{name} Accumulated Cache Hits (High-Level): {this.accCacheHits}"); - Console.WriteLine($"{name} Accumulated Times Target Estimate Was Too High (High-Level): {this.accTargetTooHigh}"); - - output.Write(this.accPruningSuccesses + Run.RESULTS_DELIMITER); - output.Write(this.accPruningFailures + Run.RESULTS_DELIMITER); - output.Write(this.accCacheHits + Run.RESULTS_DELIMITER); - output.Write(this.accTargetTooHigh + Run.RESULTS_DELIMITER); + string name = GetName(); + Console.WriteLine($"{name} Accumulated Pruning Successes (High-Level): {_accPruningSuccesses}"); + Console.WriteLine($"{name} Accumulated Pruning Failures (High-Level): {_accPruningFailures}"); + Console.WriteLine($"{name} Accumulated Cache Hits (High-Level): {_accCacheHits}"); + Console.WriteLine($"{name} Accumulated Times Target Estimate Was Too High (High-Level): {_accTargetTooHigh}"); + + output.Write(_accPruningSuccesses + Run.RESULTS_DELIMITER); + output.Write(_accPruningFailures + Run.RESULTS_DELIMITER); + output.Write(_accCacheHits + Run.RESULTS_DELIMITER); + output.Write(_accTargetTooHigh + Run.RESULTS_DELIMITER); } public void OutputStatistics(TextWriter output) { - string name = this.GetName(); - Console.WriteLine($"{name} Pruning successes (High-Level): {this.pruningSuccesses}"); - Console.WriteLine($"{name} Pruning failures (High-Level): {this.pruningFailures}"); - Console.WriteLine($"{name} Cache hits (High-Level): {this.cacheHits}"); - Console.WriteLine($"{name} Times Target Estimate was Too High (High-Level): {this.targetTooHigh}"); - - output.Write(this.pruningSuccesses + Run.RESULTS_DELIMITER); - output.Write(this.pruningFailures + Run.RESULTS_DELIMITER); - output.Write(this.cacheHits + Run.RESULTS_DELIMITER); - output.Write(this.targetTooHigh + Run.RESULTS_DELIMITER); + string name = GetName(); + Console.WriteLine($"{name} Pruning successes (High-Level): {_pruningSuccesses}"); + Console.WriteLine($"{name} Pruning failures (High-Level): {_pruningFailures}"); + Console.WriteLine($"{name} Cache hits (High-Level): {_cacheHits}"); + Console.WriteLine($"{name} Times Target Estimate was Too High (High-Level): {_targetTooHigh}"); + + output.Write(_pruningSuccesses + Run.RESULTS_DELIMITER); + output.Write(_pruningFailures + Run.RESULTS_DELIMITER); + output.Write(_cacheHits + Run.RESULTS_DELIMITER); + output.Write(_targetTooHigh + Run.RESULTS_DELIMITER); } public void OutputStatisticsHeader(TextWriter output) { - string name = this.GetName(); + string name = GetName(); output.Write($"{name} Pruning Successes (HL)"); output.Write(Run.RESULTS_DELIMITER); output.Write($"{name} Pruning Failures (HL)"); From 18b8647448a5fd5a644d36f0d6b60ff7c381f037 Mon Sep 17 00:00:00 2001 From: Artem Kliatchkine Date: Tue, 6 May 2025 14:07:40 +0200 Subject: [PATCH 09/14] Using Stopwatch instead of Runner to count time in solvers (removing dependency on testing code). --- A_Star.cs | 104 ++++++++++------------ A_Star_MDDs.cs | 62 +++++-------- A_Star_WithOD.cs | 13 +-- CBS.cs | 34 ++++---- CbsHeuristicForAStar.cs | 22 ++--- CbsNode.cs | 32 +++---- CooperativeA_Star.cs | 66 ++++---------- CostTreeNodeSolver.cs | 72 +++++++-------- CostTreeSearchSolver.cs | 131 +++++++++++----------------- DynamicLazyOpenList.cs | 12 ++- DynamicRationalLazyOpenList.cs | 12 +-- EPEA_Star.cs | 7 +- ISolver.cs | 23 ++--- IndependenceDetection.cs | 30 +++---- IndependenceDetectionAgentsGroup.cs | 23 ++--- MddMatchAndPrune.cs | 23 +++-- PEA_Star.cs | 5 +- Program.cs | 6 +- Run.cs | 54 +++--------- 19 files changed, 289 insertions(+), 442 deletions(-) diff --git a/A_Star.cs b/A_Star.cs index 06b1a08..7e9f39b 100644 --- a/A_Star.cs +++ b/A_Star.cs @@ -16,7 +16,7 @@ public class A_Star : ICbsSolver, IMStarSolver, IHeuristicSolver, II protected ProblemInstance instance; protected IHeuristicCalculator heuristic; public OpenList openList; - public Dictionary closedList; + public Dictionary closedList = []; /// /// How much more expensive the solution was than the heuristic's initial estimate /// @@ -69,7 +69,7 @@ public class A_Star : ICbsSolver, IMStarSolver, IHeuristicSolver, II //protected Dictionary mstarPlanBasesToTheirPlans; protected Dictionary[]> mstarPlanBasesToTheirConstraints; // Most nodes won't have constraints so I don't want to make their memory footprint needlessly large. protected List mstarBackPropagationConflictList; - protected Run runner; + protected Stopwatch stopwatch; protected Plan solution; //// //// For CBS/A* @@ -81,13 +81,9 @@ public class A_Star : ICbsSolver, IMStarSolver, IHeuristicSolver, II /// public A_Star(IHeuristicCalculator heuristic = null, bool mStar = false, bool mStarShuffle = false) { - this.closedList = new Dictionary(); this.openList = new OpenList(this); this.heuristic = heuristic; - this.queryConstraint = new CbsConstraint(); - this.queryConstraint.queryInstance = true; - this.mstar = mStar; this.doMstarShuffle = mStarShuffle; } @@ -95,13 +91,13 @@ public A_Star(IHeuristicCalculator heuristic = null, bool mStar = fa /// /// Setup the relevant data structures for a run under CBS. /// - public virtual void Setup(ProblemInstance problemInstance, int minDepth, Run runner, + public virtual void Setup(ProblemInstance problemInstance, int minDepth, Stopwatch stopwatch, ConflictAvoidanceTable CAT = null, ISet constraints = null, ISet positiveConstraints = null, int minCost = -1, int maxCost = int.MaxValue, MDD mdd = null) { this.instance = problemInstance; - this.runner = runner; + this.stopwatch = stopwatch; MDDNode mddRoot = null; if (mdd != null) { @@ -136,7 +132,7 @@ public virtual void Setup(ProblemInstance problemInstance, int minDepth, Run run { int timeStep = con.GetTimeStep(); if (this.mustConstraints[timeStep] == null) - this.mustConstraints[timeStep] = new Dictionary(); + this.mustConstraints[timeStep] = []; this.mustConstraints[timeStep][con.agentNum] = con.move; } } @@ -146,7 +142,7 @@ public virtual void Setup(ProblemInstance problemInstance, int minDepth, Run run root.backPropagationSet = new HashSet(); root.collisionSets = new DisjointSets(); - this.mstarBackPropagationConflictList = new List(); + this.mstarBackPropagationConflictList = []; } } @@ -182,9 +178,9 @@ public IHeuristicCalculator GetHeuristic() public virtual String GetName() { - if (this.mstar == false) + if (!this.mstar) return "A*"; - else if (this.doMstarShuffle == false) + else if (!this.doMstarShuffle) return "rM*"; else return "rM*+shuffle"; @@ -406,7 +402,7 @@ public virtual bool Solve() while (openList.Count > 0) { // Check if max time has been exceeded - if (runner.ElapsedMilliseconds() > Constants.MAX_TIME) + if (stopwatch.ElapsedMilliseconds > Constants.MAX_TIME) { totalCost = (int) Constants.SpecialCosts.TIMEOUT_COST; Console.WriteLine("Out of time"); @@ -453,11 +449,11 @@ public virtual bool Solve() // } //} - if (this.mstar == false && // Backpropagation can cause the root to be re-expanded after many more expensive nodes were expanded. - (this.openList is DynamicLazyOpenList) == false && // When the open list has just one node, + if (!this.mstar && // Backpropagation can cause the root to be re-expanded after many more expensive nodes were expanded. + this.openList is not DynamicLazyOpenList && // When the open list has just one node, // application of the expensive heuristic is skipped altogether. // This can cause decreasing F values. - (this.openList is DynamicRationalLazyOpenList) == false && + this.openList is not DynamicRationalLazyOpenList && currentNode.minGoalCost == -1 // If we were given a minGoalCost (by ID, for example), then at some point we're going to use up the boost to the h-value we gave the root ) if (currentNode.F < lastF) @@ -488,7 +484,7 @@ public virtual bool Solve() } // Expand - if (this.mstar == false) + if (!this.mstar) { //Expand(currentNode); } @@ -543,7 +539,7 @@ public virtual void Expand(WorldState node) for (int agentIndex = 0; agentIndex < this.instance.agents.Length ; ++agentIndex) { - if (runner.ElapsedMilliseconds() > Constants.MAX_TIME) + if (stopwatch.ElapsedMilliseconds > Constants.MAX_TIME) return; intermediateNodes = ExpandOneAgent(intermediateNodes, agentIndex); @@ -552,7 +548,7 @@ public virtual void Expand(WorldState node) foreach (var currentNode in finalGeneratedNodes) { - if (runner.ElapsedMilliseconds() > Constants.MAX_TIME) + if (stopwatch.ElapsedMilliseconds > Constants.MAX_TIME) return; currentNode.makespan++; currentNode.CalculateG(); @@ -629,7 +625,7 @@ protected virtual List ExpandOneAgent(List intermediateN foreach (var currentNode in intermediateNodes) { - if (runner.ElapsedMilliseconds() > Constants.MAX_TIME) + if (stopwatch.ElapsedMilliseconds > Constants.MAX_TIME) break; if (currentNode.mddNode == null) @@ -672,12 +668,12 @@ protected virtual List ExpandOneAgent(List intermediateN this.mustConstraints[makespan] != null && this.mustConstraints[makespan].ContainsKey(agentNum)) // This agent has a must constraint for this time step { - if (this.mustConstraints[makespan][agentNum].Equals(possibleMove) == false) + if (!this.mustConstraints[makespan][agentNum].Equals(possibleMove)) continue; } // Check if the tile is not free (out of the grid or with an obstacle) - if (this.instance.IsValid(possibleMove) == false) + if (!this.instance.IsValid(possibleMove)) continue; // Check against all the agents that have already moved to see if current move collides with their move @@ -687,14 +683,14 @@ protected virtual List ExpandOneAgent(List intermediateN { bool agentInCollisionSet = fromNode.collisionSets.IsSingle(agentIndex); - if (agentInCollisionSet == false) // Only one move allowed + if (!agentInCollisionSet) // Only one move allowed { bool hasPlan = true; if (hasPlan) { // If this move isn't its individually optimal one according to its planned route, return false. - if (this.instance.GetSingleAgentOptimalMove(fromNode.allAgentsState[agentIndex]).Equals(possibleMove) == false) + if (!this.instance.GetSingleAgentOptimalMove(fromNode.allAgentsState[agentIndex]).Equals(possibleMove)) continue; } } @@ -715,8 +711,7 @@ protected virtual List ExpandOneAgent(List intermediateN bool otherAgentInColSet = fromNode.collisionSets.IsSingle(collidingAgentIndex); // Check if one of the colliding agents isn't in the collision set yet - if (agentInCollisionSet == false || - otherAgentInColSet == false) + if (!agentInCollisionSet || !otherAgentInColSet) { if (this.debug) Debug.WriteLine("Agent planned route collides with another move!"); @@ -726,7 +721,7 @@ protected virtual List ExpandOneAgent(List intermediateN intermediateMode.allAgentsState[collidingAgentIndex].lastMove, makespan); if (this.debug) Debug.WriteLine(conflict.ToString()); - if (success == false) + if (!success) { this.mstarBackPropagationConflictList.Add(conflict); } @@ -788,7 +783,7 @@ protected virtual WorldState CreateSearchNode(WorldState from) /// /// Just an optimization /// - private CbsConstraint queryConstraint; + private readonly CbsConstraint queryConstraint = new() { queryInstance = true }; /// /// Check if the move is valid, i.e. not colliding into walls or other agents. @@ -830,12 +825,12 @@ protected virtual bool IsValid(TimedMove possibleMove, this.mustConstraints[makespan] != null && this.mustConstraints[makespan].ContainsKey(agentNum)) // This agent has a must constraint for this time step { - if (this.mustConstraints[makespan][agentNum].Equals(possibleMove) == false) + if (!this.mustConstraints[makespan][agentNum].Equals(possibleMove)) return false; } // Check if the tile is not free (out of the grid or with an obstacle) - if (this.instance.IsValid(possibleMove) == false) + if (!this.instance.IsValid(possibleMove)) return false; // Check against all the agents that have already moved to see if current move collides with their move @@ -848,7 +843,7 @@ protected virtual bool IsValid(TimedMove possibleMove, //(fromNode.individualMStarPlanBases[agentIndex] != null && // this.mstarPlanBasesToTheirPlans[fromNode.individualMStarPlanBases[agentIndex]][agentIndex] == null); // Parent plan was abandoned. Imagine a backpropagation happened. - if (agentInCollisionSet == false) // Only one move allowed + if (!agentInCollisionSet) // Only one move allowed { bool hasPlan = true; //// if the agent doesn't have a planned route, give it a planned route, @@ -884,7 +879,7 @@ protected virtual bool IsValid(TimedMove possibleMove, //Move allowed = plan.GetLocationAt(fromNode.individualMStarBookmarks[agentIndex] + 1); //if (possibleMove.Equals(allowed) == false) // return false; - if (this.instance.GetSingleAgentOptimalMove(fromNode.allAgentsState[agentIndex]).Equals(possibleMove) == false) + if (!this.instance.GetSingleAgentOptimalMove(fromNode.allAgentsState[agentIndex]).Equals(possibleMove)) return false; } } @@ -907,8 +902,8 @@ protected virtual bool IsValid(TimedMove possibleMove, //this.mstarPlanBasesToTheirPlans[fromNode.individualMStarPlanBases[collidingAgentIndex]][collidingAgentIndex] == null); // Parent plan was abandoned; // Check if one of the colliding agents isn't in the collision set yet - if (agentInCollisionSet == false || - otherAgentInColSet == false) + if (!agentInCollisionSet || + !otherAgentInColSet) { if (this.debug) Debug.WriteLine("Agent planned route collides with another move!"); @@ -962,7 +957,7 @@ protected virtual bool IsValid(TimedMove possibleMove, // Debug.WriteLine("Replanning Agent {0} for the same cost failed", collidingAgentIndex); // } //} - if (success == false) + if (!success) { this.mstarBackPropagationConflictList.Add(conflict); } @@ -975,7 +970,7 @@ protected virtual bool IsValid(TimedMove possibleMove, collision = possibleMove.IsColliding(currentMoves); } - return collision == false; + return !collision; } /// @@ -1006,21 +1001,15 @@ public virtual int[] GetSingleCosts() public int GetSolutionDepth() { return this.solutionDepth; } public long GetMemoryUsed() { return Process.GetCurrentProcess().VirtualMemorySize64; } - public void Setup(ProblemInstance problemInstance, Run runner) + public void Setup(ProblemInstance problemInstance, Stopwatch stopwatch) { - this.Setup(problemInstance, -1, runner); + this.Setup(problemInstance, -1, stopwatch); } /// /// For new ID groups /// - /// - /// - /// - /// - /// - /// - public void Setup(ProblemInstance problemInstance, Run runner, ConflictAvoidanceTable CAT, + public void Setup(ProblemInstance problemInstance, Stopwatch stopwatch, ConflictAvoidanceTable CAT, int parentGroup1Cost, int parentGroup2Cost, int parentGroup1Size) { // Use the solutions of previously solved subproblems as a lower bound @@ -1030,12 +1019,12 @@ public void Setup(ProblemInstance problemInstance, Run runner, ConflictAvoidance if (Constants.costFunction == Constants.CostFunction.SUM_OF_COSTS) { - this.Setup(problemInstance, -1, runner, CAT, + Setup(problemInstance, -1, stopwatch, CAT, minCost: parentGroup1Cost + parentGroup2Cost); } else if (Constants.costFunction == Constants.CostFunction.MAKESPAN || Constants.costFunction == Constants.CostFunction.MAKESPAN_THEN_SUM_OF_COSTS) { - this.Setup(problemInstance, -1, runner, CAT, + Setup(problemInstance, -1, stopwatch, CAT, minCost: Math.Max(parentGroup1Cost, parentGroup2Cost)); } else @@ -1047,16 +1036,11 @@ public void Setup(ProblemInstance problemInstance, Run runner, ConflictAvoidance /// /// For replanning ID groups to resolve a conflict /// - /// - /// - /// - /// /// - /// - public void Setup(ProblemInstance problemInstance, Run runner, ConflictAvoidanceTable CAT, + public void Setup(ProblemInstance problemInstance, Stopwatch stopwatch, ConflictAvoidanceTable CAT, int targetCost, ISet illegalMoves) { this.illegalMoves = illegalMoves; - this.Setup(problemInstance, -1, runner, CAT, minCost: targetCost, maxCost: targetCost); + Setup(problemInstance, -1, stopwatch, CAT, minCost: targetCost, maxCost: targetCost); } /// @@ -1076,9 +1060,9 @@ protected virtual bool ProcessGeneratedNode(WorldState currentNode) // Accumulating the conflicts count from parent to child // We're counting conflicts along the entire path, so the parent's conflicts count is added to the child's: currentNode.conflictCounts = new Dictionary(currentNode.prevStep.conflictCounts); - currentNode.conflictTimes = new Dictionary>(); + currentNode.conflictTimes = []; foreach (var kvp in currentNode.prevStep.conflictTimes) - currentNode.conflictTimes[kvp.Key] = new List(kvp.Value); + currentNode.conflictTimes[kvp.Key] = [.. kvp.Value]; currentNode.IncrementConflictCounts(this.CAT); // We're counting conflicts along the entire path, so the parent's conflicts count // is added to the child's. @@ -1088,8 +1072,10 @@ protected virtual bool ProcessGeneratedNode(WorldState currentNode) { //currentNode.currentCollisionSet = null; // Clear expansion data - currentNode.backPropagationSet = new HashSet(); - currentNode.backPropagationSet.Add(currentNode.prevStep); + currentNode.backPropagationSet = new HashSet + { + currentNode.prevStep + }; currentNode.collisionSets = new DisjointSets(); @@ -1200,7 +1186,7 @@ protected virtual bool ProcessGeneratedNode(WorldState currentNode) } } - if (wasInClosedList == false || removedFromClosedList) + if (!wasInClosedList || removedFromClosedList) { this.closedList.Add(currentNode, currentNode); this.generated++; // Reopened nodes are also recounted here. diff --git a/A_Star_MDDs.cs b/A_Star_MDDs.cs index fce1ff6..14c380f 100644 --- a/A_Star_MDDs.cs +++ b/A_Star_MDDs.cs @@ -12,24 +12,22 @@ namespace mapf; class A_Star_MDDs : IConflictReporting { MDD[] problem; - Dictionary closedList; - Run runner; - BinaryHeap openList; + readonly Dictionary closedList = []; + readonly Stopwatch stopwatch; + readonly BinaryHeap openList = new(); public int expanded; public int generated; public int conflictCount; - ConflictAvoidanceTable CAT; + readonly ConflictAvoidanceTable CAT; - public A_Star_MDDs(MDD[] problem, Run runner, ConflictAvoidanceTable CAT) + public A_Star_MDDs(MDD[] problem, Stopwatch stopwatch, ConflictAvoidanceTable CAT) { this.expanded = 0; this.generated = 0; A_Star_MDDs_Node root; this.problem = problem; - this.runner = runner; + this.stopwatch = stopwatch; this.CAT = CAT; - this.closedList = new Dictionary(); - this.openList = new BinaryHeap(); MDDNode[] sRoot = new MDDNode[problem.Length]; for (int i = 0; i < problem.Length; i++) { @@ -48,7 +46,7 @@ public SinglePlan[] Solve() while (openList.Count > 0) { - if (runner.ElapsedMilliseconds() > Constants.MAX_TIME) + if (stopwatch.ElapsedMilliseconds > Constants.MAX_TIME) { return null; } @@ -95,7 +93,7 @@ public void Expand(A_Star_MDDs_Node node) for (int mddIndex = 0; mddIndex < this.problem.Length && intermediateNodes.Count != 0; ++mddIndex) { - if (runner.ElapsedMilliseconds() > Constants.MAX_TIME) + if (stopwatch.ElapsedMilliseconds > Constants.MAX_TIME) return; intermediateNodes = ExpandOneAgent(intermediateNodes, mddIndex); @@ -110,9 +108,9 @@ public void Expand(A_Star_MDDs_Node node) // Accumulating the conflicts count from parent to child // We're counting conflicts along the entire path, so the parent's conflicts count is added to the child's: child.conflictCounts = new Dictionary(child.prev.conflictCounts); - child.conflictTimes = new Dictionary>(); + child.conflictTimes = []; foreach (var kvp in child.prev.conflictTimes) - child.conflictTimes[kvp.Key] = new List(kvp.Value); + child.conflictTimes[kvp.Key] = [.. kvp.Value]; child.IncrementConflictCounts(this.CAT); // We're counting conflicts along the entire path, so the parent's conflicts count // is added to the child's. @@ -164,7 +162,7 @@ public void Expand(A_Star_MDDs_Node node) protected List ExpandOneAgent(List intermediateNodes, int mddIndex) { - var generated = new List(); + List generated = []; // Expand the mdd node foreach (A_Star_MDDs_Node node in intermediateNodes) @@ -175,7 +173,7 @@ protected List ExpandOneAgent(List intermedi if (node.currentMoves != null && childMddNode.move.IsColliding(node.currentMoves)) // Can happen. We only prune partially, we don't build the full k-agent MDD. continue; - var childNode = new A_Star_MDDs_Node(node, mddIndex != node.allSteps.Length - 1); + A_Star_MDDs_Node childNode = new(node, mddIndex != node.allSteps.Length - 1); childNode.allSteps[mddIndex] = childMddNode; // Update target conflict count and prune nodes that can't get to the target conflict count @@ -208,19 +206,13 @@ protected List ExpandOneAgent(List intermedi /// /// /// Map each external agent to the number of conflicts with their path the solution has - public Dictionary GetExternalConflictCounts() - { - return this.conflictCounts; - } + public Dictionary GetExternalConflictCounts() => this.conflictCounts; /// /// /// Map each external agent to a list of times the solution has a conflict with theirs - public Dictionary> GetConflictTimes() - { - return this.conflictTimes; - } - + public Dictionary> GetConflictTimes() => this.conflictTimes; + public void Expand(A_Star_MDDs_Expander currentNode) { while (true) @@ -256,9 +248,9 @@ public void Expand(A_Star_MDDs_Expander currentNode) } } - public int GetGenerated() { return this.generated; } + public int GetGenerated() => this.generated; - public int GetExpanded() { return this.expanded; } + public int GetExpanded() => this.expanded; private bool GoalTest(A_Star_MDDs_Node toCheck) { @@ -293,10 +285,7 @@ private SinglePlan[] GetAnswer(A_Star_MDDs_Node finish) return ans; } - private bool CheckIfLegal(MDDNode to1, MDDNode to2) - { - return to1.move.IsColliding(to2.move) == false; - } + private bool CheckIfLegal(MDDNode to1, MDDNode to2) => to1.move.IsColliding(to2.move) == false; private bool IsLegalMove(A_Star_MDDs_Node to) { @@ -383,20 +372,11 @@ public virtual void IncrementConflictCounts(ConflictAvoidanceTable CAT) /// Returns whether all possible f values were generated from this node already /// /// - public bool hasMoreChildren() - { - return this.targetDeltaConflictCount <= this.maxDeltaConflictCount; - } + public bool hasMoreChildren() => this.targetDeltaConflictCount <= this.maxDeltaConflictCount; - public bool IsAlreadyExpanded() - { - return alreadyExpanded; - } + public bool IsAlreadyExpanded() => alreadyExpanded; - public bool hasChildrenForCurrentDeltaConflictCount(int agentNum = 0) - { - return existsChildForConflictCount(agentNum, this.remainingDeltaConflictCount); - } + public bool hasChildrenForCurrentDeltaConflictCount(int agentNum = 0) => existsChildForConflictCount(agentNum, this.remainingDeltaConflictCount); /// /// An MDD child node was chosen between calculating the singleAgentDeltaConflictCounts and this call. diff --git a/A_Star_WithOD.cs b/A_Star_WithOD.cs index de45a92..6f0d60e 100644 --- a/A_Star_WithOD.cs +++ b/A_Star_WithOD.cs @@ -1,5 +1,6 @@ using System; using System.Collections.Generic; +using System.Diagnostics; using System.IO; namespace mapf; @@ -30,12 +31,12 @@ protected override WorldState CreateSearchNode(WorldState from) public override string GetName() { return base.GetName() + "+OD"; } - public override void Setup(ProblemInstance problemInstance, int minDepth, Run runner, + public override void Setup(ProblemInstance problemInstance, int minDepth, Stopwatch stopwatch, ConflictAvoidanceTable CAT = null, ISet constraints = null, ISet positiveConstraints = null, int minCost = -1, int maxCost = int.MaxValue, MDD mdd = null) { - base.Setup(problemInstance, minDepth, runner, CAT, constraints, positiveConstraints, + base.Setup(problemInstance, minDepth, stopwatch, CAT, constraints, positiveConstraints, minCost, maxCost, mdd); this.expandedFullStates = 0; this.generatedFullStates = 0; @@ -121,13 +122,7 @@ public override void OutputStatistics(TextWriter output) output.Write(this.generatedFullStates + Run.RESULTS_DELIMITER); } - public override int NumStatsColumns - { - get - { - return 2 + base.NumStatsColumns; - } - } + public override int NumStatsColumns => 2 + base.NumStatsColumns; public override void ClearAccumulatedStatistics() { diff --git a/CBS.cs b/CBS.cs index 8673f84..48a6854 100644 --- a/CBS.cs +++ b/CBS.cs @@ -95,7 +95,7 @@ public class CBS : ICbsSolver, IHeuristicSolver, IIndependenceDetection /// Notice root.g != 0 in CBS. /// private int _solutionDepth; - public Run _runner; // TODO: remove this dependency + public Stopwatch _stopwatch; private CbsNode _goalNode; private Plan _solution; @@ -228,10 +228,10 @@ public CBS(ICbsSolver singleAgentSolver, ICbsSolver generalSolver, /// /// Implements IIndependenceDetection.Setup for new groups /// - public void Setup(ProblemInstance problemInstance, Run runner, ConflictAvoidanceTable CAT, + public void Setup(ProblemInstance problemInstance, Stopwatch stopwatch, ConflictAvoidanceTable CAT, int parentGroup1Cost, int parentGroup2Cost, int parentGroup1Size) { - Setup(problemInstance, -1, runner, CAT, null, null, parentGroup1Cost + parentGroup2Cost); // TODO: Support a makespan cost function + Setup(problemInstance, -1, stopwatch, CAT, null, null, parentGroup1Cost + parentGroup2Cost); // TODO: Support a makespan cost function // I think we don't want to merge the agents in each group. We'd be left with two large meta-agents, // and in CBS one conflict isn't a reason to merge agents. The groups are arbitrarily large so we can't // even increment their conflict counts toward the merge threshold. @@ -240,7 +240,7 @@ public void Setup(ProblemInstance problemInstance, Run runner, ConflictAvoidance /// /// Implements IIndependenceDetection.Setup for replanning groups to resolve a conflict /// - public void Setup(ProblemInstance problemInstance, Run runner, ConflictAvoidanceTable CAT, + public void Setup(ProblemInstance problemInstance, Stopwatch stopwatch, ConflictAvoidanceTable CAT, int targetCost, ISet illegalMoves) { // Turn the set of reserved/illegal moves into constraints for every agent. Not the prettiest solution. @@ -255,7 +255,7 @@ public void Setup(ProblemInstance problemInstance, Run runner, ConflictAvoidance constraints.Add(new CbsConstraint(agentState.agent.agentNum, illegalMove)); } } - Setup(problemInstance, illegalMoves.Max(move => move.Time), runner, CAT, constraints, null, targetCost, targetCost); + Setup(problemInstance, illegalMoves.Max(move => move.Time), stopwatch, CAT, constraints, null, targetCost, targetCost); } /// @@ -267,15 +267,15 @@ public void Setup(ProblemInstance problemInstance, Run runner, ConflictAvoidance /// /// /// Currently ignored. FIXME: Need to convert to array of MDDs to use. - public virtual void Setup(ProblemInstance problemInstance, int minSolutionTimeStep, Run runner, + public virtual void Setup(ProblemInstance problemInstance, int minSolutionTimeStep, Stopwatch stopwatch, ConflictAvoidanceTable externalCAT, ISet externalConstraints, ISet externalPositiveConstraints, int minSolutionCost = -1, int maxSolutionCost = int.MaxValue, MDD mdd = null) { _instance = problemInstance; - _runner = runner; + _stopwatch = stopwatch; if (OpenList is DynamicLazyOpenList list) - list.runner = runner; + list.stopwatch = stopwatch; ClearPrivateStatistics(); SolutionCost = 0; @@ -345,7 +345,7 @@ public virtual void Setup(ProblemInstance problemInstance, int minSolutionTimeSt /// /// /// - public virtual void Setup(ProblemInstance problemInstance, Run runner) => Setup(problemInstance, 0, runner, null, null, null); + public virtual void Setup(ProblemInstance problemInstance, Stopwatch stopwatch) => Setup(problemInstance, 0, stopwatch, null, null, null); public IHeuristicCalculator GetHeuristic() => _heuristic; @@ -755,7 +755,7 @@ public bool Solve() while (OpenList.Count > 0) { // Check if max time has been exceeded - if (_runner.ElapsedMilliseconds() > Constants.MAX_TIME) + if (_stopwatch.ElapsedMilliseconds > Constants.MAX_TIME) { SolutionCost = (int) Constants.SpecialCosts.TIMEOUT_COST; Console.WriteLine("Out of time"); @@ -849,7 +849,7 @@ public bool Solve() // We're looking at _generated_ low level nodes since that's an indication to the amount of work done, // while expanded nodes is an indication of the amount of good work done. (MilliCap != int.MaxValue && // (This check is much cheaper than the method call) - _runner.ElapsedMilliseconds() > MilliCap)) // Search is taking too long. + _stopwatch.ElapsedMilliseconds > MilliCap)) // Search is taking too long. { Debug.WriteLine("-----------------"); SolutionCost = maxExpandedNodeF; // This is the min possible cost so far. @@ -871,7 +871,7 @@ public bool Solve() } // Check if max time has been exceeded - if (_runner.ElapsedMilliseconds() > Constants.MAX_TIME) + if (_stopwatch.ElapsedMilliseconds > Constants.MAX_TIME) { SolutionCost = (int)Constants.SpecialCosts.TIMEOUT_COST; Console.WriteLine("Out of time"); @@ -989,7 +989,7 @@ public virtual void Reset() leftSameCost = closedListHitChildCost == node.G; } - if (_runner.ElapsedMilliseconds() > Constants.MAX_TIME) + if (_stopwatch.ElapsedMilliseconds > Constants.MAX_TIME) return (adopted: false, children, reinsertParent); // Generate right child: @@ -1077,7 +1077,7 @@ private void ExpandIgnoringCardinalsButSupportingBP2(CbsNode node) break; } - if (_runner.ElapsedMilliseconds() > Constants.MAX_TIME) + if (_stopwatch.ElapsedMilliseconds > Constants.MAX_TIME) return; CbsNode lookAheadNode = lookAheadOpenList.Remove(); @@ -1200,7 +1200,7 @@ private void ExpandIgnoringCardinalsButSupportingBP2(CbsNode node) CbsNode lookAheadNode = lookAheadOpenList.Remove(); lookAheadNode.ChooseConflict(); - if (_runner.ElapsedMilliseconds() > Constants.MAX_TIME) + if (_stopwatch.ElapsedMilliseconds > Constants.MAX_TIME) return; Debug.WriteLine($"Looking ahead from node hash: {lookAheadNode.GetHashCode()}."); @@ -1848,10 +1848,10 @@ public MACBS_WholeTreeThreshold(ICbsSolver singleAgentSolver, ICbsSolver general /// /// /// - public override void Setup(ProblemInstance problemInstance, Run runner) + public override void Setup(ProblemInstance problemInstance, Stopwatch stopwatch) { MakeConflictMatrix(problemInstance); - base.Setup(problemInstance, runner); + base.Setup(problemInstance, stopwatch); } private void MakeConflictMatrix(ProblemInstance problemInstance) diff --git a/CbsHeuristicForAStar.cs b/CbsHeuristicForAStar.cs index 0eb39ff..eb64cbd 100644 --- a/CbsHeuristicForAStar.cs +++ b/CbsHeuristicForAStar.cs @@ -16,7 +16,7 @@ class CbsHeuristicForAStar : IHeuristicCalculator protected CBS cbs; protected ProblemInstance instance; protected List agentsToConsider; - protected Run runner; + protected Stopwatch stopwatch; protected bool validate; protected bool reportSolution; @@ -43,11 +43,11 @@ class CbsHeuristicForAStar : IHeuristicCalculator /// Larger values would cause each call to the heuristic to take longer, but make it return better estimates. /// /// - public CbsHeuristicForAStar(CBS cbs, Run runner, bool reportSolution = false, + public CbsHeuristicForAStar(CBS cbs, Stopwatch stopwatch, bool reportSolution = false, int minAboveSic = 1, bool validate = false) { this.cbs = cbs; - this.runner = runner; + this.stopwatch = stopwatch; this.reportSolution = reportSolution; this.minAboveSic = Math.Max(minAboveSic, 1); @@ -94,7 +94,7 @@ public uint h(WorldState s) protected uint h(WorldState s, int targetCost, int sicEstimate=-1, int lowLevelGeneratedCap=-1, int milliCap=int.MaxValue, bool resume=false) { - double start = this.runner.ElapsedMilliseconds(); + double start = this.stopwatch.ElapsedMilliseconds; ProblemInstance sAsProblemInstance; if (resume == false) @@ -106,7 +106,7 @@ protected uint h(WorldState s, int targetCost, int sicEstimate=-1, int lowLevelG Math.Max(s.makespan, // This forces must-constraints to be upheld when dealing with A*+OD nodes, // at the cost of forcing every agent to move when a goal could be found earlier with all must constraints upheld. s.minGoalTimeStep), // No point in finding shallower goal nodes - this.runner, null, null, positiveConstraints); + this.stopwatch, null, null, positiveConstraints); if (this.cbs.OpenList.Count > 0 && this.cbs.ExternalCAT == null) { @@ -155,7 +155,7 @@ protected uint h(WorldState s, int targetCost, int sicEstimate=-1, int lowLevelG this.nodesSolved++; } - double end = this.runner.ElapsedMilliseconds(); + double end = this.stopwatch.ElapsedMilliseconds; this.totalRuntime += end - start; this.nCalls++; @@ -200,7 +200,7 @@ protected uint h(WorldState s, int targetCost, int sicEstimate=-1, int lowLevelG heuristic.Init(this.instance, this.agentsToConsider); var epeastarsic = new EPEA_Star(heuristic); - epeastarsic.Setup(sAsProblemInstance, s.makespan, runner); + epeastarsic.Setup(sAsProblemInstance, s.makespan, stopwatch); bool epeastarsicSolved = epeastarsic.Solve(); if (epeastarsicSolved) Trace.Assert(epeastarsic.totalCost - s.G >= this.cbs.SolutionCost - s.G, "Inadmissible!!"); @@ -379,16 +379,12 @@ public string GetName() class DyanamicLazyCbsHeuristicForAStar : CbsHeuristicForAStar, IBoundedLazyHeuristic { - public DyanamicLazyCbsHeuristicForAStar(CBS cbs, Run runner, bool reportSolution = false, bool validate = false) - : base(cbs, runner, reportSolution, -1, validate) {} + public DyanamicLazyCbsHeuristicForAStar(CBS cbs, Stopwatch stopwatch, bool reportSolution = false, bool validate = false) + : base(cbs, stopwatch, reportSolution, -1, validate) {} /// /// Assumes g of node was already calculated. /// - /// - /// - /// - /// public uint h(WorldState s, int targetH, float effectiveBranchingFactor) { // No need to check if SIC is zero because this heuristic is run after SIC was already computed, not instead of it. diff --git a/CbsNode.cs b/CbsNode.cs index f6cbe02..accc7ca 100644 --- a/CbsNode.cs +++ b/CbsNode.cs @@ -179,10 +179,10 @@ public CbsNode(CbsNode parent, CbsConstraint newConstraint, int agentToReplan) newConstraint.move.Direction != Direction.NO_DIRECTION))) { // We have an MDD and same cost can still be achieved - adapt the existing MDD - double startTime = CBS._runner.ElapsedMilliseconds(); + double startTime = CBS._stopwatch.ElapsedMilliseconds; _mdds[agentToReplan] = new MDD(_mdds[agentToReplan], newConstraint); MDDNarrownessValues[agentToReplan] = _mdds[agentToReplan].getLevelNarrownessValues(); - double endTime = CBS._runner.ElapsedMilliseconds(); + double endTime = CBS._stopwatch.ElapsedMilliseconds; CBS.MDDsAdapted++; CBS.TimeBuildingMdds += endTime - startTime; } @@ -545,11 +545,11 @@ public bool Replan(int agentToReplan, int minPathTimeStep, if (CBS.ReplanSameCostWithMdd) mdd = _mdds[agentToReplan]; - double startTime = CBS._runner.ElapsedMilliseconds(); - relevantSolver.Setup(subProblem, minPathTimeStep, CBS._runner, CAT, constraints, positiveConstraints, + double startTime = CBS._stopwatch.ElapsedMilliseconds; + relevantSolver.Setup(subProblem, minPathTimeStep, CBS._stopwatch, CAT, constraints, positiveConstraints, minPathCost, maxPathCost, mdd); bool solved = relevantSolver.Solve(); - double endTime = CBS._runner.ElapsedMilliseconds(); + double endTime = CBS._stopwatch.ElapsedMilliseconds; CBS.TimePlanningPaths += endTime - startTime; relevantSolver.AccumulateStatistics(); @@ -2079,15 +2079,15 @@ private bool CopyAppropriateMddFromParent(int agentIndex) if (nodeToGiveAnMdd._constraint != null && AgentNumToIndex[nodeToGiveAnMdd._constraint.agentNum] == agentIndex) { - double startTime = CBS._runner.ElapsedMilliseconds(); + double startTime = CBS._stopwatch.ElapsedMilliseconds; mdd = new MDD(mdd, nodeToGiveAnMdd._constraint); mddValues = mdd.getLevelNarrownessValues(); - double endTime = CBS._runner.ElapsedMilliseconds(); + double endTime = CBS._stopwatch.ElapsedMilliseconds; CBS.TimeBuildingMdds += endTime - startTime; CBS.MDDsAdapted++; if (CBS.CacheMdds) { - CbsCacheEntry entry = new CbsCacheEntry(nodeToGiveAnMdd, agentIndex); + CbsCacheEntry entry = new(nodeToGiveAnMdd, agentIndex); CBS.MDDCache[agentIndex][entry] = mdd; CBS.MDDNarrownessValuesCache[agentIndex][entry] = mddValues; } @@ -2112,7 +2112,7 @@ public bool buildMddForAgentWithItsCurrentCost(int agentIndex) if (MDDNarrownessValues[agentIndex] != null) // Already have an MDD with the current cost (they're nulled when the cost increases) return false; - if (CBS.CacheMdds == false || CBS.MDDCache[agentIndex].ContainsKey(new CbsCacheEntry(this, agentIndex)) == false) + if (!CBS.CacheMdds || !CBS.MDDCache[agentIndex].ContainsKey(new CbsCacheEntry(this, agentIndex))) { // Caching not enabled or no cache hit if (CopyAppropriateMddFromParent(agentIndex)) @@ -2170,18 +2170,18 @@ public bool buildMddForAgentWithItsCurrentCost(int agentIndex) Debug.WriteLine($"Building MDD for agent index {agentIndex} of cost {SingleAgentCosts[agentIndex]} and depth {depth}"); - double startTime = CBS._runner.ElapsedMilliseconds(); + double startTime = CBS._stopwatch.ElapsedMilliseconds; _mdds[agentIndex] = new MDD(agentIndex, problem.agents[agentIndex].agent.agentNum, problem.agents[agentIndex].GetMove(), SingleAgentCosts[agentIndex], depth, problem.GetNumOfAgents(), problem, ignoreConstraints: false, supportPruning: false, constraints: constraints, positiveConstraints: positiveConstraints); MDDNarrownessValues[agentIndex] = _mdds[agentIndex].getLevelNarrownessValues(); - double endTime = CBS._runner.ElapsedMilliseconds(); + double endTime = CBS._stopwatch.ElapsedMilliseconds; CBS.TimeBuildingMdds += endTime - startTime; if (CBS.CacheMdds) { - CbsCacheEntry entry = new CbsCacheEntry(this, agentIndex); + CbsCacheEntry entry = new(this, agentIndex); CBS.MDDCache[agentIndex][entry] = _mdds[agentIndex]; CBS.MDDNarrownessValuesCache[agentIndex][entry] = MDDNarrownessValues[agentIndex]; } @@ -2190,7 +2190,7 @@ public bool buildMddForAgentWithItsCurrentCost(int agentIndex) else { // The MDD is in the cache! - CbsCacheEntry entry = new CbsCacheEntry(this, agentIndex); + CbsCacheEntry entry = new(this, agentIndex); _mdds[agentIndex] = CBS.MDDCache[agentIndex][entry]; MDDNarrownessValues[agentIndex] = CBS.MDDNarrownessValuesCache[agentIndex][entry]; CBS.MDDCacheHits++; @@ -2994,11 +2994,11 @@ public bool Replan3b(int agentToReplan, int depthToReplan, int minPathCost = -1, MDD mdd = null; if (CBS.ReplanSameCostWithMdd) mdd = _mdds[agentToReplan]; - double startTime = CBS._runner.ElapsedMilliseconds(); - relevantSolver.Setup(subProblem, depthToReplan, CBS._runner, CAT, constraints, positiveConstraints, + double startTime = CBS._stopwatch.ElapsedMilliseconds; + relevantSolver.Setup(subProblem, depthToReplan, CBS._stopwatch, CAT, constraints, positiveConstraints, minPathCost, maxPathCost, mdd); bool solved = relevantSolver.Solve(); - double endTime = CBS._runner.ElapsedMilliseconds(); + double endTime = CBS._stopwatch.ElapsedMilliseconds; CBS.TimePlanningPaths += endTime - startTime; relevantSolver.AccumulateStatistics(); diff --git a/CooperativeA_Star.cs b/CooperativeA_Star.cs index b16f5c3..160b662 100644 --- a/CooperativeA_Star.cs +++ b/CooperativeA_Star.cs @@ -15,13 +15,13 @@ class CooperativeAStar : IStatisticsCsvWriter, ISolver /// /// The Reservation Table /// - HashSet reservationTable; + HashSet reservationTable = []; AgentState[] allAgentsState; /// /// Maps locations (moves) to the time an agent parked there. From that point on they're /// blocked. /// - Dictionary parked; + Dictionary parked = []; int[] pathCosts; SinglePlan[] paths; int maxPathCostSoFar; @@ -29,19 +29,12 @@ class CooperativeAStar : IStatisticsCsvWriter, ISolver public int generated; public int totalcost; private ProblemInstance problem; - private Run runner; + private Stopwatch stopwatch; private int initialEstimate; - public CooperativeAStar() - { - reservationTable = new HashSet(); - parked = new Dictionary(); - } + public CooperativeAStar() {} - public string GetName() - { - return "CA*"; - } + public string GetName() => "CA*"; public void Clear() { @@ -60,22 +53,13 @@ public void ClearStatistics() this.generated = 0; } - public int GetExpanded() - { - return this.expanded; - } + public int GetExpanded() => this.expanded; - public int GetGenerated() - { - return this.generated; - } + public int GetGenerated() => this.generated; - public long GetMemoryUsed() - { - return Process.GetCurrentProcess().VirtualMemorySize64; - } + public long GetMemoryUsed() => Process.GetCurrentProcess().VirtualMemorySize64; - public void Setup(ProblemInstance instance, Run runner) + public void Setup(ProblemInstance instance, Stopwatch stopwatch) { this.Clear(); this.ClearStatistics(); @@ -83,12 +67,12 @@ public void Setup(ProblemInstance instance, Run runner) this.allAgentsState = instance.agents; this.pathCosts = new int[this.allAgentsState.Length]; this.paths = new SinglePlan[this.allAgentsState.Length]; - this.runner = runner; + this.stopwatch = stopwatch; } - public Plan GetPlan() { return new Plan(this.paths.TakeWhile(plan => plan != null)); } + public Plan GetPlan() => new Plan(this.paths.TakeWhile(plan => plan != null)); - public int GetSolutionCost() { return this.totalcost; } + public int GetSolutionCost() => this.totalcost; public void OutputStatisticsHeader(TextWriter output) { @@ -96,15 +80,9 @@ public void OutputStatisticsHeader(TextWriter output) output.Write(this.ToString() + " generated" + Run.RESULTS_DELIMITER); } - public override string ToString() - { - return GetName(); - } + public override string ToString() => GetName(); - public int GetSolutionDepth() - { - return this.totalcost - this.initialEstimate; - } + public int GetSolutionDepth() => this.totalcost - this.initialEstimate; /// /// Prints statistics of a single run to the given output. @@ -117,13 +95,7 @@ public void OutputStatistics(TextWriter output) output.Write(this.generated + Run.RESULTS_DELIMITER); } - public int NumStatsColumns - { - get - { - return 2; - } - } + public int NumStatsColumns => 2; public bool Solve() { @@ -151,17 +123,17 @@ public bool AddOneAgent(int index) private bool singleAgentAStar(AgentState agent) { AgentState.EquivalenceOverDifferentTimes = false; - BinaryHeap openList = new BinaryHeap(); // TODO: Safe to use OpenList here instead? - HashSet closedList = new HashSet(); + BinaryHeap openList = new(); // TODO: Safe to use OpenList here instead? + HashSet closedList = []; agent.h = this.problem.GetSingleAgentOptimalCost(agent); openList.Add(agent); AgentState node; this.initialEstimate += agent.h; - TimedMove queryTimedMove = new TimedMove(); + TimedMove queryTimedMove = new(); while (openList.Count > 0) { - if (this.runner.ElapsedMilliseconds() > Constants.MAX_TIME) + if (this.stopwatch.ElapsedMilliseconds > Constants.MAX_TIME) { return false; } diff --git a/CostTreeNodeSolver.cs b/CostTreeNodeSolver.cs index 057016b..056b92a 100644 --- a/CostTreeNodeSolver.cs +++ b/CostTreeNodeSolver.cs @@ -1,4 +1,6 @@ -using System.Collections.Generic; +using System; +using System.Collections.Generic; +using System.Diagnostics; using System.Linq; namespace mapf; @@ -15,7 +17,7 @@ abstract class CostTreeNodeSolver : IConflictReporting public int totalCost; public int generated; public int expanded; - protected Run runner; + protected Stopwatch stopwatch; protected ProblemInstance problem; public CostTreeSearchSolver solver; @@ -23,9 +25,9 @@ abstract class CostTreeNodeSolver : IConflictReporting public int matchCounter; // For debugging - public CostTreeNodeSolver(ProblemInstance problem, Run runner, CostTreeSearchSolver solver) + public CostTreeNodeSolver(ProblemInstance problem, Stopwatch stopwatch, CostTreeSearchSolver solver) { - this.runner = runner; + this.stopwatch = stopwatch; this.problem = problem; this.solver = solver; this.allMDDs = new MDD[problem.GetNumOfAgents()]; @@ -40,10 +42,10 @@ public CostTreeNodeSolver(ProblemInstance problem, Run runner, CostTreeSearchSol /// /// Of all the agents, not just the ones selected /// - public CostTreeNodeSolver(ProblemInstance problem, Run runner, CostTreeSearchSolver solver, + public CostTreeNodeSolver(ProblemInstance problem, Stopwatch stopwatch, CostTreeSearchSolver solver, int[] agentNums, CostTreeNode costsNode, ISet reserved) { - this.runner = runner; + this.stopwatch = stopwatch; this.problem = problem; this.solver = solver; this.allMDDs = new MDD[agentNums.Length]; @@ -58,9 +60,9 @@ public CostTreeNodeSolver(ProblemInstance problem, Run runner, CostTreeSearchSol /// TODO: Maybe just pass the array of costs here? /// /// - public CostTreeNodeSolver(ProblemInstance problem, CostTreeNode costNode, Run runner, + public CostTreeNodeSolver(ProblemInstance problem, CostTreeNode costNode, Stopwatch stopwatch, CostTreeSearchSolver solver, ISet reserved) // Make sure agent numbers are in the correct order - : this(problem, runner, solver) + : this(problem, stopwatch, solver) { this.Setup(costNode, reserved); } @@ -140,9 +142,9 @@ public void Expand(Queue openList, HashSet closedLis { for (int j = 0; j < costs.Length; j++) { - int[] newCosts = this.costs.ToArray(); + int[] newCosts = [.. this.costs]; newCosts[j]++; - var child = new CostTreeNode(newCosts); + CostTreeNode child = new CostTreeNode(newCosts); if (!closedList.Contains(child)) { closedList.Add(child); @@ -192,11 +194,11 @@ class CostTreeNodeSolverOldMatching : CostTreeNodeSolver /// Currently the only supported values are 3 and non-3. This is equivalent to bool checkTriples. /// int syncSize; - public CostTreeNodeSolverOldMatching(ProblemInstance problem, Run runner, CostTreeSearchSolver solver) - : base(problem, runner, solver) { } - public CostTreeNodeSolverOldMatching(ProblemInstance problem, CostTreeNode costNode, Run runner, CostTreeSearchSolver solver, + public CostTreeNodeSolverOldMatching(ProblemInstance problem, Stopwatch stopwatch, CostTreeSearchSolver solver) + : base(problem, stopwatch, solver) { } + public CostTreeNodeSolverOldMatching(ProblemInstance problem, CostTreeNode costNode, Stopwatch stopwatch, CostTreeSearchSolver solver, int syncSize, ISet reserved) - : base(problem, costNode, runner, solver, reserved) { this.syncSize = syncSize; } + : base(problem, costNode, stopwatch, solver, reserved) { this.syncSize = syncSize; } public void Setup(CostTreeNode costNode, int syncSize, ISet reserved) { @@ -218,7 +220,7 @@ public override SinglePlan[] Solve(ConflictAvoidanceTable CAT) return null; this.solver.survivedPruningHL++; - A_Star_MDDs findSolution = new A_Star_MDDs(allMDDs, runner, CAT); + A_Star_MDDs findSolution = new(allMDDs, stopwatch, CAT); SinglePlan[] ans = findSolution.Solve(); this.generated = findSolution.generated; @@ -254,11 +256,11 @@ public bool Prune() class CostTreeNodeSolverDDBF : CostTreeNodeSolver { - public CostTreeNodeSolverDDBF(ProblemInstance problem, Run runner, CostTreeSearchSolver solver) - : base(problem, runner, solver) { } - public CostTreeNodeSolverDDBF(ProblemInstance problem, CostTreeNode costNode, Run runner, CostTreeSearchSolver solver, + public CostTreeNodeSolverDDBF(ProblemInstance problem, Stopwatch stopwatch, CostTreeSearchSolver solver) + : base(problem, stopwatch, solver) { } + public CostTreeNodeSolverDDBF(ProblemInstance problem, CostTreeNode costNode, Stopwatch stopwatch, CostTreeSearchSolver solver, ISet reserved) - : base(problem, costNode, runner, solver, reserved) { } + : base(problem, costNode, stopwatch, solver, reserved) { } public override void Setup(CostTreeNode costNode, ISet reserved) { base.Setup(costNode, reserved); @@ -268,7 +270,7 @@ public override SinglePlan[] Solve(ConflictAvoidanceTable CAT) for (int i = 0; i < allMDDs.Length; i++) if (allMDDs[i].levels == null) return null; - A_Star_MDDs findSolution = new A_Star_MDDs(allMDDs, runner, CAT); + A_Star_MDDs findSolution = new(allMDDs, stopwatch, CAT); SinglePlan[] ans = findSolution.Solve(); generated = findSolution.generated; expanded = findSolution.expanded; @@ -282,12 +284,12 @@ public override SinglePlan[] Solve(ConflictAvoidanceTable CAT) class CostTreeNodeSolverKSimpleMatching : CostTreeNodeSolver { int maxGroupChecked; - public CostTreeNodeSolverKSimpleMatching(ProblemInstance problem, Run runner, CostTreeSearchSolver solver) - : base(problem, runner, solver) { } + public CostTreeNodeSolverKSimpleMatching(ProblemInstance problem, Stopwatch stopwatch, CostTreeSearchSolver solver) + : base(problem, stopwatch, solver) { } public CostTreeNodeSolverKSimpleMatching(ProblemInstance problem, CostTreeNode costNode, - Run runner, CostTreeSearchSolver solver, int maxGroupChecked, + Stopwatch stopwatch, CostTreeSearchSolver solver, int maxGroupChecked, ISet reserved) - : base(problem, costNode, runner, solver, reserved) { this.maxGroupChecked = maxGroupChecked; } + : base(problem, costNode, stopwatch, solver, reserved) { this.maxGroupChecked = maxGroupChecked; } public void Setup(CostTreeNode costNode, int maxGroupChecked, ISet reserved) { base.Setup(costNode, reserved); @@ -298,7 +300,7 @@ public override SinglePlan[] Solve(ConflictAvoidanceTable CAT) A_Star_MDDs findSolution; SinglePlan[] subCheck; MDD[] match; - MddMatchAndPrune matcher = new MddMatchAndPrune(runner, this); + MddMatchAndPrune matcher = new(stopwatch, this); foreach (MDD checkValid in allMDDs) { @@ -318,7 +320,7 @@ public override SinglePlan[] Solve(ConflictAvoidanceTable CAT) //matcher.initialize(match); //if (matcher.pruneMDDs() == false) - findSolution = new A_Star_MDDs(match, runner, CAT); + findSolution = new A_Star_MDDs(match, stopwatch, CAT); subCheck = findSolution.Solve(); if (subCheck == null || subCheck[0] == null) @@ -343,7 +345,7 @@ public override SinglePlan[] Solve(ConflictAvoidanceTable CAT) //matcher.initialize(match); //if (matcher.pruneMDDs() == false) - findSolution = new A_Star_MDDs(match, runner, CAT); + findSolution = new A_Star_MDDs(match, stopwatch, CAT); subCheck = findSolution.Solve(); if (subCheck == null || subCheck[0] == null) @@ -372,7 +374,7 @@ public override SinglePlan[] Solve(ConflictAvoidanceTable CAT) //matcher.initialize(match); //if (matcher.pruneMDDs() == false) - findSolution = new A_Star_MDDs(match, runner, CAT); + findSolution = new A_Star_MDDs(match, stopwatch, CAT); subCheck = findSolution.Solve(); if (subCheck == null || subCheck[0] == null) @@ -389,7 +391,7 @@ public override SinglePlan[] Solve(ConflictAvoidanceTable CAT) this.solver.survivedPruningHL++; if (allMDDs[0].levels == null) return null; - findSolution = new A_Star_MDDs(allMDDs, runner, CAT); + findSolution = new A_Star_MDDs(allMDDs, stopwatch, CAT); SinglePlan[] ans = findSolution.Solve(); generated = findSolution.generated; expanded = findSolution.expanded; @@ -403,12 +405,12 @@ public override SinglePlan[] Solve(ConflictAvoidanceTable CAT) class CostTreeNodeSolverRepeatedMatching : CostTreeNodeSolver { int syncSize; - public CostTreeNodeSolverRepeatedMatching(ProblemInstance problem, Run runner, CostTreeSearchSolver solver) - : base(problem, runner, solver) { } + public CostTreeNodeSolverRepeatedMatching(ProblemInstance problem, Stopwatch stopwatch, CostTreeSearchSolver solver) + : base(problem, stopwatch, solver) { } public CostTreeNodeSolverRepeatedMatching(ProblemInstance problem, CostTreeNode costNode, - Run runner, CostTreeSearchSolver solver, int syncSize, + Stopwatch stopwatch, CostTreeSearchSolver solver, int syncSize, ISet reserved) - : base(problem, costNode, runner, solver, reserved) { this.syncSize = syncSize; } + : base(problem, costNode, stopwatch, solver, reserved) { this.syncSize = syncSize; } public void setup(CostTreeNode costNode, int syncSize, ISet reserved) { base.Setup(costNode, reserved); @@ -418,7 +420,7 @@ public override SinglePlan[] Solve(ConflictAvoidanceTable CAT) { MDD[] match = new MDD[2]; bool Converging = true; - int[] changed = new int[allMDDs.Length]; + Span changed = stackalloc int[allMDDs.Length]; int currentIteration = 0; MDD.PruningDone conflictStatus = MDD.PruningDone.NOTHING; @@ -468,7 +470,7 @@ public override SinglePlan[] Solve(ConflictAvoidanceTable CAT) this.solver.survivedPruningHL++; if (allMDDs[0].levels == null) return null; - A_Star_MDDs findSolution = new A_Star_MDDs(allMDDs, runner, CAT); + A_Star_MDDs findSolution = new(allMDDs, stopwatch, CAT); SinglePlan[] ans = findSolution.Solve(); generated = findSolution.generated; expanded = findSolution.expanded; diff --git a/CostTreeSearchSolver.cs b/CostTreeSearchSolver.cs index 1a09586..5deb97b 100644 --- a/CostTreeSearchSolver.cs +++ b/CostTreeSearchSolver.cs @@ -35,7 +35,7 @@ abstract class CostTreeSearchSolver : ICbsSolver, IIndependenceDetectionSolver int accSurvivedPruningHL; int accGoalTestSkipped; protected ProblemInstance problem; - protected Run runner; + protected Stopwatch stopwatch; public CostTreeNode costTreeNode; protected int costParentGroupA; protected int costParentGroupB; @@ -58,32 +58,16 @@ public CostTreeSearchSolver() /// Return the name of the solver, useful for outputting results. /// /// The name of the solver - public virtual String GetName() - { - return "CostTreeSearch+pairsMatch"; - } + public virtual String GetName() => "CostTreeSearch+pairsMatch"; - public override string ToString() - { - return GetName(); - } + public override string ToString() => GetName(); - public virtual void Setup(ProblemInstance problemInstance, Run runner) { - - - Setup(problemInstance, 0, runner, null); - } + public virtual void Setup(ProblemInstance problemInstance, Stopwatch stopwatch) => Setup(problemInstance, 0, stopwatch, null); /// /// For new groups under Independence Detection /// - /// - /// - /// - /// - /// - /// - public virtual void Setup(ProblemInstance problemInstance, Run runner, ConflictAvoidanceTable CAT, + public virtual void Setup(ProblemInstance problemInstance, Stopwatch stopwatch, ConflictAvoidanceTable CAT, int parentGroup1Cost, int parentGroup2Cost, int parentGroup1Size) { // Use the solutions of previously solved subproblems as a lower bound @@ -91,22 +75,17 @@ public virtual void Setup(ProblemInstance problemInstance, Run runner, ConflictA this.costParentGroupB = parentGroup2Cost; this.sizeParentGroupA = parentGroup1Size; - Setup(problemInstance, 0, runner, CAT, minCost: parentGroup1Cost + parentGroup2Cost, maxCost: int.MaxValue); // TODO: Support a makespan cost function + Setup(problemInstance, 0, stopwatch, CAT, minCost: parentGroup1Cost + parentGroup2Cost, maxCost: int.MaxValue); // TODO: Support a makespan cost function } /// /// For replanning groups to resolve a conflict under independence Detection /// - /// - /// - /// - /// /// - /// - public virtual void Setup(ProblemInstance problemInstance, Run runner, ConflictAvoidanceTable CAT, + public virtual void Setup(ProblemInstance problemInstance, Stopwatch stopwatch, ConflictAvoidanceTable CAT, int targetCost, ISet reserved) { this.reserved = reserved; - Setup(problemInstance, 0, runner, CAT, minCost: targetCost, maxCost: targetCost); + Setup(problemInstance, 0, stopwatch, CAT, minCost: targetCost, maxCost: targetCost); } /// @@ -121,7 +100,7 @@ public virtual void Setup(ProblemInstance problemInstance, Run runner, ConflictA /// /// /// FIXME: Not taken into account, just added to comply with ICbsSolver - public virtual void Setup(ProblemInstance problemInstance, int minTimeStep, Run runner, + public virtual void Setup(ProblemInstance problemInstance, int minTimeStep, Stopwatch stopwatch, ConflictAvoidanceTable CAT = null, ISet constraints = null, ISet positiveConstraints = null, int minCost = -1, int maxCost = int.MaxValue, MDD mdd = null) @@ -136,9 +115,9 @@ public virtual void Setup(ProblemInstance problemInstance, int minTimeStep, Run this.totalCost = (int) Constants.SpecialCosts.TIMEOUT_COST; this.problem = problemInstance; - this.runner = runner; + this.stopwatch = stopwatch; - closedList = new HashSet(); + closedList = []; openList = new Queue(); int[] costs = new int[problem.GetNumOfAgents()]; for (int i = 0; i < problem.GetNumOfAgents(); i++) @@ -177,10 +156,7 @@ public void Clear() /// /// Returns the cost of the solution found, or error codes otherwise. /// - public int GetSolutionCost() - { - return this.totalCost; - } + public int GetSolutionCost() => this.totalCost; protected Dictionary conflictCounts; protected Dictionary> conflictTimes; @@ -287,25 +263,22 @@ public void OutputAccumulatedStatistics(TextWriter output) /// public abstract bool Solve(); - public int GetHighLevelExpanded() { return this.expandedHL; } - public int GetHighLevelGenerated() { return this.generatedHL; } - public int GetLowLevelExpanded() { return this.expandedLL; } - public int GetLowLevelGenerated() { return this.generatedLL; } - public int GetExpanded() { return this.expandedHL; } - public int GetGenerated() { return this.generatedHL; } - public int GetAccumulatedExpanded() { return this.accExpandedHL; } - public int GetAccumulatedGenerated() { return this.accGeneratedHL; } - public int GetSolutionDepth() { return this.solutionDepth; } - public long GetMemoryUsed() { return Process.GetCurrentProcess().VirtualMemorySize64; } - public int GetMaxGroupSize() { return problem.agents.Length; } - public SinglePlan[] GetSinglePlans() { return solution; } - - public virtual int[] GetSingleCosts() - { - return costs; - } - - public abstract CostTreeNodeSolver CreateNodeSolver(ProblemInstance instance, Run runner); + public int GetHighLevelExpanded() => this.expandedHL; + public int GetHighLevelGenerated() => this.generatedHL; + public int GetLowLevelExpanded() => this.expandedLL; + public int GetLowLevelGenerated() => this.generatedLL; + public int GetExpanded() => this.expandedHL; + public int GetGenerated() => this.generatedHL; + public int GetAccumulatedExpanded() => this.accExpandedHL; + public int GetAccumulatedGenerated() => this.accGeneratedHL; + public int GetSolutionDepth() => this.solutionDepth; + public long GetMemoryUsed() => Process.GetCurrentProcess().VirtualMemorySize64; + public int GetMaxGroupSize() => problem.agents.Length; + public SinglePlan[] GetSinglePlans() => solution; + + public virtual int[] GetSingleCosts() => costs; + + public abstract CostTreeNodeSolver CreateNodeSolver(ProblemInstance instance, Stopwatch stopwatch); } class CostTreeSearchSolverOldMatching : CostTreeSearchSolver @@ -314,18 +287,18 @@ class CostTreeSearchSolverOldMatching : CostTreeSearchSolver public CostTreeSearchSolverOldMatching(int syncSize) : base() { this.syncSize = syncSize; } - public override CostTreeNodeSolver CreateNodeSolver(ProblemInstance instance, Run runner) + public override CostTreeNodeSolver CreateNodeSolver(ProblemInstance instance, Stopwatch stopwatch) { - return new CostTreeNodeSolverOldMatching(problem, runner, this); + return new CostTreeNodeSolverOldMatching(problem, stopwatch, this); } public override bool Solve() { - CostTreeNodeSolver nodeSolver = CreateNodeSolver(this.problem, this.runner); + CostTreeNodeSolver nodeSolver = CreateNodeSolver(this.problem, this.stopwatch); SinglePlan[] ans = null; //TODO if no solution found the algorithm will never stop - while (runner.ElapsedMilliseconds() < Constants.MAX_TIME) + while (stopwatch.ElapsedMilliseconds < Constants.MAX_TIME) { costTreeNode = openList.Peek(); int sumSubGroupA = costTreeNode.Sum(from: 0, to: this.sizeParentGroupA); @@ -414,19 +387,19 @@ public override String GetName() class CostTreeSearchSolverNoPruning : CostTreeSearchSolver { - public override CostTreeNodeSolver CreateNodeSolver(ProblemInstance instance, Run runner) + public override CostTreeNodeSolver CreateNodeSolver(ProblemInstance instance, Stopwatch stopwatch) { - return new CostTreeNodeSolverDDBF(problem, runner, this); + return new CostTreeNodeSolverDDBF(problem, stopwatch, this); } public override bool Solve() { - CostTreeNodeSolver next = CreateNodeSolver(this.problem, this.runner); + CostTreeNodeSolver next = CreateNodeSolver(this.problem, this.stopwatch); SinglePlan[] ans = null; int sumSubGroupA; int sumSubGroupB; //TODO if no solution found the algorithm will never stop - while (runner.ElapsedMilliseconds() < Constants.MAX_TIME) + while (stopwatch.ElapsedMilliseconds < Constants.MAX_TIME) { costTreeNode = openList.Peek(); sumSubGroupA = costTreeNode.Sum(0, sizeParentGroupA); @@ -504,32 +477,32 @@ class CostTreeSearchSolverKMatch : CostTreeSearchWithEdgesMatrix int maxGroupChecked; public CostTreeSearchSolverKMatch(int maxGroupChecked) : base() { this.maxGroupChecked = maxGroupChecked; } - public override void Setup(ProblemInstance problemInstance, Run runner) { - base.Setup(problemInstance, 0, runner); + public override void Setup(ProblemInstance problemInstance, Stopwatch stopwatch) { + base.Setup(problemInstance, 0, stopwatch); } - public override void Setup(ProblemInstance problemInstance, int minTimeStep, Run runner, + public override void Setup(ProblemInstance problemInstance, int minTimeStep, Stopwatch stopwatch, ConflictAvoidanceTable CAT, ISet constraints = null, ISet positiveConstraints = null, int minCost = -1, int maxCost = int.MaxValue, MDD mdd = null) { edgesMatrix = new int[problemInstance.agents.Length, problemInstance.GetMaxX() * problemInstance.GetMaxY() + problemInstance.GetMaxY(), Move.NUM_NON_DIAG_MOVES]; edgesMatrixCounter = 0; - base.Setup(problemInstance, minTimeStep, runner, CAT, constraints, positiveConstraints, minCost, maxCost, mdd); + base.Setup(problemInstance, minTimeStep, stopwatch, CAT, constraints, positiveConstraints, minCost, maxCost, mdd); } - public override CostTreeNodeSolver CreateNodeSolver(ProblemInstance instance, Run runner) + public override CostTreeNodeSolver CreateNodeSolver(ProblemInstance instance, Stopwatch stopwatch) { - return new CostTreeNodeSolverKSimpleMatching(problem, runner, this); + return new CostTreeNodeSolverKSimpleMatching(problem, stopwatch, this); } public override bool Solve() { - CostTreeNodeSolver next = CreateNodeSolver(problem, runner); + CostTreeNodeSolver next = CreateNodeSolver(problem, stopwatch); SinglePlan[] ans = null; int sumSubGroupA; int sumSubGroupB; //TODO if no solution found the algorithm will never stop - while (runner.ElapsedMilliseconds() < Constants.MAX_TIME) + while (stopwatch.ElapsedMilliseconds < Constants.MAX_TIME) { costTreeNode = openList.Peek(); sumSubGroupA = costTreeNode.Sum(0, sizeParentGroupA); @@ -599,32 +572,32 @@ class CostTreeSearchSolverRepeatedMatch : CostTreeSearchWithEdgesMatrix { int syncSize; public CostTreeSearchSolverRepeatedMatch(int syncSize) : base() { this.syncSize = syncSize; } - public override void Setup(ProblemInstance problemInstance, Run runner) { Setup(problemInstance, 0, runner); } - public override void Setup(ProblemInstance problemInstance, int minTimeStep, Run runner, + public override void Setup(ProblemInstance problemInstance, Stopwatch stopwatch) { Setup(problemInstance, 0, stopwatch); } + public override void Setup(ProblemInstance problemInstance, int minTimeStep, Stopwatch stopwatch, ConflictAvoidanceTable CAT = null, ISet constraints = null, ISet positiveConstraints = null, int minCost = -1, int maxCost = int.MaxValue, MDD mdd = null) { edgesMatrix = new int[problemInstance.agents.Length, problemInstance.GetMaxX() * problemInstance.GetMaxY() + problemInstance.GetMaxY(), Move.NUM_NON_DIAG_MOVES]; edgesMatrixCounter = 0; - base.Setup(problemInstance, minTimeStep, runner, CAT, constraints, positiveConstraints, minCost, maxCost, mdd); + base.Setup(problemInstance, minTimeStep, stopwatch, CAT, constraints, positiveConstraints, minCost, maxCost, mdd); } - public override CostTreeNodeSolver CreateNodeSolver(ProblemInstance instance, Run runner) + public override CostTreeNodeSolver CreateNodeSolver(ProblemInstance instance, Stopwatch stopwatch) { - return new CostTreeNodeSolverRepeatedMatching(problem, runner, this); + return new CostTreeNodeSolverRepeatedMatching(problem, stopwatch, this); } public override bool Solve() { //int time = 0; - CostTreeNodeSolver next = CreateNodeSolver(problem, runner); + CostTreeNodeSolver next = CreateNodeSolver(problem, stopwatch); SinglePlan[] ans = null; - Stopwatch sw = new Stopwatch(); + Stopwatch sw = new(); int sumSubGroupA; int sumSubGroupB; //TODO if no solution found the algorithm will never stop - while (runner.ElapsedMilliseconds() < Constants.MAX_TIME) + while (stopwatch.ElapsedMilliseconds < Constants.MAX_TIME) { sw.Reset(); costTreeNode = openList.Peek(); diff --git a/DynamicLazyOpenList.cs b/DynamicLazyOpenList.cs index 68843ce..a10bd55 100644 --- a/DynamicLazyOpenList.cs +++ b/DynamicLazyOpenList.cs @@ -1,6 +1,4 @@ using System; -using System.Collections.Generic; -using System.Linq; using System.IO; using System.Diagnostics; @@ -17,11 +15,11 @@ namespace mapf; public class DynamicLazyOpenList : OpenList where Item: IBinaryHeapItem, IHeuristicSearchNode { public ILazyHeuristic expensive; - public Run runner; + public Stopwatch stopwatch; protected int lastF; protected int nodesPushedBack; protected int accNodesPushedBack; - public bool debug; + private bool _debug; public DynamicLazyOpenList(ISolver user, ILazyHeuristic expensive) : base(user) @@ -29,7 +27,7 @@ public DynamicLazyOpenList(ISolver user, ILazyHeuristic expensive) this.expensive = expensive; this.ClearStatistics(); this.accNodesPushedBack = 0; - this.debug = false; + this._debug = false; } public override string GetName() @@ -61,7 +59,7 @@ public override Item Remove() if (node.GoalTest() == true || // Can't improve the h of the goal node.HBonus > 0 || // Already computed the expensive heuristic - this.runner.ElapsedMilliseconds() > Constants.MAX_TIME) // No time to continue improving H. + this.stopwatch.ElapsedMilliseconds > Constants.MAX_TIME) // No time to continue improving H. break; var next = base.Peek(); @@ -77,7 +75,7 @@ public override Item Remove() { this.Add(node); this.nodesPushedBack++; - if (this.debug) + if (this._debug) Debug.Print("Pushing back the node into the open list with an increased h."); } else diff --git a/DynamicRationalLazyOpenList.cs b/DynamicRationalLazyOpenList.cs index e9561e9..9e65292 100644 --- a/DynamicRationalLazyOpenList.cs +++ b/DynamicRationalLazyOpenList.cs @@ -8,7 +8,7 @@ namespace mapf; public class DynamicRationalLazyOpenList : OpenList { - public Run runner; + public Stopwatch stopwatch; public IBoundedLazyHeuristic expensive; protected int lastF; protected int skips; @@ -68,7 +68,7 @@ public override WorldState Remove() if (this.lastF != -1) { this.numExpands++; - double expandFinishTime = this.runner.ElapsedMilliseconds(); + double expandFinishTime = this.stopwatch.ElapsedMilliseconds; this.sumExpandTimes += expandFinishTime - this.expandStartTime; } @@ -91,7 +91,7 @@ public override WorldState Remove() node = base.Remove(); if (node.GoalTest() == true || // Can't improve the h of the goal - this.runner.ElapsedMilliseconds() > Constants.MAX_TIME) // No time to continue improving H. + this.stopwatch.ElapsedMilliseconds > Constants.MAX_TIME) // No time to continue improving H. { if (node.G + node.H < this.lastF) // This can happen if the last removed node had many runs of the expensive heuristic, which this node didn't yet have. { @@ -225,9 +225,9 @@ public override WorldState Remove() next = this.Peek(); int targetH = node.GetTargetH(next.F + 1); - double expensiveCallStartTime = this.runner.ElapsedMilliseconds(); + double expensiveCallStartTime = this.stopwatch.ElapsedMilliseconds; int expensiveEstimate = (int)this.expensive.h(node, targetH, -1, (int)(expensiveCallStartTime + millisCap), false); - double expensiveCallTotalTime = this.runner.ElapsedMilliseconds() - expensiveCallStartTime; + double expensiveCallTotalTime = this.stopwatch.ElapsedMilliseconds - expensiveCallStartTime; bool nodeSolved = node.GoalTest(); @@ -293,7 +293,7 @@ public override WorldState Remove() finish: this.lastF = node.G + node.H; - this.expandStartTime = this.runner.ElapsedMilliseconds(); + this.expandStartTime = this.stopwatch.ElapsedMilliseconds; return node; } diff --git a/EPEA_Star.cs b/EPEA_Star.cs index d4b1392..cf9f6e4 100644 --- a/EPEA_Star.cs +++ b/EPEA_Star.cs @@ -37,12 +37,12 @@ protected override WorldState CreateSearchNode(WorldState from) override public string GetName() { return "EPE" + base.GetName(); } - public override void Setup(ProblemInstance problemInstance, int minDepth, Run runner, + public override void Setup(ProblemInstance problemInstance, int minDepth, Stopwatch stopwatch, ConflictAvoidanceTable CAT = null, ISet constraints = null, ISet positiveConstraints = null, int minCost = -1, int maxCost = int.MaxValue, MDD mdd = null) { - base.Setup(problemInstance, minDepth, runner, CAT, constraints, positiveConstraints, + base.Setup(problemInstance, minDepth, stopwatch, CAT, constraints, positiveConstraints, minCost, maxCost, mdd); this.expandedFullStates = 0; } @@ -51,14 +51,11 @@ public override void Expand(WorldState nodeP) { var node = (WorldStateForPartialExpansion)nodeP; - bool wasAlreadyExpanded = true; - if (node.IsAlreadyExpanded() == false) { node.calcSingleAgentDeltaFs(instance, this.IsValid); expandedFullStates++; node.alreadyExpanded = true; - wasAlreadyExpanded = false; //node.hBonus = 0; // Locking any hbonus that doesn't come from partial expansion node.targetDeltaF = 0; // Assuming a consistent heuristic (as done in the paper), the min delta F is zero. node.remainingDeltaF = node.targetDeltaF; // Just for the following hasChildrenForCurrentDeltaF call. diff --git a/ISolver.cs b/ISolver.cs index 34be611..e135cca 100644 --- a/ISolver.cs +++ b/ISolver.cs @@ -1,4 +1,5 @@ using System.Collections.Generic; +using System.Diagnostics; namespace mapf; @@ -19,9 +20,7 @@ public interface ISolver : IStatisticsCsvWriter /// /// Setup the relevant data structures for a run. /// - /// - /// - void Setup(ProblemInstance problemInstance, Run runner); + void Setup(ProblemInstance problemInstance, Stopwatch stopwatch); /// /// Clears the relevant data structures and variables to free memory usage. @@ -31,7 +30,6 @@ public interface ISolver : IStatisticsCsvWriter /// /// Returns the found plan, or null if no plan was found. /// - /// Plan GetPlan(); /// @@ -68,24 +66,13 @@ public interface IIndependenceDetectionSolver : ISolver, IConflictReporting, IAc /// /// For new groups /// - /// - /// - /// - /// - /// - /// - void Setup(ProblemInstance problemInstance, Run runner, ConflictAvoidanceTable CAT, + void Setup(ProblemInstance problemInstance, Stopwatch stopwatch, ConflictAvoidanceTable CAT, int parentGroup1Cost, int parentGroup2Cost, int parentGroup1Size); /// /// For replanning groups to resolve a conflict /// - /// - /// - /// - /// /// - /// - void Setup(ProblemInstance problemInstance, Run runner, ConflictAvoidanceTable CAT, + void Setup(ProblemInstance problemInstance, Stopwatch stopwatch, ConflictAvoidanceTable CAT, int targetCost, ISet illegalMoves); int[] GetSingleCosts(); @@ -110,7 +97,7 @@ public interface ICbsSolver : ISolver, IConflictReporting, IAccumulatingStatisti /// /// If known, can speed up the search (no surplus nodes would be generated) /// Optional MDD of cost minCost=maxCost - void Setup(ProblemInstance problemInstance, int minTimeStep, Run runner, + void Setup(ProblemInstance problemInstance, int minTimeStep, Stopwatch stopwatch, ConflictAvoidanceTable CAT, ISet constraints, ISet positiveConstraints, int minCost, int maxCost, MDD mdd); SinglePlan[] GetSinglePlans(); diff --git a/IndependenceDetection.cs b/IndependenceDetection.cs index 9e7fead..371b859 100644 --- a/IndependenceDetection.cs +++ b/IndependenceDetection.cs @@ -39,7 +39,7 @@ class IndependenceDetection : ISolver protected int accMerges; public bool provideGroupCostsToSolver; public int totalCost; - protected Run runner; + protected Stopwatch stopwatch; /// /// The complete plan for all the agents that was found. @@ -87,10 +87,10 @@ public void Clear() this.maxSolutionCostFound = -1; } - public void Setup(ProblemInstance instance, Run runner) + public void Setup(ProblemInstance instance, Stopwatch stopwatch) { this.instance = instance; - this.runner = runner; + this.stopwatch = stopwatch; this.totalCost = 0; this.ClearStatistics(); this.conflictAvoidanceTable = new ConflictAvoidanceTable(); @@ -102,7 +102,7 @@ public void Setup(ProblemInstance instance, Run runner) foreach (AgentState agentStartState in instance.agents) { this.allGroups.AddLast(new IndependenceDetectionAgentsGroup( - this.instance, new AgentState[1] { agentStartState }, + this.instance,[ agentStartState ], this.singleAgentSolver, this.groupSolver, this) ); this.conflictAvoidanceTable.agentSizes[agentStartState.agent.agentNum] = 1; @@ -112,8 +112,8 @@ public void Setup(ProblemInstance instance, Run runner) conflictTimesPerGroup = new Dictionary>[instance.GetNumOfAgents()]; for (int i = 0; i < instance.GetNumOfAgents(); i++) { - conflictCountsPerGroup[i] = new Dictionary(); - conflictTimesPerGroup[i] = new Dictionary>(); + conflictCountsPerGroup[i] = []; + conflictTimesPerGroup[i] = []; } countsOfGroupsThatConflict = new int[instance.GetNumOfAgents()]; } @@ -626,7 +626,7 @@ public IndependenceDetectionConflict FindFirstConflict() /// /// /// - public bool SimpleID(Run runner) + public bool SimpleID(Stopwatch stopwatch) { while (true) { @@ -640,7 +640,7 @@ public bool SimpleID(Run runner) ++merges; // Solve composite group with the subsolver - bool solved = compositeGroup.Solve(runner, conflictAvoidanceTable); + bool solved = compositeGroup.Solve(stopwatch, conflictAvoidanceTable); if (solved == false) { this.totalCost = compositeGroup.solutionCost; @@ -660,7 +660,7 @@ public bool SimpleID(Run runner) /// /// /// - public bool ImprovedID(Run runner) + public bool ImprovedID(Stopwatch stopwatch) { while (true) { @@ -722,7 +722,7 @@ public bool ImprovedID(Run runner) //Debug.WriteLine($"Old plan:\n{conflict.group1.GetPlan()}"); } conflict.group1.removeGroupFromCAT(conflictAvoidanceTable); - bool resolved = conflict.group1.ReplanUnderConstraints(conflict.group2.GetPlan(), runner, this.conflictAvoidanceTable); + bool resolved = conflict.group1.ReplanUnderConstraints(conflict.group2.GetPlan(), stopwatch, this.conflictAvoidanceTable); ++resolutionAttempts; this.expanded += conflict.group1.expanded; this.generated += conflict.group1.generated; @@ -791,7 +791,7 @@ public bool ImprovedID(Run runner) //Debug.WriteLine($"Old plan: {conflict.group2.GetPlan()}"); } conflict.group2.removeGroupFromCAT(conflictAvoidanceTable); - bool resolved = conflict.group2.ReplanUnderConstraints(conflict.group1.GetPlan(), runner, this.conflictAvoidanceTable); + bool resolved = conflict.group2.ReplanUnderConstraints(conflict.group1.GetPlan(), stopwatch, this.conflictAvoidanceTable); ++resolutionAttempts; this.expanded += conflict.group2.expanded; this.generated += conflict.group2.generated; @@ -881,7 +881,7 @@ public bool ImprovedID(Run runner) ++merges; // Solve composite group with the underlying group solver - bool solved = compositeGroup.Solve(runner, conflictAvoidanceTable, + bool solved = compositeGroup.Solve(stopwatch, conflictAvoidanceTable, group1Cost, group2Cost, group1Size); if (compositeGroup.solutionCost > maxSolutionCostFound) @@ -988,7 +988,7 @@ public bool Solve() foreach (var group in this.allGroups) { - solved = group.Solve(runner, this.conflictAvoidanceTable); + solved = group.Solve(stopwatch, this.conflictAvoidanceTable); // Check if max time has been exceeded or search failed for another reason if (solved == false) @@ -1033,9 +1033,9 @@ public bool Solve() CountConflicts(); if (this.simple == false) - solved = this.ImprovedID(runner); + solved = this.ImprovedID(stopwatch); else - solved = this.SimpleID(runner); // TODO: it doesn't manage all the stats. Just add flags to ImprovedID instead + solved = this.SimpleID(stopwatch); // TODO: it doesn't manage all the stats. Just add flags to ImprovedID instead // Record found solution if (solved == true) { diff --git a/IndependenceDetectionAgentsGroup.cs b/IndependenceDetectionAgentsGroup.cs index 941d5fd..f84507a 100644 --- a/IndependenceDetectionAgentsGroup.cs +++ b/IndependenceDetectionAgentsGroup.cs @@ -43,24 +43,17 @@ public IndependenceDetectionAgentsGroup(ProblemInstance instance, AgentState[] a /// /// Solve the group of agents together. /// - /// - /// - /// - /// - /// - /// /// true if optimal solution for the group of agents were found, false otherwise - public bool Solve(Run runner, ConflictAvoidanceTable CAT, - int group1Cost = 0, int group2Cost = 0, int group1Size = 1 - ) + public bool Solve(Stopwatch stopwatch, ConflictAvoidanceTable CAT, + int group1Cost = 0, int group2Cost = 0, int group1Size = 1) { IIndependenceDetectionSolver relevantSolver = this.groupSolver; if (this.allAgentsState.Length == 1) relevantSolver = this.singleAgentSolver; // TODO: Consider using CBS's root trick to really get single agent paths fast. Though it won't respect illegal moves or avoid conflicts. if (this.id.provideGroupCostsToSolver) - relevantSolver.Setup(this.instance, runner, CAT, group1Cost, group2Cost, group1Size); + relevantSolver.Setup(this.instance, stopwatch, CAT, group1Cost, group2Cost, group1Size); else // For experiments only - relevantSolver.Setup(this.instance, runner, CAT, 0, 0, 0); + relevantSolver.Setup(this.instance, stopwatch, CAT, 0, 0, 0); bool solved = relevantSolver.Solve(); this.solutionCost = relevantSolver.GetSolutionCost(); if (solved == false) @@ -145,7 +138,7 @@ public override int GetHashCode() /// /// /// - public bool ReplanUnderConstraints(Plan planToAvoid, Run runner, ConflictAvoidanceTable CAT) + public bool ReplanUnderConstraints(Plan planToAvoid, Stopwatch stopwatch, ConflictAvoidanceTable CAT) { int oldCost = this.solutionCost; Plan oldPlan = this.plan; @@ -159,7 +152,7 @@ public bool ReplanUnderConstraints(Plan planToAvoid, Run runner, ConflictAvoidan IIndependenceDetectionSolver relevantSolver = this.groupSolver; if (this.allAgentsState.Length == 1) relevantSolver = this.singleAgentSolver; - relevantSolver.Setup(this.instance, runner, CAT, oldCost, reserved); + relevantSolver.Setup(this.instance, stopwatch, CAT, oldCost, reserved); bool solved = relevantSolver.Solve(); this.solutionCost = relevantSolver.GetSolutionCost(); @@ -198,7 +191,7 @@ public void addGroupToCAT(ConflictAvoidanceTable CAT) for (int i = 0; i < this.allAgentsState.Length; i++) { - var singleAgentPlan = new SinglePlan(this.plan, i, this.groupNum); // Note all the plans are inserted under the group's identifier + SinglePlan singleAgentPlan = new(this.plan, i, this.groupNum); // Note all the plans are inserted under the group's identifier CAT.AddPlan(singleAgentPlan); } } @@ -210,7 +203,7 @@ public void removeGroupFromCAT(ConflictAvoidanceTable CAT) for (int i = 0; i < this.allAgentsState.Length; i++) { - var singleAgentPlan = new SinglePlan(this.plan, i, this.groupNum); + SinglePlan singleAgentPlan = new(this.plan, i, this.groupNum); CAT.RemovePlan(singleAgentPlan); } } diff --git a/MddMatchAndPrune.cs b/MddMatchAndPrune.cs index 353511d..6d7c072 100644 --- a/MddMatchAndPrune.cs +++ b/MddMatchAndPrune.cs @@ -11,24 +11,21 @@ namespace mapf; class MddMatchAndPrune { MDD[] allMDDs; - Queue openList; - Dictionary closedList; + readonly Queue openList = new(); + readonly Dictionary closedList = []; int solutionDepth; // The depth is the cost + 1 because the root also counts as a level MddMatchAndPruneState goal; // This will contain the goal node if such was found bool legal; //indicates if all single MDDs are legal - public bool[] conflicted; //indicates if the matching process found any illegal nodes/edges and pruned any of the MDDs - Run runner; - CostTreeNodeSolver nodeSolver; + public bool[] conflicted = new bool[4]; //indicates if the matching process found any illegal nodes/edges and pruned any of the MDDs + readonly Stopwatch stopwatch; + readonly CostTreeNodeSolver nodeSolver; /// /// constructor /// - public MddMatchAndPrune(Run runner, CostTreeNodeSolver nodeSolver) + public MddMatchAndPrune(Stopwatch stopwatch, CostTreeNodeSolver nodeSolver) { - this.openList = new Queue(); - this.closedList = new Dictionary(); - conflicted = new bool[4]; - this.runner = runner; + this.stopwatch = stopwatch; this.nodeSolver = nodeSolver; } @@ -61,7 +58,7 @@ public void initialize(MDD[] allMDDs) private bool buildGeneralMDD() { MddMatchAndPruneState current = openList.Dequeue(); - successorIterator allChildren = new successorIterator(allMDDs.Length); + successorIterator allChildren = new(allMDDs.Length); int currentLevel = current.stateLevel; while (current.stateLevel + 1 != this.solutionDepth) // while not goal @@ -75,7 +72,7 @@ private bool buildGeneralMDD() closedList.Clear(); currentLevel++; } - if (runner.ElapsedMilliseconds() > Constants.MAX_TIME) + if (stopwatch.ElapsedMilliseconds > Constants.MAX_TIME) return false; } return true; @@ -189,7 +186,7 @@ public bool pruneMDDs() while (current.stateLevel > 0) // while not root { - if (runner.ElapsedMilliseconds() > Constants.MAX_TIME) + if (stopwatch.ElapsedMilliseconds > Constants.MAX_TIME) return false; if (current.stateLevel < currentLevel) diff --git a/PEA_Star.cs b/PEA_Star.cs index bbc8520..9586cba 100644 --- a/PEA_Star.cs +++ b/PEA_Star.cs @@ -2,6 +2,7 @@ using System.Linq; using System.IO; using System; +using System.Diagnostics; namespace mapf; @@ -37,11 +38,11 @@ protected override WorldState CreateSearchNode(WorldState from) return new WorldStateForPartialExpansion((WorldStateForPartialExpansion)from); } - public override void Setup(ProblemInstance problemInstance, int minTimeStep, Run runner, + public override void Setup(ProblemInstance problemInstance, int minTimeStep, Stopwatch stopwatch, ConflictAvoidanceTable CAT, ISet constraints, ISet positiveConstraints, int minCost, int maxCost, MDD mdd) { - base.Setup(problemInstance, minTimeStep, runner, CAT, constraints, positiveConstraints, minCost, maxCost, mdd); + base.Setup(problemInstance, minTimeStep, stopwatch, CAT, constraints, positiveConstraints, minCost, maxCost, mdd); this.generatedAndDiscarded = 0; this.expandedFullStates = 0; } diff --git a/Program.cs b/Program.cs index 28deca4..b6216c5 100644 --- a/Program.cs +++ b/Program.cs @@ -356,8 +356,8 @@ static void Main(string[] args) Console.WriteLine(e.StackTrace); return; } - Run runner = new Run(); // instantiates stuff unnecessarily - runner.startTime = runner.ElapsedMillisecondsTotal(); + Run runner = new(); // instantiates stuff unnecessarily + runner.watch.Restart(); IHeuristicCalculator lowLevelHeuristic = new SumIndividualCosts(); List agentList = Enumerable.Range(0, instance.agents.Length).Select(x=> (uint)x).ToList(); // FIXME: Must the heuristics really receive a list of uints? @@ -376,7 +376,7 @@ static void Main(string[] args) //ISolver solver = new IndependenceDetection(lowLevel, new EPEA_Star(lowLevelHeuristic)); //ISolver solver = new IndependenceDetection(lowLevel, new CostTreeSearchSolverOldMatching(3)); ISolver solver = new IndependenceDetection(lowLevel, new A_Star_WithOD(lowLevelHeuristic)); - solver.Setup(instance, runner); + solver.Setup(instance, runner.watch); bool solved = solver.Solve(); if (solved == false) { diff --git a/Run.cs b/Run.cs index 1a33199..697d0e5 100644 --- a/Run.cs +++ b/Run.cs @@ -11,10 +11,6 @@ namespace mapf; /// public class Run : IDisposable { - ////////debug - // public static TextWriter resultsWriterdd; - ///////////// - /// /// Delimiter character used when writing the results of the runs to the output file. /// @@ -45,7 +41,9 @@ public enum CsvSolutionCodes /// EH: I introduced this variable so that debugging and experiments /// can have deterministic results. /// - static public Random rand = new Random(); + static public Random rand = new(); + + public readonly Stopwatch watch = new(); /// /// Calls resultsWriter.Dispose() @@ -105,8 +103,6 @@ public void CloseResultsFile() /// public Run() { - this.watch = Stopwatch.StartNew(); - // Preparing the heuristics: astar_heuristics = new List>(); IHeuristicCalculator simple = null; @@ -933,10 +929,9 @@ public void SolveGivenProblemIncrementally(ProblemInstance instance) // Preparing a list of agent indices (not agent nums) for the heuristics' Init() method List agentList = Enumerable.Range(0, instance.agents.Length).Select(x => (uint)x).ToList(); // FIXME: Must the heuristics really receive a list of uints? - CooperativeAStar cooperativeAStar = new CooperativeAStar(); - cooperativeAStar.Setup(instance, this); - this.startTime = this.ElapsedMillisecondsTotal(); - double handlingStartTime = this.ElapsedMillisecondsTotal(); + CooperativeAStar cooperativeAStar = new(); + cooperativeAStar.Setup(instance, watch); + double elapsedTime = 0; foreach (var agentIndex in Enumerable.Range(0, instance.agents.Length)) @@ -948,10 +943,10 @@ public void SolveGivenProblemIncrementally(ProblemInstance instance) GC.Collect(); GC.WaitForPendingFinalizers(); - this.startTime += this.ElapsedMillisecondsTotal() - handlingStartTime; + watch.Restart(); bool solved = cooperativeAStar.AddOneAgent(agentIndex); - elapsedTime = this.ElapsedMilliseconds(); - handlingStartTime = this.ElapsedMillisecondsTotal(); + elapsedTime = watch.ElapsedMilliseconds; + if (solved) { Console.WriteLine("Total cost: {0}", cooperativeAStar.GetSolutionCost()); @@ -999,10 +994,10 @@ private void run(ISolver solver, ProblemInstance instance) // Run the algorithm bool solved; Console.WriteLine($"-----------------{solver}-----------------"); - this.startTime = this.ElapsedMillisecondsTotal(); - solver.Setup(instance, this); + watch.Restart(); + solver.Setup(instance, watch); solved = solver.Solve(); - double elapsedTime = this.ElapsedMilliseconds(); + double elapsedTime = watch.ElapsedMilliseconds; if (solved) { Console.WriteLine("Total cost: {0}", solver.GetSolutionCost()); @@ -1138,29 +1133,4 @@ public void ResetOutOfTimeCounters() outOfTimeCounters[i] = 0; } } - - private Stopwatch watch; - public double ElapsedMillisecondsTotal() - { - return this.watch.Elapsed.TotalMilliseconds; - } - - public double ElapsedMilliseconds() - { - return ElapsedMillisecondsTotal() - this.startTime; - } - - public void StartOracle() - { - this.watch.Stop(); - // NOTE: This allows the algorithm with the oracle to solve harder problems without timing out. - // Care must be taken when comparing average runtimes of algorithms, to avoid the average - // runtime of algorithms with an oracle appearing longer since they managed to solve - // harder problems. - } - - public void StopOracle() - { - this.watch.Start(); - } } From 7bc307a8cd9c1e5428336391491fa9a4e6a449ae Mon Sep 17 00:00:00 2001 From: Artem Kliatchkine Date: Tue, 6 May 2025 16:01:37 +0200 Subject: [PATCH 10/14] ProblemInstance: code refresh. --- A_Star.cs | 8 +- A_Star_WithOD.cs | 4 +- CBS.cs | 16 +-- CbsConstraint.cs | 4 +- CbsHeuristicForAStar.cs | 4 +- CbsNode.cs | 54 +++++----- CooperativeA_Star.cs | 2 +- CostTreeNodeSolver.cs | 6 +- CostTreeSearchSolver.cs | 8 +- EPEA_Star.cs | 2 +- EnumeratedPDB.cs | 8 +- IndependenceDetection.cs | 10 +- MddPruningHeuristicForCbs.cs | 8 +- PDB.cs | 2 +- PEA_Star.cs | 2 +- Plan.cs | 2 +- ProblemInstance.cs | 189 ++++++++++++++++------------------- Program.cs | 12 +-- Run.cs | 37 +++---- 19 files changed, 180 insertions(+), 198 deletions(-) diff --git a/A_Star.cs b/A_Star.cs index 7e9f39b..c1cfda8 100644 --- a/A_Star.cs +++ b/A_Star.cs @@ -101,7 +101,7 @@ public virtual void Setup(ProblemInstance problemInstance, int minDepth, Stopwat MDDNode mddRoot = null; if (mdd != null) { - Trace.Assert(problemInstance.agents.Length == 1, "Using MDDs to find new paths is currently only supported for single agent search"); + Trace.Assert(problemInstance.Agents.Length == 1, "Using MDDs to find new paths is currently only supported for single agent search"); mddRoot = mdd.levels[0].First.Value; } WorldState root = this.CreateSearchRoot(minDepth, minCost, mddRoot); @@ -120,7 +120,7 @@ public virtual void Setup(ProblemInstance problemInstance, int minDepth, Stopwat this.conflictCounts = null; this.conflictTimes = null; this.solutionDepth = -1; - this.numOfAgents = problemInstance.agents.Length; + this.numOfAgents = problemInstance.Agents.Length; this.maxSolutionCost = maxCost; this.CAT = CAT; @@ -153,7 +153,7 @@ public virtual void Setup(ProblemInstance problemInstance, int minDepth, Stopwat /// The root of the search tree protected virtual WorldState CreateSearchRoot(int minDepth = -1, int minCost = -1, MDDNode mddNode = null) { - return new WorldState(this.instance.agents, minDepth, minCost, mddNode); + return new WorldState(this.instance.Agents, minDepth, minCost, mddNode); } /// @@ -537,7 +537,7 @@ public virtual void Expand(WorldState node) { var intermediateNodes = new List() { node }; - for (int agentIndex = 0; agentIndex < this.instance.agents.Length ; ++agentIndex) + for (int agentIndex = 0; agentIndex < this.instance.Agents.Length ; ++agentIndex) { if (stopwatch.ElapsedMilliseconds > Constants.MAX_TIME) return; diff --git a/A_Star_WithOD.cs b/A_Star_WithOD.cs index 6f0d60e..d17ce4a 100644 --- a/A_Star_WithOD.cs +++ b/A_Star_WithOD.cs @@ -21,7 +21,7 @@ public A_Star_WithOD(IHeuristicCalculator heuristic = null, bool mSt override protected WorldState CreateSearchRoot(int minDepth = -1, int minCost = -1, MDDNode mddNode = null) { - return new WorldStateWithOD(this.instance.agents, minDepth, minCost, mddNode); + return new WorldStateWithOD(this.instance.Agents, minDepth, minCost, mddNode); } protected override WorldState CreateSearchNode(WorldState from) @@ -75,7 +75,7 @@ protected override List ExpandOneAgent(List intermediate var generated = base.ExpandOneAgent(intermediateNodes, agentIndex); - int childAgentTurn = ((parent.agentTurn + 1) % (this.instance.agents.Length)); + int childAgentTurn = ((parent.agentTurn + 1) % (this.instance.Agents.Length)); foreach (var node in generated) { WorldStateWithOD childNode = (WorldStateWithOD)node; diff --git a/CBS.cs b/CBS.cs index 48a6854..8825d85 100644 --- a/CBS.cs +++ b/CBS.cs @@ -247,10 +247,10 @@ public void Setup(ProblemInstance problemInstance, Stopwatch stopwatch, Conflict // TODO: Instead, maybe add the agents in the reservation table into the problem instance, and add positive // constraints for them along their entire path. Would require changing IIndependenceDetectionSolver.Setup // to specify the agents the reserved moves belong to. - var constraints = new HashSet(illegalMoves.Count * problemInstance.agents.Length); + var constraints = new HashSet(illegalMoves.Count * problemInstance.Agents.Length); foreach (var illegalMove in illegalMoves) { - foreach (var agentState in problemInstance.agents) + foreach (var agentState in problemInstance.Agents) { constraints.Add(new CbsConstraint(agentState.agent.agentNum, illegalMove)); } @@ -295,9 +295,9 @@ public virtual void Setup(ProblemInstance problemInstance, int minSolutionTimeSt if (CacheMdds) { - MDDCache = new Dictionary[_instance.agents.Length]; - MDDNarrownessValuesCache = new Dictionary>[_instance.agents.Length]; - for (int i = 0; i < _instance.agents.Length; i++) + MDDCache = new Dictionary[_instance.Agents.Length]; + MDDNarrownessValuesCache = new Dictionary>[_instance.Agents.Length]; + for (int i = 0; i < _instance.Agents.Length; i++) { MDDCache[i] = []; MDDNarrownessValuesCache[i] = []; @@ -320,7 +320,7 @@ public virtual void Setup(ProblemInstance problemInstance, int minSolutionTimeSt SetGlobals(); - CbsNode root = new(_instance.agents.Length, _solver, _singleAgentSolver, this); // Problem instance and various strategy data is all passed under 'this'. + CbsNode root = new(_instance.Agents.Length, _solver, _singleAgentSolver, this); // Problem instance and various strategy data is all passed under 'this'. // Solve the root node bool solved = root.Solve(minSolutionTimeStep); @@ -935,7 +935,7 @@ public virtual void Reset() // cost as the sum of their current paths and no other conflicts exist? Should just // adopt this solution and get a goal node. // TODO: Save the cost of the group in a table, and use it as a heuristic in the future! - child = new CbsNode(_instance.agents.Length, _solver, + child = new CbsNode(_instance.Agents.Length, _solver, _singleAgentSolver, this, node.AgentsGroupAssignment); // This will be the new root node child.MergeGroups(node.AgentsGroupAssignment[conflict.agentAIndex], node.AgentsGroupAssignment[conflict.agentBIndex], fixCounts: false // This is a new root node, it doesn't have conflict counts yet @@ -1856,7 +1856,7 @@ public override void Setup(ProblemInstance problemInstance, Stopwatch stopwatch) private void MakeConflictMatrix(ProblemInstance problemInstance) { - globalConflictsCounter = new int[problemInstance.agents.Length][]; + globalConflictsCounter = new int[problemInstance.Agents.Length][]; for (int i = 0; i < globalConflictsCounter.Length; i++) { globalConflictsCounter[i] = new int[i]; diff --git a/CbsConstraint.cs b/CbsConstraint.cs index 3fab267..b6142a7 100644 --- a/CbsConstraint.cs +++ b/CbsConstraint.cs @@ -31,12 +31,12 @@ public CbsConstraint(CbsConflict conflict, ProblemInstance instance, bool agentA if (agentA) { move = conflict.agentAmove; - agentNum = instance.agents[conflict.agentAIndex].agent.agentNum; + agentNum = instance.Agents[conflict.agentAIndex].agent.agentNum; } else { move = conflict.agentBmove; - agentNum = instance.agents[conflict.agentBIndex].agent.agentNum; + agentNum = instance.Agents[conflict.agentBIndex].agent.agentNum; } this.agentNum = (byte)agentNum; diff --git a/CbsHeuristicForAStar.cs b/CbsHeuristicForAStar.cs index eb64cbd..939711c 100644 --- a/CbsHeuristicForAStar.cs +++ b/CbsHeuristicForAStar.cs @@ -133,7 +133,7 @@ protected uint h(WorldState s, int targetCost, int sicEstimate=-1, int lowLevelG if (lowLevelGeneratedCap == -1) { // Rough estimate of the branching factor: - lowLevelGeneratedCap = (int) Math.Pow(Constants.NUM_ALLOWED_DIRECTIONS, this.instance.agents.Length); + lowLevelGeneratedCap = (int) Math.Pow(Constants.NUM_ALLOWED_DIRECTIONS, this.instance.Agents.Length); } // Calc the h: @@ -388,7 +388,7 @@ public DyanamicLazyCbsHeuristicForAStar(CBS cbs, Stopwatch stopwatch, bool repor public uint h(WorldState s, int targetH, float effectiveBranchingFactor) { // No need to check if SIC is zero because this heuristic is run after SIC was already computed, not instead of it. - int lowLevelGeneratedCap = (int) Math.Round(effectiveBranchingFactor * this.instance.agents.Length); // Cap of B_of_AStar * K, + int lowLevelGeneratedCap = (int) Math.Round(effectiveBranchingFactor * this.instance.Agents.Length); // Cap of B_of_AStar * K, // because CBS low level nodes are of one agent only so they're about k times cheaper to work with return base.h(s, s.G + targetH, -1, lowLevelGeneratedCap); } diff --git a/CbsNode.cs b/CbsNode.cs index accc7ca..bd5d658 100644 --- a/CbsNode.cs +++ b/CbsNode.cs @@ -142,7 +142,7 @@ public CbsNode(int numberOfAgents, ICbsSolver solver, ICbsSolver singleAgentSolv AgentNumToIndex = []; for (int i = 0; i < numberOfAgents; i++) { - AgentNumToIndex[CBS.GetProblemInstance().agents[i].agent.agentNum] = i; + AgentNumToIndex[CBS.GetProblemInstance().Agents[i].agent.agentNum] = i; } _depth = 0; ReplanSize = 1; @@ -345,13 +345,13 @@ public bool Solve(int depthToReplan) // layers of CBS/ID solvers, each one adding its own constraints and respecting those of the solvers above it. // Find all the agents groups: - List[] subGroups = new List[problem.agents.Length]; + List[] subGroups = new List[problem.Agents.Length]; for (int i = 0; i < AgentsGroupAssignment.Length; i++) { if (subGroups[AgentsGroupAssignment[i]] == null) - subGroups[AgentsGroupAssignment[i]] = [ problem.agents[i] ]; + subGroups[AgentsGroupAssignment[i]] = [ problem.Agents[i] ]; else - subGroups[AgentsGroupAssignment[i]].Add(problem.agents[i]); + subGroups[AgentsGroupAssignment[i]].Add(problem.Agents[i]); } bool success = true; @@ -370,9 +370,9 @@ public bool Solve(int depthToReplan) agentGroupHasMustConstraints == false && subGroup.Count == 1) // No constraints on this agent. Shortcut available (that doesn't consider the CAT, though!). { - SingleAgentPlans[i] = new SinglePlan(problem.agents[i]); // All moves up to starting pos, if any - SingleAgentPlans[i].AgentNum = problem.agents[AgentsGroupAssignment[i]].agent.agentNum; // Use the group's representative - SinglePlan optimalPlan = problem.GetSingleAgentOptimalPlan(problem.agents[i]); + SingleAgentPlans[i] = new SinglePlan(problem.Agents[i]); // All moves up to starting pos, if any + SingleAgentPlans[i].AgentNum = problem.Agents[AgentsGroupAssignment[i]].agent.agentNum; // Use the group's representative + SinglePlan optimalPlan = problem.GetSingleAgentOptimalPlan(problem.Agents[i]); // Count conflicts: ConflictCountsPerAgent[i] = []; ConflictTimesPerAgent[i] = []; @@ -382,7 +382,7 @@ public bool Solve(int depthToReplan) timedMove.IncrementConflictCounts(CAT, ConflictCountsPerAgent[i], ConflictTimesPerAgent[i]); } SingleAgentPlans[i].ContinueWith(optimalPlan); - SingleAgentCosts[i] = problem.agents[i].g + problem.GetSingleAgentOptimalCost(problem.agents[i]); + SingleAgentCosts[i] = problem.Agents[i].g + problem.GetSingleAgentOptimalCost(problem.Agents[i]); if (Constants.costFunction == Constants.CostFunction.SUM_OF_COSTS) { G += (ushort)SingleAgentCosts[i]; @@ -424,9 +424,9 @@ public bool Solve(int depthToReplan) AgentNumToIndex[pair.Key] < i) // Just an optimization. Would also be correct without this check. { ConflictCountsPerAgent[AgentNumToIndex[pair.Key]] // Yes, index here, num there - [problem.agents[i].agent.agentNum] = pair.Value; // Collisions are symmetrical, and agent "key" didn't see the route for agent "i" when planning. + [problem.Agents[i].agent.agentNum] = pair.Value; // Collisions are symmetrical, and agent "key" didn't see the route for agent "i" when planning. ConflictTimesPerAgent[AgentNumToIndex[pair.Key]] - [problem.agents[i].agent.agentNum] = ConflictTimesPerAgent[i][pair.Key]; + [problem.Agents[i].agent.agentNum] = ConflictTimesPerAgent[i][pair.Key]; } } } @@ -474,7 +474,7 @@ public bool Replan(int agentToReplan, int minPathTimeStep, for (int i = 0; i < AgentsGroupAssignment.Length; i++) { if (AgentsGroupAssignment[i] == groupNum) - subGroup.Add(problem.agents[i]); + subGroup.Add(problem.Agents[i]); else internalCAT.AddPlan(SingleAgentPlans[i]); } @@ -591,7 +591,7 @@ public bool Replan(int agentToReplan, int minPathTimeStep, int agentNum = subGroup[i].agent.agentNum; int agentIndex = AgentNumToIndex[agentNum]; SingleAgentPlans[agentIndex] = singlePlans[i]; - SingleAgentPlans[agentIndex].AgentNum = problem.agents[groupNum].agent.agentNum; // Use the group's representative - that's how the plans will be inserted into the CAT later too. + SingleAgentPlans[agentIndex].AgentNum = problem.Agents[groupNum].agent.agentNum; // Use the group's representative - that's how the plans will be inserted into the CAT later too. SingleAgentCosts[agentIndex] = singleCosts[i]; if (i == 0) // This is the group representative { @@ -630,7 +630,7 @@ public bool Replan(int agentToReplan, int minPathTimeStep, int representativeAgentNum = subGroup[0].agent.agentNum; for (int i = 0; i < ConflictCountsPerAgent.Length; i++) { - int agentNum = problem.agents[i].agent.agentNum; + int agentNum = problem.Agents[i].agent.agentNum; if (perAgent.ContainsKey(agentNum)) { ConflictCountsPerAgent[i][representativeAgentNum] = perAgent[agentNum]; @@ -741,7 +741,7 @@ public void DebugPrint() { if (ConflictCountsPerAgent[j].Count != 0) { - Debug.Write($"Agent {problem.agents[j].agent.agentNum} conflict counts: "); + Debug.Write($"Agent {problem.Agents[j].agent.agentNum} conflict counts: "); foreach (var pair in ConflictCountsPerAgent[j]) { Debug.Write($"{pair.Key}:{pair.Value} "); @@ -754,7 +754,7 @@ public void DebugPrint() { if (ConflictCountsPerAgent[j].Count != 0) { - Debug.Write($"Agent {problem.agents[j].agent.agentNum} conflict times: "); + Debug.Write($"Agent {problem.Agents[j].agent.agentNum} conflict times: "); foreach (var pair in ConflictTimesPerAgent[j]) { Debug.Write($"{pair.Key}:[{String.Join(",", pair.Value)}], "); @@ -792,7 +792,7 @@ protected void UpdateAtGoalConflictCounts(int agentIndex, ConflictAvoidanceTable { ProblemInstance problem = CBS.GetProblemInstance(); var afterGoal = new TimedMove( - problem.agents[agentIndex].agent.Goal.X, problem.agents[agentIndex].agent.Goal.Y, + problem.Agents[agentIndex].agent.Goal.X, problem.Agents[agentIndex].agent.Goal.Y, Direction.Wait, time: 0); for (int time = SingleAgentPlans[agentIndex].GetSize(); time < CAT.GetMaxPlanSize(); time++) { @@ -1967,7 +1967,7 @@ private CbsConflict FindConflict(int aConflictingGroupMemberIndex, out specificConflictingAgentA, out specificConflictingAgentB, groups); ProblemInstance problem = CBS.GetProblemInstance(); - int initialTimeStep = problem.agents[0].lastMove.Time; // To account for solving partially solved problems. + int initialTimeStep = problem.Agents[0].lastMove.Time; // To account for solving partially solved problems. // This assumes the makespan of all the agents is the same. Move first = SingleAgentPlans[specificConflictingAgentA].GetLocationAt(time); Move second = SingleAgentPlans[specificConflictingAgentB].GetLocationAt(time); @@ -2151,7 +2151,7 @@ public bool buildMddForAgentWithItsCurrentCost(int agentIndex) int depth = SingleAgentCosts.Max(); IEnumerable myConstraints = constraints.Where( - constraint => constraint.agentNum == problem.agents[agentIndex].agent.agentNum); + constraint => constraint.agentNum == problem.Agents[agentIndex].agent.agentNum); if (myConstraints.Count() != 0) { int maxConstraintTimeStep = myConstraints.Max(constraint => constraint.time); @@ -2160,7 +2160,7 @@ public bool buildMddForAgentWithItsCurrentCost(int agentIndex) if (positiveConstraints != null && positiveConstraints.Count != 0) { IEnumerable myMustConstraints = positiveConstraints.Where( - constraint => constraint.agentNum == problem.agents[agentIndex].agent.agentNum); + constraint => constraint.agentNum == problem.Agents[agentIndex].agent.agentNum); if (myMustConstraints.Count() != 0) { int maxMustConstraintTimeStep = myMustConstraints.Max(constraint => constraint.time); @@ -2171,8 +2171,8 @@ public bool buildMddForAgentWithItsCurrentCost(int agentIndex) Debug.WriteLine($"Building MDD for agent index {agentIndex} of cost {SingleAgentCosts[agentIndex]} and depth {depth}"); double startTime = CBS._stopwatch.ElapsedMilliseconds; - _mdds[agentIndex] = new MDD(agentIndex, problem.agents[agentIndex].agent.agentNum, - problem.agents[agentIndex].GetMove(), SingleAgentCosts[agentIndex], + _mdds[agentIndex] = new MDD(agentIndex, problem.Agents[agentIndex].agent.agentNum, + problem.Agents[agentIndex].GetMove(), SingleAgentCosts[agentIndex], depth, problem.GetNumOfAgents(), problem, ignoreConstraints: false, supportPruning: false, constraints: constraints, positiveConstraints: positiveConstraints); @@ -2265,7 +2265,7 @@ public bool buildMddForAgentWithItsCurrentCost(int agentIndex) ProblemInstance problem = CBS.GetProblemInstance(); time = ConflictTimesPerAgent[chosenAgentIndex] // Yes, the index of the first and the num of the second - [problem.agents[chosenConflictingAgentIndex].agent.agentNum][0]; + [problem.Agents[chosenConflictingAgentIndex].agent.agentNum][0]; return (groupRepA, groupRepB, time); } @@ -2825,8 +2825,8 @@ public void MergeGroups(int a, int b, bool fixCounts = true) (a, b) = (b, a); ProblemInstance problem = CBS.GetProblemInstance(); - int aAgentNum = problem.agents[a].agent.agentNum; - int bAgentNum = problem.agents[b].agent.agentNum; + int aAgentNum = problem.Agents[a].agent.agentNum; + int bAgentNum = problem.Agents[b].agent.agentNum; for (int i = 0; i < AgentsGroupAssignment.Length; i++) { @@ -2977,7 +2977,7 @@ public bool Replan3b(int agentToReplan, int depthToReplan, int minPathCost = -1, for (int i = 0; i < AgentsGroupAssignment.Length; i++) { if (AgentsGroupAssignment[i] == groupNum) - subGroup.Add(problem.agents[i]); + subGroup.Add(problem.Agents[i]); else internalCAT.AddPlan(SingleAgentPlans[i]); } @@ -2989,7 +2989,7 @@ public bool Replan3b(int agentToReplan, int depthToReplan, int minPathCost = -1, relevantSolver = _singleAgentSolver; ProblemInstance subProblem = problem.Subproblem(subGroup.ToArray()); - subProblem.parameters = problem.parameters; + subProblem.Parameters = problem.Parameters; MDD mdd = null; if (CBS.ReplanSameCostWithMdd) @@ -3015,7 +3015,7 @@ public bool Replan3b(int agentToReplan, int depthToReplan, int minPathCost = -1, if (AgentsGroupAssignment[i] == groupNum) { SingleAgentPlans[i] = singlePlans[j]; - SingleAgentPlans[i].AgentNum = problem.agents[groupNum].agent.agentNum; // Use the group's representative + SingleAgentPlans[i].AgentNum = problem.Agents[groupNum].agent.agentNum; // Use the group's representative SingleAgentCosts[i] = singleCosts[j]; j++; } diff --git a/CooperativeA_Star.cs b/CooperativeA_Star.cs index 160b662..4fd7c1b 100644 --- a/CooperativeA_Star.cs +++ b/CooperativeA_Star.cs @@ -64,7 +64,7 @@ public void Setup(ProblemInstance instance, Stopwatch stopwatch) this.Clear(); this.ClearStatistics(); this.problem = instance; - this.allAgentsState = instance.agents; + this.allAgentsState = instance.Agents; this.pathCosts = new int[this.allAgentsState.Length]; this.paths = new SinglePlan[this.allAgentsState.Length]; this.stopwatch = stopwatch; diff --git a/CostTreeNodeSolver.cs b/CostTreeNodeSolver.cs index 056b92a..2852161 100644 --- a/CostTreeNodeSolver.cs +++ b/CostTreeNodeSolver.cs @@ -69,7 +69,7 @@ public CostTreeNodeSolver(ProblemInstance problem, CostTreeNode costNode, Stopwa public virtual void Setup(CostTreeNode costsNode, ISet reserved) { - this.startingPos = problem.agents; + this.startingPos = problem.Agents; this.totalCost = costsNode.costs.Sum(); this.maxCost = costsNode.costs.Max(); @@ -89,9 +89,9 @@ public virtual void Setup(int[] agentNums, CostTreeNode costsNode, ISet this.accGeneratedHL; public int GetSolutionDepth() => this.solutionDepth; public long GetMemoryUsed() => Process.GetCurrentProcess().VirtualMemorySize64; - public int GetMaxGroupSize() => problem.agents.Length; + public int GetMaxGroupSize() => problem.Agents.Length; public SinglePlan[] GetSinglePlans() => solution; public virtual int[] GetSingleCosts() => costs; @@ -485,7 +485,7 @@ public override void Setup(ProblemInstance problemInstance, int minTimeStep, Sto ISet constraints = null, ISet positiveConstraints = null, int minCost = -1, int maxCost = int.MaxValue, MDD mdd = null) { - edgesMatrix = new int[problemInstance.agents.Length, problemInstance.GetMaxX() * problemInstance.GetMaxY() + problemInstance.GetMaxY(), Move.NUM_NON_DIAG_MOVES]; + edgesMatrix = new int[problemInstance.Agents.Length, problemInstance.GetMaxX() * problemInstance.GetMaxY() + problemInstance.GetMaxY(), Move.NUM_NON_DIAG_MOVES]; edgesMatrixCounter = 0; base.Setup(problemInstance, minTimeStep, stopwatch, CAT, constraints, positiveConstraints, minCost, maxCost, mdd); } @@ -578,7 +578,7 @@ public override void Setup(ProblemInstance problemInstance, int minTimeStep, Sto ISet constraints = null, ISet positiveConstraints = null, int minCost = -1, int maxCost = int.MaxValue, MDD mdd = null) { - edgesMatrix = new int[problemInstance.agents.Length, problemInstance.GetMaxX() * problemInstance.GetMaxY() + problemInstance.GetMaxY(), Move.NUM_NON_DIAG_MOVES]; + edgesMatrix = new int[problemInstance.Agents.Length, problemInstance.GetMaxX() * problemInstance.GetMaxY() + problemInstance.GetMaxY(), Move.NUM_NON_DIAG_MOVES]; edgesMatrixCounter = 0; base.Setup(problemInstance, minTimeStep, stopwatch, CAT, constraints, positiveConstraints, minCost, maxCost, mdd); } diff --git a/EPEA_Star.cs b/EPEA_Star.cs index cf9f6e4..e4db9b7 100644 --- a/EPEA_Star.cs +++ b/EPEA_Star.cs @@ -25,7 +25,7 @@ public EPEA_Star(IHeuristicCalculator heuristic = null, bool mstar = override protected WorldState CreateSearchRoot(int minDepth = -1, int minCost = -1, MDDNode mddNode = null) { - var root = new WorldStateForPartialExpansion(this.instance.agents, minDepth, minCost, mddNode); + var root = new WorldStateForPartialExpansion(this.instance.Agents, minDepth, minCost, mddNode); root.sic = (int)SumIndividualCosts.h(root, this.instance); return root; } diff --git a/EnumeratedPDB.cs b/EnumeratedPDB.cs index 71736ca..6d077fa 100644 --- a/EnumeratedPDB.cs +++ b/EnumeratedPDB.cs @@ -138,7 +138,7 @@ public void Dispose() /// pattern database in units of bytes. public override ulong estimateSize() { - return permutations[0] * problem.numLocations + (ulong) (sizeof(ulong) * permutations.Length); + return permutations[0] * problem.NumLocations + (ulong) (sizeof(ulong) * permutations.Length); } /// @@ -168,7 +168,7 @@ public override void build() // agents data structure, because during the building process // our state already is a projection. - WorldState goal = new WorldState(problem.agents, agentsToConsider); + WorldState goal = new WorldState(problem.Agents, agentsToConsider); foreach (AgentState ags in goal.allAgentsState) ags.SwapCurrentWithGoal(); List vBackup = agentsToConsider; @@ -183,7 +183,7 @@ public override void build() // particular state, which is also the shortest path to that state // because we are conducting an uninformed breadth-first search. - table = new Byte[permutations[0] * (problem.numLocations + 1)]; + table = new Byte[permutations[0] * (problem.NumLocations + 1)]; for (int i = 0; i < table.Length; ++i) table[i] = Byte.MaxValue; Context c = new Context(); @@ -374,6 +374,6 @@ void computePermutations() permutations = new UInt64[agentsToConsider.Count]; permutations[permutations.Length - 1] = 1; for(var i = permutations.Length - 2; i >= 0; --i) - permutations[i] = permutations[i + 1] * (UInt64) (problem.numLocations - (i + 1)); + permutations[i] = permutations[i + 1] * (UInt64) (problem.NumLocations - (i + 1)); } } diff --git a/IndependenceDetection.cs b/IndependenceDetection.cs index 371b859..4e9a0bc 100644 --- a/IndependenceDetection.cs +++ b/IndependenceDetection.cs @@ -99,7 +99,7 @@ public void Setup(ProblemInstance instance, Stopwatch stopwatch) this.resolutionAttemptedSecondGroup = new HashSet(); this.allGroups = new LinkedList(); // Initialize the agent group collection with a group for every agent - foreach (AgentState agentStartState in instance.agents) + foreach (AgentState agentStartState in instance.Agents) { this.allGroups.AddLast(new IndependenceDetectionAgentsGroup( this.instance,[ agentStartState ], @@ -207,7 +207,7 @@ public void ClearStatistics() this.expanded = 0; this.generated = 0; this.maxGroupSize = 1; - this.minGroupSize = instance.agents.Length; + this.minGroupSize = instance.Agents.Length; this.resolutionAttempts = 0; this.resolutionSuccesses = 0; this.merges = 0; @@ -218,7 +218,7 @@ public void ClearAccumulatedStatistics() this.accExpanded = 0; this.accGenerated = 0; this.accMaxGroupSize = 1; - this.accMinGroupSize = this.instance.agents.Length; + this.accMinGroupSize = this.instance.Agents.Length; this.accResolutionAttempts = 0; this.accResolutionSuccesses = 0; this.accMerges = 0; @@ -374,7 +374,7 @@ private IndependenceDetectionConflict ChooseConflictOfSmallestAgentsThatConflict this.countsOfGroupsThatConflict[i] / ((double)(1 << (this.GetGroupSize(i) - 1))) : -1; - int chosenGroupNum = Enumerable.Range(0, this.instance.agents.Length).MaxByKeyFunc(formula); + int chosenGroupNum = Enumerable.Range(0, this.instance.Agents.Length).MaxByKeyFunc(formula); // We could just look for any of this agent's conflicts, // but the best choice among the agents it conflicts with is the one which maximizes the formula itself. @@ -503,7 +503,7 @@ private IndependenceDetectionConflict ChooseConflictOfLeastConflictingLargestRes private IndependenceDetectionConflict ChooseConflictOfMostConflictingSmallestResultingGroup() { Dictionary groupSizes = this.allGroups.ToDictionary(group => group.groupNum, group => group.Size()); - int minResultingGroupSize = this.instance.agents.Length + 1; + int minResultingGroupSize = this.instance.Agents.Length + 1; int maxGroupsTheyWereInConflictWith = -1; int minTime = int.MaxValue; IndependenceDetectionAgentsGroup groupA = null; // The must be at least one conflict diff --git a/MddPruningHeuristicForCbs.cs b/MddPruningHeuristicForCbs.cs index e1cd404..42df67f 100644 --- a/MddPruningHeuristicForCbs.cs +++ b/MddPruningHeuristicForCbs.cs @@ -108,12 +108,12 @@ public uint h(CbsNode s) s.SingleAgentCosts[s.Conflict.agentBIndex]); // Building MDDs for the conflicting agents. We can't keep them because we're // destructively syncing them later (the first one, at least). - var mddA = new MDD(s.Conflict.agentAIndex, _instance.agents[s.Conflict.agentAIndex].agent.agentNum, - _instance.agents[s.Conflict.agentAIndex].lastMove, + var mddA = new MDD(s.Conflict.agentAIndex, _instance.Agents[s.Conflict.agentAIndex].agent.agentNum, + _instance.Agents[s.Conflict.agentAIndex].lastMove, s.SingleAgentCosts[s.Conflict.agentAIndex], maxCost, _instance.GetNumOfAgents(), _instance, _ignoreConstraints); - var mddB = new MDD(s.Conflict.agentBIndex, _instance.agents[s.Conflict.agentBIndex].agent.agentNum, - _instance.agents[s.Conflict.agentBIndex].lastMove, + var mddB = new MDD(s.Conflict.agentBIndex, _instance.Agents[s.Conflict.agentBIndex].agent.agentNum, + _instance.Agents[s.Conflict.agentBIndex].lastMove, s.SingleAgentCosts[s.Conflict.agentBIndex], maxCost, _instance.GetNumOfAgents(), _instance, _ignoreConstraints); s.CBS.MDDsBuilt += 2; diff --git a/PDB.cs b/PDB.cs index 9ea0f53..18c432c 100644 --- a/PDB.cs +++ b/PDB.cs @@ -91,7 +91,7 @@ public void Expand(WorldState currentNode, int agentIndex, ICollection heuristic = null) override protected WorldState CreateSearchRoot(int minDepth = -1, int minCost = -1, MDDNode mddNode = null) { - return new WorldStateForPartialExpansion(this.instance.agents, minDepth, minCost, mddNode); // Consider using a WorldStateForBasicPartialExpansion that only has the IsAlreadyExpanded stuff + return new WorldStateForPartialExpansion(this.instance.Agents, minDepth, minCost, mddNode); // Consider using a WorldStateForBasicPartialExpansion that only has the IsAlreadyExpanded stuff } protected override WorldState CreateSearchNode(WorldState from) diff --git a/Plan.cs b/Plan.cs index 1fe075f..caad468 100644 --- a/Plan.cs +++ b/Plan.cs @@ -144,7 +144,7 @@ public void Check(ProblemInstance problem) SinglePlan[] singles = new SinglePlan[_locationsAtTimes.First().Count]; for (int i = 0; i < singles.Length; i++) { - singles[i] = new SinglePlan(this, i, problem.agents[i].agent.agentNum); + singles[i] = new SinglePlan(this, i, problem.Agents[i].agent.agentNum); foreach ((int time, var move) in singles[i].LocationAtTimes.Enumerate()) Trace.Assert(problem.IsValid(move), $"Plan of agent {i} uses an invalid location {move} at time {time}!"); } diff --git a/ProblemInstance.cs b/ProblemInstance.cs index 807e232..3afeada 100644 --- a/ProblemInstance.cs +++ b/ProblemInstance.cs @@ -21,12 +21,12 @@ public class ProblemInstance /// /// This contains extra data of this problem instance (used for special problem instances, e.g. subproblems of a bigger problem instance). /// - public IDictionary parameters; + public Dictionary Parameters { get; set; } /// /// Contains true at [x][y] if cell (x,y) is an obstacle /// - public BitMatrix grid; + public BitMatrix Grid { get; private set; } /// /// We keep a reference to the array of agents in the original problem. @@ -34,34 +34,34 @@ public class ProblemInstance /// iteration that a new set of agents must be jointly planned due /// to their mutual conflicts. /// - public AgentState[] agents; + public AgentState[] Agents { get; private set; } /// /// This is a matrix that contains the cost of the optimal path to the goal of every agent from any point in the grid. /// The first dimension of the matrix is the number of agents. /// The second dimension of the matrix is the cardinality of the location from which we want the shortest path. /// - public int[][] singleAgentOptimalCosts; + private int[][] _singleAgentOptimalCosts; /// /// The time it took to compute the shortest paths. /// - public double shortestPathComputeTime; + public double ShortestPathComputeTime { get; private set; } /// /// This is a matrix that contains the best move towards the goal of every agent from any point in the grid. /// The first dimension of the matrix is the number of agents. /// The second dimension of the matrix is the cardinality of the location from which we want the shortest path. /// - public Move[][] singleAgentOptimalMoves; + private Move[][] _singleAgentOptimalMoves; - public uint numObstacles; - public uint numLocations; + public uint NumObstacles { get; private set; } + public uint NumLocations { get; private set; } /// /// This field is used to identify an instance when running a set of experiments /// - public int instanceId; + public int InstanceId { get; set; } /// /// Enumerates all of the empty spots in the grid. The indices @@ -70,17 +70,17 @@ public class ProblemInstance /// the y-axis. If there are obstacles, it's more space-efficient to store /// data for each non-empty spot. /// - public int[,] cardinality; + private int[,] _cardinality; - public string gridName; - public string instanceName; + public string GridName { get; set; } + public string InstanceName { get; set; } - public ProblemInstance(IDictionary parameters = null) + public ProblemInstance(Dictionary parameters = null) { if (parameters != null) - this.parameters = parameters; + Parameters = parameters; else - this.parameters = new Dictionary(); + Parameters = []; } /// @@ -92,13 +92,13 @@ public ProblemInstance Subproblem(AgentState[] selectedAgents) { // Notice selected agents may actually be a completely different set of agents. // Not copying instance id. This isn't the same problem. - ProblemInstance subproblemInstance = new(this.parameters); - subproblemInstance.Init(selectedAgents, this.grid, (int)this.numObstacles, (int)this.numLocations, this.cardinality); - subproblemInstance.singleAgentOptimalCosts = this.singleAgentOptimalCosts; // Each subproblem knows every agent's single shortest paths so this.singleAgentOptimalCosts[agent_num] would easily work - subproblemInstance.singleAgentOptimalMoves = this.singleAgentOptimalMoves; - subproblemInstance.gridName = this.gridName; - subproblemInstance.instanceName = this.instanceName; - subproblemInstance.instanceId = this.instanceId; + ProblemInstance subproblemInstance = new(Parameters); + subproblemInstance.Init(selectedAgents, Grid, (int)NumObstacles, (int)NumLocations, _cardinality); + subproblemInstance._singleAgentOptimalCosts = _singleAgentOptimalCosts; // Each subproblem knows every agent's single shortest paths so singleAgentOptimalCosts[agent_num] would easily work + subproblemInstance._singleAgentOptimalMoves = _singleAgentOptimalMoves; + subproblemInstance.GridName = GridName; + subproblemInstance.InstanceName = InstanceName; + subproblemInstance.InstanceId = InstanceId; return subproblemInstance; } @@ -113,8 +113,8 @@ public ProblemInstance Subproblem(AgentState[] selectedAgents) public void Init(AgentState[] agentStartStates, BitMatrix grid, int nObstacles=-1, int nLocations=-1, int[,] cardinality=null) { - agents = agentStartStates; - this.grid = grid; + Agents = agentStartStates; + Grid = grid; if (nObstacles == -1) { @@ -122,21 +122,21 @@ public void Init(AgentState[] agentStartStates, BitMatrix grid, int nObstacles=- for (int j = 0; j < grid.RowsCount; j++) { if (grid[i, j]) - numObstacles++; + NumObstacles++; } } else - numObstacles = (uint)nObstacles; + NumObstacles = (uint)nObstacles; if (nLocations == -1) - numLocations = ((uint)(grid.ColumnsCount * grid.RowsCount)) - numObstacles; + NumLocations = ((uint)(grid.ColumnsCount * grid.RowsCount)) - NumObstacles; else - numLocations = (uint)nLocations; + NumLocations = (uint)nLocations; if (cardinality == null) PrecomputeCardinality(); else - this.cardinality = cardinality; + _cardinality = cardinality; } /// @@ -150,23 +150,23 @@ public void ComputeSingleAgentShortestPaths() double startTime = watch.Elapsed.TotalMilliseconds; //return; // Add for generator - this.singleAgentOptimalCosts = new int[this.GetNumOfAgents()][]; - this.singleAgentOptimalMoves = new Move[this.GetNumOfAgents()][]; + _singleAgentOptimalCosts = new int[GetNumOfAgents()][]; + _singleAgentOptimalMoves = new Move[GetNumOfAgents()][]; - for (int agentId = 0; agentId < this.GetNumOfAgents(); agentId++) + for (int agentId = 0; agentId < 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++) + var shortestPathLengths = new int[NumLocations]; + var optimalMoves = new Move[NumLocations]; + for (int i = 0; i < NumLocations; i++) shortestPathLengths[i] = -1; var openlist = new Queue(); // Create initial state - var agentStartState = this.agents[agentId]; + var agentStartState = 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); + int goalIndex = GetCardinality(goalState.lastMove); shortestPathLengths[goalIndex] = 0; optimalMoves[goalIndex] = new Move(goalState.lastMove); openlist.Enqueue(goalState); @@ -180,7 +180,7 @@ public void ComputeSingleAgentShortestPaths() { if (IsValid(aMove)) { - int entry = cardinality[aMove.X, aMove.Y]; + 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)) { @@ -195,7 +195,7 @@ public void ComputeSingleAgentShortestPaths() } - int start = this.GetCardinality(agentStartState.lastMove); + int start = GetCardinality(agentStartState.lastMove); if (shortestPathLengths[start] == -1) { throw new Exception($"Unsolvable instance! Agent {agentId} cannot reach its goal"); @@ -203,62 +203,47 @@ public void ComputeSingleAgentShortestPaths() // s1-g2-g1-s2 } - this.singleAgentOptimalCosts[agentId] = shortestPathLengths; - this.singleAgentOptimalMoves[agentId] = optimalMoves; + _singleAgentOptimalCosts[agentId] = shortestPathLengths; + _singleAgentOptimalMoves[agentId] = optimalMoves; } double endTime = watch.Elapsed.TotalMilliseconds; - this.shortestPathComputeTime = endTime - startTime; + ShortestPathComputeTime = endTime - startTime; } /// /// Returns the length of the shortest path between a given coordinate and the goal location of the given agent. /// - /// - /// - /// /// The length of the shortest path from x,y to the goal of the agent. - public int GetSingleAgentOptimalCost(int agentNum, int x, int y) - { - return this.singleAgentOptimalCosts[agentNum][this.cardinality[x, y]]; - } + public int GetSingleAgentOptimalCost(int agentNum, int x, int y) => _singleAgentOptimalCosts[agentNum][_cardinality[x, y]]; /// /// Returns the length of the shortest path between a given coordinate and the goal location of the given agent. /// - /// - /// /// The length of the shortest path from x,y to the goal of the agent. - public int GetSingleAgentOptimalCost(int agentNum, Move move) - { - return this.singleAgentOptimalCosts[agentNum][this.cardinality[move.X, move.Y]]; - } + public int GetSingleAgentOptimalCost(int agentNum, Move move) => _singleAgentOptimalCosts[agentNum][_cardinality[move.X, move.Y]]; /// /// Returns the length of the shortest path between a given agent's location and the goal of that agent. /// - /// /// The length of the shortest path between a given agent's location and the goal of that agent public int GetSingleAgentOptimalCost(AgentState agentState) { - int locationCardinality = this.cardinality[agentState.lastMove.X, agentState.lastMove.Y]; - return this.singleAgentOptimalCosts[agentState.agent.agentNum][locationCardinality]; + int locationCardinality = _cardinality[agentState.lastMove.X, agentState.lastMove.Y]; + return _singleAgentOptimalCosts[agentState.agent.agentNum][locationCardinality]; } /// /// Returns the optimal move towards the goal of the given agent. Move isn't necessarily unique. /// - /// - /// public Move GetSingleAgentOptimalMove(AgentState agentState) { - int locationCardinality = this.cardinality[agentState.lastMove.X, agentState.lastMove.Y]; - return this.singleAgentOptimalMoves[agentState.agent.agentNum][locationCardinality]; + int locationCardinality = _cardinality[agentState.lastMove.X, agentState.lastMove.Y]; + return _singleAgentOptimalMoves[agentState.agent.agentNum][locationCardinality]; } /// /// Note: The returned plan wasn't constructed considering a CAT, so it's possible there's an alternative plan with the same cost and less collisions. /// - /// /// An optimal plan for the agent, ignoring all others public SinglePlan GetSingleAgentOptimalPlan(AgentState agentState) { @@ -276,7 +261,7 @@ public SinglePlan GetSingleAgentOptimalPlan(AgentState agentState) // Get next optimal move time++; - Move optimal = this.singleAgentOptimalMoves[agentNum][this.GetCardinality(current)]; + Move optimal = _singleAgentOptimalMoves[agentNum][GetCardinality(current)]; current = new TimedMove(optimal, time); } @@ -288,18 +273,18 @@ public SinglePlan GetSingleAgentOptimalPlan(AgentState agentState) /// public int GetNumOfAgents() { - return this.agents.Length; + return Agents.Length; } /// /// Utility function that returns the x dimension of the grid /// - public int GetMaxX() => grid.ColumnsCount; + public int GetMaxX() => Grid.ColumnsCount; /// /// Utility function that returns the y dimension of the grid /// - public int GetMaxY() => grid.RowsCount; + public int GetMaxY() => Grid.RowsCount; private static BitMatrix readMapFile(string mapFilePath) { @@ -353,7 +338,8 @@ private static BitMatrix readBenchmarkMap(TextReader input, string line) } - private static BitMatrix readLironMap(TextReader input, string line) { + private static BitMatrix readLironMap(TextReader input, string line) + { string[] lineParts; lineParts = line.Split(','); int maxX = int.Parse(lineParts[0]); @@ -447,9 +433,9 @@ public static ProblemInstance Import(string filePath, string mapFilePath = null) // Generate the problem instance ProblemInstance instance = new(); instance.Init(states, grid); - instance.instanceId = instanceId; - instance.gridName = mapfileNameWithoutExtension; - instance.instanceName = fileNameWithoutExtension + ".agents"; + instance.InstanceId = instanceId; + instance.GridName = mapfileNameWithoutExtension; + instance.InstanceName = fileNameWithoutExtension + ".agents"; instance.ComputeSingleAgentShortestPaths(); return instance; } @@ -547,9 +533,9 @@ public static ProblemInstance Import(string filePath, string mapFilePath = null) // Generate the problem instance ProblemInstance instance = new(); instance.Init([.. stateList], grid); - instance.instanceId = instanceId; - instance.gridName = mapfileName; - instance.instanceName = Path.GetFileName(filePath); + instance.InstanceId = instanceId; + instance.GridName = mapfileName; + instance.InstanceName = Path.GetFileName(filePath); //instance.ComputeSingleAgentShortestPaths(); // FIXME: Uncomment this hack later return instance; } @@ -630,9 +616,9 @@ public static ProblemInstance Import(string filePath, string mapFilePath = null) // Generate the problem instance ProblemInstance instance = new ProblemInstance(); instance.Init(states, grid); - instance.instanceId = instanceId; - instance.gridName = gridName; - instance.instanceName = Path.GetFileNameWithoutExtension(filePath); + instance.InstanceId = instanceId; + instance.GridName = gridName; + instance.InstanceName = Path.GetFileNameWithoutExtension(filePath); instance.ComputeSingleAgentShortestPaths(); return instance; } @@ -655,17 +641,17 @@ public void Export(string fileName, string mapFileName = null) if (mapFileName == null) throw new Exception("Map file name needed for .scen format"); - foreach (var agentState in this.agents) + foreach (var agentState in Agents) { // Output all agent as block 1, with optimal cost -1 - output.WriteLine($"{1}\t{mapFileName}\t{grid.RowsCount}\t{grid.ColumnsCount}\t{agentState.lastMove.Y}\t{agentState.lastMove.X}\t{agentState.agent.Goal.Y}\t{agentState.agent.Goal.X}\t{-1}"); + output.WriteLine($"{1}\t{mapFileName}\t{Grid.RowsCount}\t{Grid.ColumnsCount}\t{agentState.lastMove.Y}\t{agentState.lastMove.X}\t{agentState.agent.Goal.Y}\t{agentState.agent.Goal.X}\t{-1}"); } } else if (fileName.EndsWith(".agents")) { - output.WriteLine(this.GetNumOfAgents()); + output.WriteLine(GetNumOfAgents()); - foreach (var agentState in this.agents) + foreach (var agentState in Agents) { output.WriteLine($"{agentState.agent.Goal.X},{agentState.agent.Goal.Y},{agentState.lastMove.X},{agentState.lastMove.X}"); } @@ -673,17 +659,17 @@ public void Export(string fileName, string mapFileName = null) else { // Output the instance ID - output.WriteLine($"{this.instanceId},{this.gridName}"); + output.WriteLine($"{InstanceId},{GridName}"); // Output the grid output.WriteLine("Grid:"); - output.WriteLine($"{this.grid.ColumnsCount},{this.grid.RowsCount}"); + output.WriteLine($"{Grid.ColumnsCount},{Grid.RowsCount}"); - for (int i = 0; i < this.grid.ColumnsCount; i++) + for (int i = 0; i < Grid.ColumnsCount; i++) { - for (int j = 0; j < this.grid.RowsCount; j++) + for (int j = 0; j < Grid.RowsCount; j++) { - if (this.grid[i, j] == true) + if (Grid[i, j] == true) output.Write('@'); else output.Write('.'); @@ -693,11 +679,11 @@ public void Export(string fileName, string mapFileName = null) } // Output the agents state output.WriteLine("Agents:"); - output.WriteLine(this.agents.Length); + output.WriteLine(Agents.Length); AgentState state; - for (int i = 0; i < this.agents.Length; i++) + for (int i = 0; i < Agents.Length; i++) { - state = this.agents[i]; + state = Agents[i]; output.WriteLine($"{state.agent.agentNum}{EXPORT_DELIMITER}{state.agent.Goal.X}{EXPORT_DELIMITER}{state.agent.Goal.Y}{EXPORT_DELIMITER}{state.lastMove.X}{EXPORT_DELIMITER}{state.lastMove.Y}"); } } @@ -712,22 +698,19 @@ public void Export(string fileName, string mapFileName = null) /// An agent's current location. /// n, where the agent is located at the nth non-obstacle /// location in our grid. - public int GetCardinality(Move move) - { - return cardinality[move.X, move.Y]; - } + public int GetCardinality(Move move) => _cardinality[move.X, move.Y]; private void PrecomputeCardinality() { - cardinality = new int[grid.ColumnsCount, grid.RowsCount]; + _cardinality = new int[Grid.ColumnsCount, Grid.RowsCount]; int maxCardinality = 0; - for (int i = 0; i < grid.ColumnsCount; ++i) - for (int j = 0; j < grid.RowsCount; ++j) + for (int i = 0; i < Grid.ColumnsCount; ++i) + for (int j = 0; j < Grid.RowsCount; ++j) { - if (grid[i,j]) - cardinality[i, j] = -1; + if (Grid[i,j]) + _cardinality[i, j] = -1; else - cardinality[i, j] = maxCardinality++; + _cardinality[i, j] = maxCardinality++; } } @@ -758,11 +741,9 @@ public bool IsValidTile(int x, int y) return false; if (y < 0 || y >= GetMaxY()) return false; - return !grid[x, y]; + return !Grid[x, y]; } - public override string ToString() - { - return $"Problem instance name:{instanceName} #Agents:{agents.Length}, GridCells:{numLocations}, #Obstacles:{numObstacles}"; - } + public override string ToString() => + $"Problem instance name:{InstanceName} #Agents:{Agents.Length}, GridCells:{NumLocations}, #Obstacles:{NumObstacles}"; } diff --git a/Program.cs b/Program.cs index b6216c5..711f9c2 100644 --- a/Program.cs +++ b/Program.cs @@ -124,7 +124,7 @@ public void RunExperimentSet(int[] gridSizes, int[] agentListSizes, int[] obstac try { instance = ProblemInstance.Import(Path.Combine(Directory.GetCurrentDirectory(), "..", "..", "..", "Instances", instanceName)); - instance.instanceId = i; + instance.InstanceId = i; } catch (Exception importException) { @@ -136,7 +136,7 @@ public void RunExperimentSet(int[] gridSizes, int[] agentListSizes, int[] obstac instance = runner.GenerateProblemInstance(gridSizes[gridSizeIndex], agentListSizes[numOfAgentsIndex], obstaclesProbs[obstaclePercentageIndex] * gridSizes[gridSizeIndex] * gridSizes[gridSizeIndex] / 100); instance.ComputeSingleAgentShortestPaths(); // REMOVE FOR GENERATOR - instance.instanceId = i; + instance.InstanceId = i; instance.Export(instanceName); } @@ -260,7 +260,7 @@ public void RunDragonAgeExperimentSet(int numInstances, string[] mapFilePaths) instance = runner.GenerateDragonAgeProblemInstance(mapFilePath, agentListSizes[ag]); instance.ComputeSingleAgentShortestPaths(); // Consider just importing the generated problem after exporting it to remove the duplication of this line from Import() - instance.instanceId = i; + instance.InstanceId = i; instance.Export(instanceName); } @@ -360,7 +360,7 @@ static void Main(string[] args) runner.watch.Restart(); IHeuristicCalculator lowLevelHeuristic = new SumIndividualCosts(); - List agentList = Enumerable.Range(0, instance.agents.Length).Select(x=> (uint)x).ToList(); // FIXME: Must the heuristics really receive a list of uints? + List agentList = Enumerable.Range(0, instance.Agents.Length).Select(x=> (uint)x).ToList(); // FIXME: Must the heuristics really receive a list of uints? lowLevelHeuristic.Init(instance, agentList); IIndependenceDetectionSolver lowLevel = new A_Star(lowLevelHeuristic); ILazyHeuristic highLevelHeuristic = new MvcHeuristicForCbs(); @@ -424,9 +424,9 @@ static void Main(string[] args) runner.OpenResultsFile(RESULTS_FILE_NAME); if (resultsFileExisted == false) runner.PrintResultsFileHeader(); - foreach (var numAgents in Enumerable.Range(1, problem.agents.Length)) + foreach (var numAgents in Enumerable.Range(1, problem.Agents.Length)) { - var subProblem = problem.Subproblem(problem.agents.Take(numAgents).ToArray()); + var subProblem = problem.Subproblem(problem.Agents.Take(numAgents).ToArray()); bool success = runner.SolveGivenProblem(subProblem); if (success == false) break; diff --git a/Run.cs b/Run.cs index 697d0e5..537b174 100644 --- a/Run.cs +++ b/Run.cs @@ -763,7 +763,7 @@ public ProblemInstance GenerateDragonAgeProblemInstance(string mapFilePath, int } ProblemInstance problem = new(); - problem.gridName = Path.GetFileNameWithoutExtension(mapFilePath); + problem.GridName = Path.GetFileNameWithoutExtension(mapFilePath); problem.Init(agentStates, grid); for (int j = 0; j < RANDOM_WALK_STEPS; j++) @@ -803,7 +803,7 @@ public bool SolveGivenProblem(ProblemInstance instance) { //return; // add for generator // Preparing a list of agent indices (not agent nums) for the heuristics' Init() method - List agentList = Enumerable.Range(0, instance.agents.Length).Select(x=> (uint)x).ToList(); // FIXME: Must the heuristics really receive a list of uints? + List agentList = Enumerable.Range(0, instance.Agents.Length).Select(x=> (uint)x).ToList(); // FIXME: Must the heuristics really receive a list of uints? // Solve using the different algorithms Console.WriteLine($"Solving {instance}"); @@ -837,9 +837,9 @@ public bool SolveGivenProblem(ProblemInstance instance) { if (((CBS)solvers[i]).MergeThreshold == 314159) // MAGIC NUMBER WHICH MAKES US ADJUST B according to map { - if (instance.gridName.StartsWith("den")) + if (instance.GridName.StartsWith("den")) ((CBS)solvers[i]).MergeThreshold = 10; - else if (instance.gridName.StartsWith("brc") || instance.gridName.StartsWith("ost")) + else if (instance.GridName.StartsWith("brc") || instance.GridName.StartsWith("ost")) ((CBS)solvers[i]).MergeThreshold = 100; } } @@ -854,9 +854,9 @@ public bool SolveGivenProblem(ProblemInstance instance) { if (((CBS)solvers[i]).MergeThreshold == 314159) // MAGIC NUMBER SEE ABOVE { - if (instance.gridName.StartsWith("den")) + if (instance.GridName.StartsWith("den")) ((CBS)solvers[i]).MergeThreshold = 10; - else if (instance.gridName.StartsWith("brc") || instance.gridName.StartsWith("ost")) + else if (instance.GridName.StartsWith("brc") || instance.GridName.StartsWith("ost")) ((CBS)solvers[i]).MergeThreshold = 100; } } @@ -927,14 +927,14 @@ public bool SolveGivenProblem(ProblemInstance instance) public void SolveGivenProblemIncrementally(ProblemInstance instance) { // Preparing a list of agent indices (not agent nums) for the heuristics' Init() method - List agentList = Enumerable.Range(0, instance.agents.Length).Select(x => (uint)x).ToList(); // FIXME: Must the heuristics really receive a list of uints? + List agentList = Enumerable.Range(0, instance.Agents.Length).Select(x => (uint)x).ToList(); // FIXME: Must the heuristics really receive a list of uints? CooperativeAStar cooperativeAStar = new(); cooperativeAStar.Setup(instance, watch); double elapsedTime = 0; - foreach (var agentIndex in Enumerable.Range(0, instance.agents.Length)) + foreach (var agentIndex in Enumerable.Range(0, instance.Agents.Length)) { // Solve using the different algorithms Console.WriteLine($"Solving {instance} agent {agentIndex}"); @@ -961,7 +961,7 @@ public void SolveGivenProblemIncrementally(ProblemInstance instance) Console.WriteLine("Time In milliseconds: {0}", elapsedTime); - this.PrintStatistics(instance, cooperativeAStar, elapsedTime + instance.shortestPathComputeTime); + this.PrintStatistics(instance, cooperativeAStar, elapsedTime + instance.ShortestPathComputeTime); Console.WriteLine(); @@ -1011,11 +1011,12 @@ private void run(ISolver solver, ProblemInstance instance) } Console.WriteLine(); - Console.WriteLine("Time In milliseconds: {0}", elapsedTime + instance.shortestPathComputeTime); + Console.WriteLine("Time In milliseconds: {0}", elapsedTime + instance.ShortestPathComputeTime); + Console.WriteLine("Of which shortest path compute time in milliseconds: {0}", instance.ShortestPathComputeTime); // TODO: Allow solvers to claim they don't use this heuristic and don't add the time to // compute it to their runtime. - this.PrintStatistics(instance, solver, elapsedTime + instance.shortestPathComputeTime); + this.PrintStatistics(instance, solver, elapsedTime + instance.ShortestPathComputeTime); // Solver clears itself when it finishes the search. solver.ClearStatistics(); } @@ -1090,19 +1091,19 @@ private void PrintStatistics(ProblemInstance instance, ISolver solver, double ru private void PrintProblemStatistics(ProblemInstance instance) { // Grid Name col: - this.resultsWriter.Write(instance.gridName + RESULTS_DELIMITER); + this.resultsWriter.Write(instance.GridName + RESULTS_DELIMITER); // Grid Rows col: - this.resultsWriter.Write(instance.grid.ColumnsCount + RESULTS_DELIMITER); + this.resultsWriter.Write(instance.Grid.ColumnsCount + RESULTS_DELIMITER); // Grid Columns col: - this.resultsWriter.Write(instance.grid.RowsCount + RESULTS_DELIMITER); + this.resultsWriter.Write(instance.Grid.RowsCount + RESULTS_DELIMITER); // Scenario/instance Name col: - this.resultsWriter.Write(instance.instanceName + RESULTS_DELIMITER); + this.resultsWriter.Write(instance.InstanceName + RESULTS_DELIMITER); // Num Of Agents col: - this.resultsWriter.Write(instance.agents.Length + RESULTS_DELIMITER); + this.resultsWriter.Write(instance.Agents.Length + RESULTS_DELIMITER); // Num Of Obstacles col: - this.resultsWriter.Write(instance.numObstacles + RESULTS_DELIMITER); + this.resultsWriter.Write(instance.NumObstacles + RESULTS_DELIMITER); // Instance Id col: - this.resultsWriter.Write(instance.instanceId + RESULTS_DELIMITER); + this.resultsWriter.Write(instance.InstanceId + RESULTS_DELIMITER); } private void ContinueToNextLine() From e5da5f83b7550709600fd2be2e4c4b2d9879a4cf Mon Sep 17 00:00:00 2001 From: Artem Kliatchkine Date: Tue, 6 May 2025 16:52:08 +0200 Subject: [PATCH 11/14] Added Conditional attribute on DebugPrint methods elliminating their strong influence on the calculation time. --- CbsNode.cs | 1 + MDD.cs | 1 + Plan.cs | 1 + ProblemInstance.cs | 9 +++++---- 4 files changed, 8 insertions(+), 4 deletions(-) diff --git a/CbsNode.cs b/CbsNode.cs index bd5d658..6ac7888 100644 --- a/CbsNode.cs +++ b/CbsNode.cs @@ -669,6 +669,7 @@ public bool Replan(int agentToReplan, int minPathTimeStep, return true; } + [Conditional("DEBUG")] public void DebugPrint() { Debug.WriteLine(""); diff --git a/MDD.cs b/MDD.cs index 8533c7c..8ad0132 100644 --- a/MDD.cs +++ b/MDD.cs @@ -369,6 +369,7 @@ public int getAgentNum() return mddNum; } + [Conditional("DEBUG")] public void DebugPrint() { Debug.WriteLine($"MDD for agent {this.agentNum}, {this.levels.Length} steps, cost {this.cost}:"); diff --git a/Plan.cs b/Plan.cs index caad468..393a152 100644 --- a/Plan.cs +++ b/Plan.cs @@ -478,6 +478,7 @@ public bool IsColliding(int time, SinglePlan otherPlan) /// Prints the plan to the Console. /// This is used for debugging purposes. /// + [Conditional("DEBUG")] public void DebugPrint() { for (int time = 0; time < LocationAtTimes.Count; time++) diff --git a/ProblemInstance.cs b/ProblemInstance.cs index 3afeada..74f28e6 100644 --- a/ProblemInstance.cs +++ b/ProblemInstance.cs @@ -153,6 +153,7 @@ public void ComputeSingleAgentShortestPaths() _singleAgentOptimalCosts = new int[GetNumOfAgents()][]; _singleAgentOptimalMoves = new Move[GetNumOfAgents()][]; + // TODO: test on bigger grids and see if Parallel loop can be beneficial. for (int agentId = 0; agentId < GetNumOfAgents(); agentId++) { // Run a single source shortest path algorithm from the _goal_ of the agent @@ -160,12 +161,12 @@ public void ComputeSingleAgentShortestPaths() var optimalMoves = new Move[NumLocations]; for (int i = 0; i < NumLocations; i++) shortestPathLengths[i] = -1; - var openlist = new Queue(); + Queue openlist = new(); // Create initial state - var agentStartState = Agents[agentId]; - var agent = agentStartState.agent; - var goalState = new AgentState(agent.Goal.X, agent.Goal.Y, -1, -1, agentId); + AgentState agentStartState = Agents[agentId]; + Agent agent = agentStartState.agent; + AgentState goalState = new(agent.Goal.X, agent.Goal.Y, -1, -1, agentId); int goalIndex = GetCardinality(goalState.lastMove); shortestPathLengths[goalIndex] = 0; optimalMoves[goalIndex] = new Move(goalState.lastMove); From 7ad2f9185fa748852c2b9863f4590be0b031156c Mon Sep 17 00:00:00 2001 From: Artem Kliatchkine Date: Wed, 7 May 2025 09:54:09 +0200 Subject: [PATCH 12/14] ConflictAvoidanceTable refactoring (keeping in mind possible future change of Move/TimedMove to value types). --- ConflictAvoidanceTable.cs | 128 ++++++++++++++------------------------ IndependenceDetection.cs | 28 ++++----- Run.cs | 6 +- WorldState.cs | 22 +++---- 4 files changed, 73 insertions(+), 111 deletions(-) diff --git a/ConflictAvoidanceTable.cs b/ConflictAvoidanceTable.cs index ff51a88..ac65e02 100644 --- a/ConflictAvoidanceTable.cs +++ b/ConflictAvoidanceTable.cs @@ -3,43 +3,46 @@ namespace mapf; +public enum AvoidanceGoal +{ + MINIMIZE_CONFLICTS, + MINIMIZE_CONFLICTING_GROUPS_THEN_CONFLICTS, + MINIMIZE_CONFLICTING_GROUPS, + MINIMIZE_LARGEST_CONFLICTING_GROUP_THEN_NUMBER_OF_SUCH_GROUPS, + MINIMIZE_LARGEST_CONFLICTING_GROUP_THEN_MAXIMIZE_CONFLICT_COUNTS_WITH_OTHERS, + MINIMIZE_CONFLICTING_GROUP_SIZE_AND_COUNT +}; + public class ConflictAvoidanceTable { - Dictionary> timedMovesToAgentNumList; - Dictionary atGoalWaitsToTimeAndAgentNum; // No need for a list of agent nums because goals can't collide :) - public Dictionary agentSizes; - public Dictionary agentConflictCounts; + private Dictionary> _timedMovesToAgentNumList = []; + private Dictionary _atGoalWaitsToTimeAndAgentNum = []; // No need for a list of agent nums because goals can't collide :) - public enum AvoidanceGoal - { - MINIMIZE_CONFLICTS, - MINIMIZE_CONFLICTING_GROUPS_THEN_CONFLICTS, - MINIMIZE_CONFLICTING_GROUPS, - MINIMIZE_LARGEST_CONFLICTING_GROUP_THEN_NUMBER_OF_SUCH_GROUPS, - MINIMIZE_LARGEST_CONFLICTING_GROUP_THEN_MAXIMIZE_CONFLICT_COUNTS_WITH_OTHERS, - MINIMIZE_CONFLICTING_GROUP_SIZE_AND_COUNT - }; - public AvoidanceGoal avoidanceGoal = AvoidanceGoal.MINIMIZE_CONFLICTING_GROUPS_THEN_CONFLICTS; // Better for CBS + public Dictionary AgentSizes { get; } = []; + public Dictionary AgentConflictCounts { get; } = []; + + public int NumPlans { get; private set; } = 0; + + public AvoidanceGoal AvoidanceGoal { get; set; } = AvoidanceGoal.MINIMIZE_CONFLICTING_GROUPS_THEN_CONFLICTS; // Better for CBS public ConflictAvoidanceTable() { - this.Clear(); } public int GetMaxPlanSize() { - if (atGoalWaitsToTimeAndAgentNum.Count > 0) - return atGoalWaitsToTimeAndAgentNum.Values.Max(tuple => tuple.time) - 1; // The first WAIT at goal we record is one time step after reaching it + if (_atGoalWaitsToTimeAndAgentNum.Count > 0) + return _atGoalWaitsToTimeAndAgentNum.Values.Max(tuple => tuple.time) - 1; // The first WAIT at goal we record is one time step after reaching it else return 0; } public void Clear() { - timedMovesToAgentNumList = new Dictionary>(); - atGoalWaitsToTimeAndAgentNum = new Dictionary(); - agentSizes = new Dictionary(); - agentConflictCounts = new Dictionary(); + _timedMovesToAgentNumList.Clear(); + _atGoalWaitsToTimeAndAgentNum.Clear(); + AgentSizes.Clear(); + AgentConflictCounts.Clear(); NumPlans = 0; } @@ -54,22 +57,19 @@ public void AddPlan(SinglePlan plan) step = (TimedMove)temp; else // It's a Move object { - queryTimedMove.Setup(temp, i); - step = queryTimedMove; + step = new TimedMove(temp, i); } - if (this.timedMovesToAgentNumList.ContainsKey(step) == false) + if (_timedMovesToAgentNumList.ContainsKey(step) == false) { - if (ReferenceEquals(step, queryTimedMove)) // Need a separate object that would serve as the key - step = new TimedMove(step); - this.timedMovesToAgentNumList[step] = new List() { plan.AgentNum }; + _timedMovesToAgentNumList[step] = [ plan.AgentNum ]; } else - this.timedMovesToAgentNumList[step].Add(plan.AgentNum); + _timedMovesToAgentNumList[step].Add(plan.AgentNum); } Move lastMove = plan.GetLocationAt(planSize - 1); - var goal = new Move(lastMove.X, lastMove.Y, Direction.Wait); - this.atGoalWaitsToTimeAndAgentNum[goal] = (planSize, plan.AgentNum); + Move goal = new(lastMove.X, lastMove.Y, Direction.Wait); + _atGoalWaitsToTimeAndAgentNum[goal] = (planSize, plan.AgentNum); ++NumPlans; } @@ -84,16 +84,15 @@ public void RemovePlan(SinglePlan plan) step = (TimedMove)temp; else // It's a Move object { - queryTimedMove.Setup(temp, i); - step = queryTimedMove; + step = new TimedMove(temp, i); } - this.timedMovesToAgentNumList[step].Remove(plan.AgentNum); + _timedMovesToAgentNumList[step].Remove(plan.AgentNum); // TODO: Add asserts that check the plan was indeed in the CAT } Move lastMove = plan.GetLocationAt(planSize - 1); - queryMove.Setup(lastMove.X, lastMove.Y, Direction.Wait); - this.atGoalWaitsToTimeAndAgentNum.Remove(queryMove); + Move indexMove = new(lastMove.X, lastMove.Y, Direction.Wait); + _atGoalWaitsToTimeAndAgentNum.Remove(indexMove); --NumPlans; } @@ -109,20 +108,19 @@ public IReadOnlyList this[TimedMove key] get { List ans = null; - if (this.timedMovesToAgentNumList.ContainsKey(key)) + if (_timedMovesToAgentNumList.TryGetValue(key, out List value)) { - ans = new List(this.timedMovesToAgentNumList[key].Count + 1); - ans.AddRange(this.timedMovesToAgentNumList[key]); + ans = new List(value.Count + 1); + ans.AddRange(value); } - queryMove.Setup(key); - if (this.atGoalWaitsToTimeAndAgentNum.ContainsKey(queryMove)) + Move indexMove = new(key.X, key.Y, key.Direction); + if (_atGoalWaitsToTimeAndAgentNum.TryGetValue(indexMove, out (int time, int agentNum) timeAndAgentNum)) { - var timeAndAgentNum = this.atGoalWaitsToTimeAndAgentNum[queryMove]; if (key.Time >= timeAndAgentNum.time) { if (ans == null) - ans = new List() { timeAndAgentNum.agentNum }; + ans = [ timeAndAgentNum.agentNum ]; else ans.Add(timeAndAgentNum.agentNum); } @@ -131,15 +129,10 @@ public IReadOnlyList this[TimedMove key] if (ans != null) return ans; else - return ConflictAvoidanceTable.emptyList; + return []; } } - private static readonly List emptyList = []; - - private Move queryMove = new(); - private TimedMove queryTimedMove = new(); - /// /// Determines whether the read-only dictionary contains an element that has /// the specified key. @@ -150,46 +143,15 @@ public IReadOnlyList this[TimedMove key] /// key is null public bool ContainsKey(TimedMove key) { - if (this.timedMovesToAgentNumList.ContainsKey(key)) + if (_timedMovesToAgentNumList.ContainsKey(key)) return true; - queryMove.Setup(key); - if (this.atGoalWaitsToTimeAndAgentNum.ContainsKey(queryMove)) + Move indexMove = new(key.X, key.Y, key.Direction); + if (_atGoalWaitsToTimeAndAgentNum.TryGetValue(indexMove, out (int time, int agentNum) value)) { - var timeAndAgentNum = this.atGoalWaitsToTimeAndAgentNum[queryMove]; - if (key.Time >= timeAndAgentNum.time) + if (key.Time >= value.time) return true; } return false; } - - /// - /// Gets the value that is associated with the specified key. - /// - /// The key to locate - /// - /// When this method returns, the value associated with the specified key, if - /// the key is found; otherwise, the default value for the type of the value - /// parameter. This parameter is passed uninitialized. - /// - /// key is null - /// - /// true if the object that implements the System.Collections.Generic.IReadOnlyDictionary<TKey,TValue> - /// interface contains an element that has the specified key; otherwise, false. - /// - public bool TryGetValue(TimedMove key, out IReadOnlyList value) - { - if (this.ContainsKey(key)) - { - value = this[key]; - return true; - } - else - { - value = null; - return false; - } - } - - public int NumPlans { get; private set; } } diff --git a/IndependenceDetection.cs b/IndependenceDetection.cs index 4e9a0bc..90cafc9 100644 --- a/IndependenceDetection.cs +++ b/IndependenceDetection.cs @@ -57,13 +57,13 @@ class IndependenceDetection : ISolver private int solutionDepth; private ConflictAvoidanceTable conflictAvoidanceTable; private int maxSolutionCostFound; // FIXME: Maintained but not used - private ConflictAvoidanceTable.AvoidanceGoal avoidanceGoal; + private AvoidanceGoal avoidanceGoal; private bool simple; public IndependenceDetection(IIndependenceDetectionSolver singleAgentSolver, IIndependenceDetectionSolver groupSolver, ConflictChoice conflictChoice = ConflictChoice.MOST_CONFLICTING_SMALLEST_AGENTS, bool provideGroupCostsToSolver = true, - ConflictAvoidanceTable.AvoidanceGoal avoidanceGoal = ConflictAvoidanceTable.AvoidanceGoal.MINIMIZE_CONFLICTING_GROUPS, // The effect of a conflict between two groups is total in ID - they're either fully merged or try to fully avoid each other's plan + AvoidanceGoal avoidanceGoal = AvoidanceGoal.MINIMIZE_CONFLICTING_GROUPS, // The effect of a conflict between two groups is total in ID - they're either fully merged or try to fully avoid each other's plan bool simple = false ) { @@ -94,7 +94,7 @@ public void Setup(ProblemInstance instance, Stopwatch stopwatch) this.totalCost = 0; this.ClearStatistics(); this.conflictAvoidanceTable = new ConflictAvoidanceTable(); - this.conflictAvoidanceTable.avoidanceGoal = this.avoidanceGoal; + this.conflictAvoidanceTable.AvoidanceGoal = this.avoidanceGoal; this.resolutionAttemptedFirstGroup = new HashSet(); this.resolutionAttemptedSecondGroup = new HashSet(); this.allGroups = new LinkedList(); @@ -105,8 +105,8 @@ public void Setup(ProblemInstance instance, Stopwatch stopwatch) this.instance,[ agentStartState ], this.singleAgentSolver, this.groupSolver, this) ); - this.conflictAvoidanceTable.agentSizes[agentStartState.agent.agentNum] = 1; - this.conflictAvoidanceTable.agentConflictCounts[agentStartState.agent.agentNum] = 0; + this.conflictAvoidanceTable.AgentSizes[agentStartState.agent.agentNum] = 1; + this.conflictAvoidanceTable.AgentConflictCounts[agentStartState.agent.agentNum] = 0; } conflictCountsPerGroup = new Dictionary[instance.GetNumOfAgents()]; conflictTimesPerGroup = new Dictionary>[instance.GetNumOfAgents()]; @@ -736,7 +736,7 @@ public bool ImprovedID(Stopwatch stopwatch) UpdateConflictCounts(conflict.group1); conflict.group1.addGroupToCAT(conflictAvoidanceTable); - conflictAvoidanceTable.agentConflictCounts[conflict.group1.groupNum] = conflictCountsPerGroup[conflict.group1.groupNum].Count; + conflictAvoidanceTable.AgentConflictCounts[conflict.group1.groupNum] = conflictCountsPerGroup[conflict.group1.groupNum].Count; ++resolutionSuccesses; continue; @@ -805,7 +805,7 @@ public bool ImprovedID(Stopwatch stopwatch) UpdateConflictCounts(conflict.group2); conflict.group2.addGroupToCAT(conflictAvoidanceTable); - conflictAvoidanceTable.agentConflictCounts[conflict.group2.groupNum] = conflictCountsPerGroup[conflict.group2.groupNum].Count; + conflictAvoidanceTable.AgentConflictCounts[conflict.group2.groupNum] = conflictCountsPerGroup[conflict.group2.groupNum].Count; ++resolutionSuccesses; continue; @@ -854,10 +854,10 @@ public bool ImprovedID(Stopwatch stopwatch) // Remove both groups from avoidance table conflict.group1.removeGroupFromCAT(conflictAvoidanceTable); conflict.group2.removeGroupFromCAT(conflictAvoidanceTable); - conflictAvoidanceTable.agentSizes.Remove(conflict.group1.groupNum); - conflictAvoidanceTable.agentSizes.Remove(conflict.group2.groupNum); - conflictAvoidanceTable.agentConflictCounts.Remove(conflict.group1.groupNum); - conflictAvoidanceTable.agentConflictCounts.Remove(conflict.group2.groupNum); + conflictAvoidanceTable.AgentSizes.Remove(conflict.group1.groupNum); + conflictAvoidanceTable.AgentSizes.Remove(conflict.group2.groupNum); + conflictAvoidanceTable.AgentConflictCounts.Remove(conflict.group1.groupNum); + conflictAvoidanceTable.AgentConflictCounts.Remove(conflict.group2.groupNum); conflictCountsPerGroup[conflict.group1.groupNum] = null; conflictTimesPerGroup[conflict.group1.groupNum] = null; conflictCountsPerGroup[conflict.group2.groupNum] = null; @@ -905,8 +905,8 @@ public bool ImprovedID(Stopwatch stopwatch) // Add the new group to conflict avoidance table compositeGroup.addGroupToCAT(conflictAvoidanceTable); - conflictAvoidanceTable.agentSizes[compositeGroup.groupNum] = compositeGroup.Size(); - conflictAvoidanceTable.agentConflictCounts[compositeGroup.groupNum] = this.conflictCountsPerGroup[compositeGroup.groupNum].Count; + conflictAvoidanceTable.AgentSizes[compositeGroup.groupNum] = compositeGroup.Size(); + conflictAvoidanceTable.AgentConflictCounts[compositeGroup.groupNum] = this.conflictCountsPerGroup[compositeGroup.groupNum].Count; allGroups.AddFirst(compositeGroup); } return true; @@ -1028,7 +1028,7 @@ public bool Solve() // Populate the CAT's agentConflictCounts foreach (var group in this.allGroups) - this.conflictAvoidanceTable.agentConflictCounts[group.groupNum] = group.conflictCounts.Count; + this.conflictAvoidanceTable.AgentConflictCounts[group.groupNum] = group.conflictCounts.Count; CountConflicts(); diff --git a/Run.cs b/Run.cs index 537b174..a02a22e 100644 --- a/Run.cs +++ b/Run.cs @@ -157,9 +157,9 @@ public Run() // Preparing the solvers: solvers = new List(); - solvers.Add(new IndependenceDetection(astar, epea, IndependenceDetection.ConflictChoice.FIRST, true, ConflictAvoidanceTable.AvoidanceGoal.MINIMIZE_CONFLICTS)); // EPEA* + ID - solvers.Add(new IndependenceDetection(astar, epea, IndependenceDetection.ConflictChoice.MOST_CONFLICTING_SMALLEST_RESULTING_GROUP, true, ConflictAvoidanceTable.AvoidanceGoal.MINIMIZE_LARGEST_CONFLICTING_GROUP_THEN_NUMBER_OF_SUCH_GROUPS)); // EPEA* + ID - solvers.Add(new IndependenceDetection(astar, epea, IndependenceDetection.ConflictChoice.LEAST_CONFLICTING_LARGEST_RESULTING_GROUP, true, ConflictAvoidanceTable.AvoidanceGoal.MINIMIZE_CONFLICTS)); // EPEA* + ID + solvers.Add(new IndependenceDetection(astar, epea, IndependenceDetection.ConflictChoice.FIRST, true, AvoidanceGoal.MINIMIZE_CONFLICTS)); // EPEA* + ID + solvers.Add(new IndependenceDetection(astar, epea, IndependenceDetection.ConflictChoice.MOST_CONFLICTING_SMALLEST_RESULTING_GROUP, true, AvoidanceGoal.MINIMIZE_LARGEST_CONFLICTING_GROUP_THEN_NUMBER_OF_SUCH_GROUPS)); // EPEA* + ID + solvers.Add(new IndependenceDetection(astar, epea, IndependenceDetection.ConflictChoice.LEAST_CONFLICTING_LARGEST_RESULTING_GROUP, true, AvoidanceGoal.MINIMIZE_CONFLICTS)); // EPEA* + ID //solvers.Add(new MACBS_WholeTreeThreshold(astar, epea)); // CBS/EPEA* //solvers.Add(new MACBS_WholeTreeThreshold( diff --git a/WorldState.cs b/WorldState.cs index 7b7bb54..283061a 100644 --- a/WorldState.cs +++ b/WorldState.cs @@ -499,17 +499,17 @@ public virtual void IncrementConflictCounts(ConflictAvoidanceTable CAT) this.allAgentsState[i].lastMove.IncrementConflictCounts(CAT, this.conflictCounts, this.conflictTimes); } - if (CAT.avoidanceGoal == ConflictAvoidanceTable.AvoidanceGoal.MINIMIZE_CONFLICTS) // For ID, the original rule + if (CAT.AvoidanceGoal == AvoidanceGoal.MINIMIZE_CONFLICTS) // For ID, the original rule this.primaryTieBreaker = this.conflictCounts.Sum(pair => pair.Value); - else if (CAT.avoidanceGoal == ConflictAvoidanceTable.AvoidanceGoal.MINIMIZE_CONFLICTING_GROUPS) + else if (CAT.AvoidanceGoal == AvoidanceGoal.MINIMIZE_CONFLICTING_GROUPS) this.primaryTieBreaker = this.conflictCounts.Keys.Count; - else if (CAT.avoidanceGoal == ConflictAvoidanceTable.AvoidanceGoal.MINIMIZE_CONFLICTING_GROUPS_THEN_CONFLICTS) + else if (CAT.AvoidanceGoal == AvoidanceGoal.MINIMIZE_CONFLICTING_GROUPS_THEN_CONFLICTS) // For CBS, minimizes the number of conflicting groups and then the number of conflicts with them { this.primaryTieBreaker = this.conflictCounts.Keys.Count; this.secondaryTieBreaker = this.conflictCounts.Sum(pair => pair.Value); } - else if (CAT.avoidanceGoal == ConflictAvoidanceTable.AvoidanceGoal.MINIMIZE_LARGEST_CONFLICTING_GROUP_THEN_NUMBER_OF_SUCH_GROUPS) + else if (CAT.AvoidanceGoal == AvoidanceGoal.MINIMIZE_LARGEST_CONFLICTING_GROUP_THEN_NUMBER_OF_SUCH_GROUPS) // For ID, minimizes the size of the largest group we conflict with and then // the number of conflicting groups with that size. The idea was to minimize conflicts that matter, and conflicts with // non-max-size groups don't. @@ -517,8 +517,8 @@ public virtual void IncrementConflictCounts(ConflictAvoidanceTable CAT) { if (this.conflictCounts.Count != 0) { - this.primaryTieBreaker = this.conflictCounts.Max(pair => CAT.agentSizes[pair.Key]); - this.secondaryTieBreaker = this.conflictCounts.Where(pair => CAT.agentSizes[pair.Key] == this.primaryTieBreaker).Count(); + this.primaryTieBreaker = this.conflictCounts.Max(pair => CAT.AgentSizes[pair.Key]); + this.secondaryTieBreaker = this.conflictCounts.Where(pair => CAT.AgentSizes[pair.Key] == this.primaryTieBreaker).Count(); } else { @@ -526,15 +526,15 @@ public virtual void IncrementConflictCounts(ConflictAvoidanceTable CAT) this.secondaryTieBreaker = 0; } } - else if (CAT.avoidanceGoal == ConflictAvoidanceTable.AvoidanceGoal.MINIMIZE_LARGEST_CONFLICTING_GROUP_THEN_MAXIMIZE_CONFLICT_COUNTS_WITH_OTHERS) + else if (CAT.AvoidanceGoal == AvoidanceGoal.MINIMIZE_LARGEST_CONFLICTING_GROUP_THEN_MAXIMIZE_CONFLICT_COUNTS_WITH_OTHERS) // For ID, minimizes the size of the largest group we conflict with and then // maximizes the number of conflicts the two groups have with other groups { if (this.conflictCounts.Count != 0) { - this.primaryTieBreaker = this.conflictCounts.Max(pair => CAT.agentSizes[pair.Key]); + this.primaryTieBreaker = this.conflictCounts.Max(pair => CAT.AgentSizes[pair.Key]); this.secondaryTieBreaker = -(this.conflictCounts.Sum(pair => pair.Value) + - this.conflictCounts.Where(pair => CAT.agentSizes[pair.Key] == this.primaryTieBreaker).Max(pair => CAT.agentConflictCounts[pair.Key])); + this.conflictCounts.Where(pair => CAT.AgentSizes[pair.Key] == this.primaryTieBreaker).Max(pair => CAT.AgentConflictCounts[pair.Key])); } else { @@ -542,9 +542,9 @@ public virtual void IncrementConflictCounts(ConflictAvoidanceTable CAT) this.secondaryTieBreaker = 0; } } - else if (CAT.avoidanceGoal == ConflictAvoidanceTable.AvoidanceGoal.MINIMIZE_CONFLICTING_GROUP_SIZE_AND_COUNT) + else if (CAT.AvoidanceGoal == AvoidanceGoal.MINIMIZE_CONFLICTING_GROUP_SIZE_AND_COUNT) { - this.primaryTieBreaker = this.conflictCounts.Sum(pair => 1 << (CAT.agentSizes[pair.Key] - 1)); + this.primaryTieBreaker = this.conflictCounts.Sum(pair => 1 << (CAT.AgentSizes[pair.Key] - 1)); } } From 2f4459f2a44b36ba27926ee00f908c2175ed06d7 Mon Sep 17 00:00:00 2001 From: Artem Kliatchkine Date: Wed, 7 May 2025 10:42:47 +0200 Subject: [PATCH 13/14] WorldState code refresh. --- A_Star.cs | 106 ++++++------ A_Star_WithOD.cs | 2 +- AdditivePDBs.cs | 10 +- CbsHeuristicForAStar.cs | 6 +- DynamicRationalLazyOpenList.cs | 2 +- EPEA_Star.cs | 4 +- EnumeratedPDB.cs | 14 +- PDB.cs | 10 +- Plan.cs | 14 +- SumIndividualCosts.cs | 4 +- WorldState.cs | 270 ++++++++++++++----------------- WorldStateForPartialExpansion.cs | 20 +-- WorldStateWithOD.cs | 34 ++-- 13 files changed, 234 insertions(+), 262 deletions(-) diff --git a/A_Star.cs b/A_Star.cs index c1cfda8..da90dc8 100644 --- a/A_Star.cs +++ b/A_Star.cs @@ -112,7 +112,7 @@ public virtual void Setup(ProblemInstance problemInstance, int minDepth, Stopwat this.closedList.Add(root, root); this.ClearPrivateStatistics(); this.generated++; // The root - root.generated = generated; + root.Generated = generated; this.totalCost = 0; this.singleCosts = null; this.solution = null; @@ -139,8 +139,8 @@ public virtual void Setup(ProblemInstance problemInstance, int minDepth, Stopwat if (this.mstar) { - root.backPropagationSet = new HashSet(); - root.collisionSets = new DisjointSets(); + root.BackPropagationSet = new HashSet(); + root.CollisionSets = new DisjointSets(); this.mstarBackPropagationConflictList = []; } @@ -454,7 +454,7 @@ public virtual bool Solve() // application of the expensive heuristic is skipped altogether. // This can cause decreasing F values. this.openList is not DynamicRationalLazyOpenList && - currentNode.minGoalCost == -1 // If we were given a minGoalCost (by ID, for example), then at some point we're going to use up the boost to the h-value we gave the root + currentNode.MinGoalCost == -1 // If we were given a minGoalCost (by ID, for example), then at some point we're going to use up the boost to the h-value we gave the root ) if (currentNode.F < lastF) Trace.Assert(false, $"A* node with decreasing F: {currentNode.F} < {lastF}."); @@ -466,7 +466,7 @@ this.openList is not DynamicRationalLazyOpenList && lastNode = currentNode; // Calculate expansion delay - int expansionDelay = this.expanded - currentNode.expandedCountWhenGenerated - 1; // -1 to make the delay zero when a node is expanded immediately after being generated. + int expansionDelay = this.expanded - currentNode.ExpandedCountWhenGenerated - 1; // -1 to make the delay zero when a node is expanded immediately after being generated. maxExpansionDelay = Math.Max(maxExpansionDelay, expansionDelay); // Check if node is the goal, or knows how to get to it @@ -476,8 +476,8 @@ this.openList is not DynamicRationalLazyOpenList && this.singleCosts = currentNode.GetSingleCosts(); this.solution = currentNode.GetPlan(); this.singlePlans = currentNode.GetSinglePlans(); - this.conflictCounts = currentNode.conflictCounts; - this.conflictTimes = currentNode.conflictTimes; + this.conflictCounts = currentNode.ConflictCounts; + this.conflictTimes = currentNode.ConflictTimes; this.solutionDepth = this.totalCost - initialEstimate; this.Clear(); return true; @@ -494,9 +494,9 @@ this.openList is not DynamicRationalLazyOpenList && // TODO: Need to clear individual agent planned moves, in case this node was reopened with an updated collision set. if (this.debug) { - Console.Write("with collision sets: {0}", currentNode.collisionSets); + Console.Write("with collision sets: {0}", currentNode.CollisionSets); } - var sets = currentNode.collisionSets.GetSets(); + var sets = currentNode.CollisionSets.GetSets(); foreach (var set in sets) { if (set.Count != 0) @@ -550,18 +550,18 @@ public virtual void Expand(WorldState node) { if (stopwatch.ElapsedMilliseconds > Constants.MAX_TIME) return; - currentNode.makespan++; + currentNode.Makespan++; currentNode.CalculateG(); currentNode.H = (int)this.heuristic.h(currentNode); // Boost h based on minGoalCost - if (currentNode.G < currentNode.minGoalCost) + if (currentNode.G < currentNode.MinGoalCost) { if (currentNode.H == 0) // Agent is at the goal, only too early currentNode.H = 2; // Otherwise waiting at goal would expand to waiting at the goal for the same too low cost, // which would expand to waiting at the goal, etc. // +2 because you need a step out of the goal and another step into it. - currentNode.H = Math.Max(currentNode.H, currentNode.minGoalCost - currentNode.G); // Like a Manhattan Distance on the time dimension + currentNode.H = Math.Max(currentNode.H, currentNode.MinGoalCost - currentNode.G); // Like a Manhattan Distance on the time dimension // TODO: Add a statistic for when the H was increased thanks to the minGoalCost } @@ -628,23 +628,23 @@ protected virtual List ExpandOneAgent(List intermediateN if (stopwatch.ElapsedMilliseconds > Constants.MAX_TIME) break; - if (currentNode.mddNode == null) + if (currentNode.MDDNode == null) { // Try all legal moves of the agents - foreach (TimedMove potentialMove in currentNode.allAgentsState[agentIndex].lastMove.GetNextMoves()) + foreach (TimedMove potentialMove in currentNode.AllAgentsState[agentIndex].lastMove.GetNextMoves()) { - WorldState origNode = agentIndex == 0 ? currentNode : currentNode.prevStep; + WorldState origNode = agentIndex == 0 ? currentNode : currentNode.PrevStep; //moveIsValid = this.IsValid(potentialMove, currentNode.currentMoves, currentNode.makespan + 1, agentIndex, origNode, currentNode); //if (moveIsValid == false) // continue; //----------------Begin pasting isValid method TimedMove possibleMove = potentialMove; - IReadOnlyDictionary currentMoves = currentNode.currentMoves; // When agentIndex == 0, this is null (nullified when generating it was done. - int makespan = currentNode.makespan + 1; + IReadOnlyDictionary currentMoves = currentNode.CurrentMoves; // When agentIndex == 0, this is null (nullified when generating it was done. + int makespan = currentNode.Makespan + 1; WorldState fromNode = origNode; WorldState intermediateMode = currentNode; - int agentNum = fromNode.allAgentsState[agentIndex].agent.agentNum; + int agentNum = fromNode.AllAgentsState[agentIndex].agent.agentNum; // Check if the proposed move is reserved in the plan of another agent. // This is used in IndependenceDetection's ImprovedID. @@ -681,7 +681,7 @@ protected virtual List ExpandOneAgent(List intermediateN if (this.mstar) { - bool agentInCollisionSet = fromNode.collisionSets.IsSingle(agentIndex); + bool agentInCollisionSet = fromNode.CollisionSets.IsSingle(agentIndex); if (!agentInCollisionSet) // Only one move allowed { @@ -690,7 +690,7 @@ protected virtual List ExpandOneAgent(List intermediateN if (hasPlan) { // If this move isn't its individually optimal one according to its planned route, return false. - if (!this.instance.GetSingleAgentOptimalMove(fromNode.allAgentsState[agentIndex]).Equals(possibleMove)) + if (!this.instance.GetSingleAgentOptimalMove(fromNode.AllAgentsState[agentIndex]).Equals(possibleMove)) continue; } } @@ -708,7 +708,7 @@ protected virtual List ExpandOneAgent(List intermediateN // Arbitrarily choosing the first colliding agent: int collidingAgentIndex = collidingWith[0]; - bool otherAgentInColSet = fromNode.collisionSets.IsSingle(collidingAgentIndex); + bool otherAgentInColSet = fromNode.CollisionSets.IsSingle(collidingAgentIndex); // Check if one of the colliding agents isn't in the collision set yet if (!agentInCollisionSet || !otherAgentInColSet) @@ -718,7 +718,7 @@ protected virtual List ExpandOneAgent(List intermediateN bool success = false; var conflict = new CbsConflict( agentIndex, collidingAgentIndex, possibleMove, - intermediateMode.allAgentsState[collidingAgentIndex].lastMove, makespan); + intermediateMode.AllAgentsState[collidingAgentIndex].lastMove, makespan); if (this.debug) Debug.WriteLine(conflict.ToString()); if (!success) @@ -739,27 +739,27 @@ protected virtual List ExpandOneAgent(List intermediateN //----------------end paste from isValid WorldState childNode = CreateSearchNode(currentNode); - childNode.allAgentsState[agentIndex].MoveTo(potentialMove); + childNode.AllAgentsState[agentIndex].MoveTo(potentialMove); - if (agentIndex < currentNode.allAgentsState.Length - 1) // More agents need to move - childNode.currentMoves.Add(childNode.allAgentsState[agentIndex].lastMove, agentIndex); + if (agentIndex < currentNode.AllAgentsState.Length - 1) // More agents need to move + childNode.CurrentMoves.Add(childNode.AllAgentsState[agentIndex].lastMove, agentIndex); else // Moved the last agent - childNode.currentMoves = null; // To reduce memory load and lookup times + childNode.CurrentMoves = null; // To reduce memory load and lookup times // Set the node's prevStep to its real parent, skipping over the intermediate nodes. if (agentIndex != 0) - childNode.prevStep = currentNode.prevStep; + childNode.PrevStep = currentNode.PrevStep; GeneratedNodes.Add(childNode); } } else { - foreach (MDDNode childMddNode in currentNode.mddNode.children) + foreach (MDDNode childMddNode in currentNode.MDDNode.children) { WorldState childNode = CreateSearchNode(currentNode); - childNode.allAgentsState[agentIndex].MoveTo(childMddNode.move); - childNode.mddNode = childMddNode; + childNode.AllAgentsState[agentIndex].MoveTo(childMddNode.move); + childNode.MDDNode = childMddNode; // No need to set the node's prevStep because we're dealing with a single agent - // no intermediate nodes. // TODO: Add that support @@ -801,7 +801,7 @@ protected virtual bool IsValid(TimedMove possibleMove, IReadOnlyDictionary currentMoves, int makespan, int agentIndex, WorldState fromNode, WorldState intermediateMode) { - int agentNum = fromNode.allAgentsState[agentIndex].agent.agentNum; + int agentNum = fromNode.AllAgentsState[agentIndex].agent.agentNum; // Check if the proposed move is reserved in the plan of another agent. // This is used in IndependenceDetection's ImprovedID. @@ -838,7 +838,7 @@ protected virtual bool IsValid(TimedMove possibleMove, if (this.mstar) { - bool agentInCollisionSet = fromNode.collisionSets.IsSingle(agentIndex); + bool agentInCollisionSet = fromNode.CollisionSets.IsSingle(agentIndex); //fromNode.currentCollisionSet.Contains(agentIndex);// || //(fromNode.individualMStarPlanBases[agentIndex] != null && // this.mstarPlanBasesToTheirPlans[fromNode.individualMStarPlanBases[agentIndex]][agentIndex] == null); // Parent plan was abandoned. Imagine a backpropagation happened. @@ -879,7 +879,7 @@ protected virtual bool IsValid(TimedMove possibleMove, //Move allowed = plan.GetLocationAt(fromNode.individualMStarBookmarks[agentIndex] + 1); //if (possibleMove.Equals(allowed) == false) // return false; - if (!this.instance.GetSingleAgentOptimalMove(fromNode.allAgentsState[agentIndex]).Equals(possibleMove)) + if (!this.instance.GetSingleAgentOptimalMove(fromNode.AllAgentsState[agentIndex]).Equals(possibleMove)) return false; } } @@ -896,7 +896,7 @@ protected virtual bool IsValid(TimedMove possibleMove, // Arbitrarily choosing the first colliding agent: int collidingAgentIndex = collidingWith[0]; - bool otherAgentInColSet = fromNode.collisionSets.IsSingle(collidingAgentIndex); + bool otherAgentInColSet = fromNode.CollisionSets.IsSingle(collidingAgentIndex); //fromNode.currentCollisionSet.Contains(collidingAgentIndex);// || //(fromNode.individualMStarPlanBases[collidingAgentIndex] != null && //this.mstarPlanBasesToTheirPlans[fromNode.individualMStarPlanBases[collidingAgentIndex]][collidingAgentIndex] == null); // Parent plan was abandoned; @@ -910,7 +910,7 @@ protected virtual bool IsValid(TimedMove possibleMove, bool success = false; var conflict = new CbsConflict( agentIndex, collidingAgentIndex, possibleMove, - intermediateMode.allAgentsState[collidingAgentIndex].lastMove, makespan); + intermediateMode.AllAgentsState[collidingAgentIndex].lastMove, makespan); if (this.debug) Debug.WriteLine(conflict.ToString()); //if (this.doMstarShuffle && agentInCollisionSet == false) @@ -1059,10 +1059,10 @@ protected virtual bool ProcessGeneratedNode(WorldState currentNode) { // Accumulating the conflicts count from parent to child // We're counting conflicts along the entire path, so the parent's conflicts count is added to the child's: - currentNode.conflictCounts = new Dictionary(currentNode.prevStep.conflictCounts); - currentNode.conflictTimes = []; - foreach (var kvp in currentNode.prevStep.conflictTimes) - currentNode.conflictTimes[kvp.Key] = [.. kvp.Value]; + currentNode.ConflictCounts = new Dictionary(currentNode.PrevStep.ConflictCounts); + currentNode.ConflictTimes = []; + foreach (var kvp in currentNode.PrevStep.ConflictTimes) + currentNode.ConflictTimes[kvp.Key] = [.. kvp.Value]; currentNode.IncrementConflictCounts(this.CAT); // We're counting conflicts along the entire path, so the parent's conflicts count // is added to the child's. @@ -1072,12 +1072,12 @@ protected virtual bool ProcessGeneratedNode(WorldState currentNode) { //currentNode.currentCollisionSet = null; // Clear expansion data - currentNode.backPropagationSet = new HashSet + currentNode.BackPropagationSet = new HashSet { - currentNode.prevStep + currentNode.PrevStep }; - currentNode.collisionSets = new DisjointSets(); + currentNode.CollisionSets = new DisjointSets(); // Copy parent's individual agent plans //currentNode.individualMStarPlans = currentNode.prevStep.individualMStarPlans.ToArray(); @@ -1099,9 +1099,9 @@ protected virtual bool ProcessGeneratedNode(WorldState currentNode) { // Unite backpropagation sets and collision sets of inClosedList and currentNode. // Notice only only of them is going to survive this method. - foreach (WorldState parent in currentNode.backPropagationSet) // Only one node expected on the backprop list - currentNode's parent. TODO: Assert this? - inClosedList.backPropagationSet.Add(parent); - currentNode.backPropagationSet = inClosedList.backPropagationSet; + foreach (WorldState parent in currentNode.BackPropagationSet) // Only one node expected on the backprop list - currentNode's parent. TODO: Assert this? + inClosedList.BackPropagationSet.Add(parent); + currentNode.BackPropagationSet = inClosedList.BackPropagationSet; //// Copy relavant individual paths //for (int i = 0; i < this.instance.GetNumOfAgents(); i++) @@ -1117,8 +1117,8 @@ protected virtual bool ProcessGeneratedNode(WorldState currentNode) } //inClosedList.collisionSets.CopyUnions(currentNode.collisionSets); // Not necessary - currentNode has no unions yet - currentNode.collisionSets = inClosedList.collisionSets; - RMStarCollisionBackPropagation(currentNode.collisionSets, currentNode.prevStep); // The collision sets are information about the future, + currentNode.CollisionSets = inClosedList.CollisionSets; + RMStarCollisionBackPropagation(currentNode.CollisionSets, currentNode.PrevStep); // The collision sets are information about the future, // not the past, so they should } @@ -1190,9 +1190,9 @@ protected virtual bool ProcessGeneratedNode(WorldState currentNode) { this.closedList.Add(currentNode, currentNode); this.generated++; // Reopened nodes are also recounted here. - currentNode.generated = this.generated; + currentNode.Generated = this.generated; this.openList.Add(currentNode); - currentNode.expandedCountWhenGenerated = this.expanded; + currentNode.ExpandedCountWhenGenerated = this.expanded; if (this.debug) Debug.WriteLine($"Generated node {currentNode}"); return true; @@ -1329,7 +1329,7 @@ void RMStarCollisionBackPropagation(CbsConflict conflict, WorldState fromNode) { var node = queue.Dequeue(); - bool onlyUnitedNow = node.collisionSets.Union(conflict.agentAIndex, conflict.agentBIndex); + bool onlyUnitedNow = node.CollisionSets.Union(conflict.agentAIndex, conflict.agentBIndex); if (onlyUnitedNow) { @@ -1337,7 +1337,7 @@ void RMStarCollisionBackPropagation(CbsConflict conflict, WorldState fromNode) Console.WriteLine("Re-opening node {0} with an updated collision set", node); this.reinsertIntoOpenList(node); - foreach (var next in node.backPropagationSet) + foreach (var next in node.BackPropagationSet) queue.Enqueue(next); } } @@ -1359,7 +1359,7 @@ void RMStarCollisionBackPropagation(DisjointSets colSets, WorldState fromNo { var node = queue.Dequeue(); - bool onlyUnitedNow = node.collisionSets.CopyUnions(colSets); + bool onlyUnitedNow = node.CollisionSets.CopyUnions(colSets); if (onlyUnitedNow) { @@ -1367,7 +1367,7 @@ void RMStarCollisionBackPropagation(DisjointSets colSets, WorldState fromNo Console.WriteLine("Re-opening node {0} with an updated collision set", node); this.reinsertIntoOpenList(node); - foreach (var next in node.backPropagationSet) + foreach (var next in node.BackPropagationSet) queue.Enqueue(next); } } diff --git a/A_Star_WithOD.cs b/A_Star_WithOD.cs index d17ce4a..41946cd 100644 --- a/A_Star_WithOD.cs +++ b/A_Star_WithOD.cs @@ -85,7 +85,7 @@ protected override List ExpandOneAgent(List intermediate // Makespan increases only if this is the move of the first agent. This makes sure that under a makespan // cost function, partial nodes have a correct cost and can even serve as goal nodes. if (parent.agentTurn != 0) - childNode.makespan--; // Cancel the increment in base + childNode.Makespan--; // Cancel the increment in base } this.alreadyExpanded = true; diff --git a/AdditivePDBs.cs b/AdditivePDBs.cs index 4ea1a05..dbabcb7 100644 --- a/AdditivePDBs.cs +++ b/AdditivePDBs.cs @@ -32,9 +32,9 @@ public void build(ProblemInstance pi, WorldState s) // with the first two, then the second two, etc. PDBs = new List(); - if (s.allAgentsState.Length > 1) + if (s.AllAgentsState.Length > 1) { - for (uint i = 0; i < s.allAgentsState.Length - 1; i += 2) + for (uint i = 0; i < s.AllAgentsState.Length - 1; i += 2) { // Make a list of agents we want to include together in the // next additive pattern database. We specify agents by @@ -50,7 +50,7 @@ public void build(ProblemInstance pi, WorldState s) // node. This is done by passing into the state copy // constructor our list of important agents. - WorldState tws = new WorldState(s.allAgentsState, agentsToConsider); + WorldState tws = new WorldState(s.AllAgentsState, agentsToConsider); // Initialize, build, and save the new pattern database. @@ -65,11 +65,11 @@ public void build(ProblemInstance pi, WorldState s) // Create single shortest path pattern database heuristics for the // remaining agents if we have any left over. - if (s.allAgentsState.Length % 2 == 1) + if (s.AllAgentsState.Length % 2 == 1) { SumIndividualCosts pdb = new SumIndividualCosts(); List agentsToConsider = new List(1); - agentsToConsider.Add((uint) s.allAgentsState.Length - 1); + agentsToConsider.Add((uint) s.AllAgentsState.Length - 1); pdb.Init(pi, agentsToConsider); pdb.build(); PDBs.Add(pdb); diff --git a/CbsHeuristicForAStar.cs b/CbsHeuristicForAStar.cs index 939711c..5dcf7c4 100644 --- a/CbsHeuristicForAStar.cs +++ b/CbsHeuristicForAStar.cs @@ -103,9 +103,9 @@ protected uint h(WorldState s, int targetCost, int sicEstimate=-1, int lowLevelG (ProblemInstance problemInstance, ISet positiveConstraints) = s.ToProblemInstance(this.instance); sAsProblemInstance = problemInstance; this.cbs.Setup(sAsProblemInstance, - Math.Max(s.makespan, // This forces must-constraints to be upheld when dealing with A*+OD nodes, + Math.Max(s.Makespan, // This forces must-constraints to be upheld when dealing with A*+OD nodes, // at the cost of forcing every agent to move when a goal could be found earlier with all must constraints upheld. - s.minGoalTimeStep), // No point in finding shallower goal nodes + s.MinGoalTimeStep), // No point in finding shallower goal nodes this.stopwatch, null, null, positiveConstraints); if (this.cbs.OpenList.Count > 0 && this.cbs.ExternalCAT == null) @@ -200,7 +200,7 @@ protected uint h(WorldState s, int targetCost, int sicEstimate=-1, int lowLevelG heuristic.Init(this.instance, this.agentsToConsider); var epeastarsic = new EPEA_Star(heuristic); - epeastarsic.Setup(sAsProblemInstance, s.makespan, stopwatch); + epeastarsic.Setup(sAsProblemInstance, s.Makespan, stopwatch); bool epeastarsicSolved = epeastarsic.Solve(); if (epeastarsicSolved) Trace.Assert(epeastarsic.totalCost - s.G >= this.cbs.SolutionCost - s.G, "Inadmissible!!"); diff --git a/DynamicRationalLazyOpenList.cs b/DynamicRationalLazyOpenList.cs index 9e65292..606b93c 100644 --- a/DynamicRationalLazyOpenList.cs +++ b/DynamicRationalLazyOpenList.cs @@ -83,7 +83,7 @@ public override WorldState Remove() const double binaryHeapTau = 0.073359375; // microseconds. From empirical experiments with this infra on my computer. double logN = Math.Log(this.heap.Count, 2); // Removals from and insertions to the queue cost practically zero. double t0 = binaryHeapTau * logN; // TODO: Measure this directly? - double overhead = 0.023 * this.Peek().allAgentsState.Length; // in milliseconds. Empirical lowest estimate. The cost of a zero-timeout CBSH run wasn't simply linear with the number of agents for some reason. + double overhead = 0.023 * this.Peek().AllAgentsState.Length; // in milliseconds. Empirical lowest estimate. The cost of a zero-timeout CBSH run wasn't simply linear with the number of agents for some reason. while (true) { diff --git a/EPEA_Star.cs b/EPEA_Star.cs index e4db9b7..532401a 100644 --- a/EPEA_Star.cs +++ b/EPEA_Star.cs @@ -102,7 +102,7 @@ public override void Expand(WorldState nodeP) node.targetDeltaF++; node.remainingDeltaF = node.targetDeltaF; // Just for the following hasChildrenForCurrentDeltaF call. } while (node.hasMoreChildren() && node.hasChildrenForCurrentDeltaF() == false); - } while (node.hasMoreChildren() && node.G + node.sic + node.targetDeltaF <= node.minGoalCost); // Generate more children immediately if we have a lower bound on the solution depth + } while (node.hasMoreChildren() && node.G + node.sic + node.targetDeltaF <= node.MinGoalCost); // Generate more children immediately if we have a lower bound on the solution depth if (node.hasMoreChildren() && node.hasChildrenForCurrentDeltaF() && node.G + node.sic + node.targetDeltaF <= this.maxSolutionCost) { @@ -118,7 +118,7 @@ public override void Expand(WorldState nodeP) openList.Add(node); if (this.debug) { - Debug.WriteLine($"Re-inserting node {node.generated} into the open list (with targetDeltaF: {node.targetDeltaF})"); + Debug.WriteLine($"Re-inserting node {node.Generated} into the open list (with targetDeltaF: {node.targetDeltaF})"); Debug.WriteLine(""); } } diff --git a/EnumeratedPDB.cs b/EnumeratedPDB.cs index 6d077fa..16cb63d 100644 --- a/EnumeratedPDB.cs +++ b/EnumeratedPDB.cs @@ -169,11 +169,11 @@ public override void build() // our state already is a projection. WorldState goal = new WorldState(problem.Agents, agentsToConsider); - foreach (AgentState ags in goal.allAgentsState) + foreach (AgentState ags in goal.AllAgentsState) ags.SwapCurrentWithGoal(); List vBackup = agentsToConsider; - agentsToConsider = new List(goal.allAgentsState.Length); - for (uint i = 0; i < goal.allAgentsState.Length; ++i) + agentsToConsider = new List(goal.AllAgentsState.Length); + for (uint i = 0; i < goal.AllAgentsState.Length; ++i) agentsToConsider.Add(i); // Initialize variables and insert the root node into our queue. We @@ -224,7 +224,7 @@ public override void build() if (offsetFromSingleShortestPath) { int nSingleAgentShortestPath = 0; - foreach (var a in i.allAgentsState) + foreach (var a in i.AllAgentsState) nSingleAgentShortestPath += this.problem.GetSingleAgentOptimalCost(a); int nDifference = i.G - nSingleAgentShortestPath; Trace.Assert(nDifference >= 0); @@ -262,7 +262,7 @@ public override uint h(WorldState s) foreach (var a in agentsToConsider) { nSingleAgentShortestPath += - this.problem.GetSingleAgentOptimalCost(s.allAgentsState[a]); + this.problem.GetSingleAgentOptimalCost(s.AllAgentsState[a]); } return (table[hash(s)] + (uint) nSingleAgentShortestPath); } @@ -297,11 +297,11 @@ uint hash(WorldState s) // us to keep figure out how many other agents have been placed // in positions previous to our current position. - int card1 = problem.GetCardinality(s.allAgentsState[agentsToConsider[i]].lastMove); + int card1 = problem.GetCardinality(s.AllAgentsState[agentsToConsider[i]].lastMove); int preceding = 0; for (int j = 0; j < i; ++j) { - int nCard2 = problem.GetCardinality(s.allAgentsState[agentsToConsider[j]].lastMove); + int nCard2 = problem.GetCardinality(s.AllAgentsState[agentsToConsider[j]].lastMove); if (nCard2 < card1) ++preceding; } diff --git a/PDB.cs b/PDB.cs index 18c432c..bb18cd6 100644 --- a/PDB.cs +++ b/PDB.cs @@ -84,7 +84,7 @@ public void Expand(WorldState currentNode, ICollection children) /// A collection of moves performed by the previous agents in this time step (needed to verify that no collisions occur) public void Expand(WorldState currentNode, int agentIndex, ICollection children, ICollection previousMoves) { - WorldState prev = currentNode.prevStep; + WorldState prev = currentNode.PrevStep; WorldState childNode; if (agentIndex == 0) // If this is the first agent that moves @@ -93,21 +93,21 @@ public void Expand(WorldState currentNode, int agentIndex, ICollection agentMoves = currentNode.GetAgentsMoves(); _locationsAtTimes.Add(agentMoves); - currentNode = currentNode.prevStep; + currentNode = currentNode.PrevStep; } _locationsAtTimes.Reverse(); } @@ -289,13 +289,13 @@ public class SinglePlan /// public SinglePlan(WorldState goalState, int agentIndex) { - AgentNum = goalState.allAgentsState[agentIndex].agent.agentNum; + AgentNum = goalState.AllAgentsState[agentIndex].agent.agentNum; WorldState currentNode = goalState; LinkedList locations = []; while (currentNode != null) { locations.AddFirst(currentNode.GetSingleAgentMove(agentIndex)); - currentNode = currentNode.prevStep; + currentNode = currentNode.PrevStep; } LocationAtTimes = [.. locations]; } @@ -499,7 +499,7 @@ public override string ToString() public static SinglePlan[] GetSinglePlans(WorldState goalState) // FIXME: Duplication with other methods. { - List[] allroutes = new List[goalState.allAgentsState.Length]; + List[] allroutes = new List[goalState.AllAgentsState.Length]; for (int i = 0; i < allroutes.Length; i++) allroutes[i] = []; @@ -508,14 +508,14 @@ public static SinglePlan[] GetSinglePlans(WorldState goalState) // FIXME: Duplic { for (int i = 0; i < allroutes.Length; i++) allroutes[i].Add(currentNode.GetSingleAgentMove(i)); - currentNode = currentNode.prevStep; + currentNode = currentNode.PrevStep; } - SinglePlan[] ans = new SinglePlan[goalState.allAgentsState.Length]; + SinglePlan[] ans = new SinglePlan[goalState.AllAgentsState.Length]; for (int i = 0; i < ans.Length; i++) { allroutes[i].Reverse(); - ans[i] = new SinglePlan(allroutes[i], goalState.allAgentsState[i].agent.agentNum); + ans[i] = new SinglePlan(allroutes[i], goalState.AllAgentsState[i].agent.agentNum); } return ans; } diff --git a/SumIndividualCosts.cs b/SumIndividualCosts.cs index 5261338..61302bb 100644 --- a/SumIndividualCosts.cs +++ b/SumIndividualCosts.cs @@ -41,7 +41,7 @@ public override uint h(WorldState s) public static uint h(WorldState s, ProblemInstance instance) { uint nHeuristic = 0; - foreach (AgentState state in s.allAgentsState) + foreach (AgentState state in s.AllAgentsState) { nHeuristic += (uint)instance.GetSingleAgentOptimalCost(state); } @@ -122,7 +122,7 @@ public static uint h(WorldState s, ProblemInstance instance) int i = 0; // TODO: Consider adding another extension function like IndexOfMax that also returns the max // to replace the loop below. Though, it would make debugging less convenient. - foreach (AgentState state in s.allAgentsState) + foreach (AgentState state in s.AllAgentsState) { uint heuristic = (uint)instance.GetSingleAgentOptimalCost(state); if (heuristic > maxHeuristic) diff --git a/WorldState.cs b/WorldState.cs index 283061a..242f954 100644 --- a/WorldState.cs +++ b/WorldState.cs @@ -10,49 +10,49 @@ namespace mapf; /// public class WorldState : IComparable, IBinaryHeapItem, IHeuristicSearchNode { - public int makespan; // Total time steps passed, max(agent makespans) + public int Makespan { get; set; } // Total time steps passed, max(agent makespans) public int G { get; set; } // Value depends on Constants.costFunction and Constants.sumOfCostsVariant, Sum of agent makespans until they reach their goal public int H { get; set; } public int HBonus { get; set; } - public AgentState[] allAgentsState; - public WorldState prevStep; - private int binaryHeapIndex; - public MDDNode mddNode; - public int generated; - - public int primaryTieBreaker; - public int secondaryTieBreaker; + public AgentState[] AllAgentsState { get; private set; } + public WorldState PrevStep { get; set; } + private int _binaryHeapIndex; + public MDDNode MDDNode { get; set; } + public int Generated { get; set; } + + protected int _primaryTieBreaker; + private int _secondaryTieBreaker; /// /// Maps from agent num to the number of times the path up to this node collides with that agent /// - public Dictionary conflictCounts; + public Dictionary ConflictCounts { get; set; } /// /// Maps from agent num to a list of the conflict times with it /// - public Dictionary> conflictTimes; + public Dictionary> ConflictTimes { get; set; } /// /// The min depth (makespan) from which a node may be considered a goal. /// TODO: Consider moving out of the node object to a static variable or something. /// It doesn't change between nodes. /// - public int minGoalTimeStep; + public int MinGoalTimeStep { get; private set; } /// /// The min cost (g) from which a node may be considered a goal. /// TODO: Consider moving out of the node object to a static variable or something. /// It doesn't change between nodes. /// - public int minGoalCost; + public int MinGoalCost { get; private set; } /// /// The last move of all agents that have already moved in this turn. /// Used for making sure the next agent move doesn't collide with moves already made. /// Used while generating this node, nullified when done. /// - public Dictionary currentMoves; - protected static readonly int NOT_SET = -1; + public Dictionary CurrentMoves { get; set; } + private const int NOT_SET = -1; /// /// For computing expansion delay /// - public int expandedCountWhenGenerated; + public int ExpandedCountWhenGenerated { get; set; } ///// ///// For lazy heuristics ///// @@ -61,10 +61,9 @@ public class WorldState : IComparable, IBinaryHeapItem, IHeuris /// For MStar. /// Disjoint sets of agent indices, since only internal agents are considered. /// - public DisjointSets collisionSets; + public DisjointSets CollisionSets { get; set; } //public ISet currentCollisionSet; - public ISet backPropagationSet; - public TimedMove[] plannedMoves; + public ISet BackPropagationSet { get; set; } /// /// Create a state with the given state for every agent. @@ -75,22 +74,22 @@ public class WorldState : IComparable, IBinaryHeapItem, IHeuris /// public WorldState(AgentState[] allAgentsState, int minDepth = -1, int minCost = -1, MDDNode mddNode = null) { - this.allAgentsState = allAgentsState.ToArray(); - this.makespan = allAgentsState.Max(state => state.lastMove.Time); // We expect to only find at most two G values within the agent group - this.CalculateG(); // G not necessarily zero when solving a partially solved problem. - this.primaryTieBreaker = 0; - this.secondaryTieBreaker = 0; - this.conflictCounts = new Dictionary(); // Unused if not running under CBS, and we can't tell at this point easily - this.conflictTimes = new Dictionary>(); // Unused if not running under CBS, and we can't tell at this point easily - this.minGoalTimeStep = minDepth; - this.minGoalCost = minCost; + AllAgentsState = [.. allAgentsState]; + Makespan = allAgentsState.Max(state => state.lastMove.Time); // We expect to only find at most two G values within the agent group + CalculateG(); // G not necessarily zero when solving a partially solved problem. + _primaryTieBreaker = 0; + _secondaryTieBreaker = 0; + ConflictCounts = []; // Unused if not running under CBS, and we can't tell at this point easily + ConflictTimes = []; // Unused if not running under CBS, and we can't tell at this point easily + MinGoalTimeStep = minDepth; + MinGoalCost = minCost; if (mddNode == null) - this.currentMoves = new Dictionary(); - this.goalCost = NOT_SET; - this.goalSingleCosts = null; - this.singlePlans = null; - this.HBonus = 0; - this.mddNode = mddNode; + CurrentMoves = []; + goalCost = NOT_SET; + goalSingleCosts = null; + singlePlans = null; + HBonus = 0; + MDDNode = mddNode; } /// @@ -99,30 +98,30 @@ public WorldState(AgentState[] allAgentsState, int minDepth = -1, int minCost = /// public WorldState(WorldState cpy) { - this.makespan = cpy.makespan; - this.G = cpy.G; - this.H = cpy.H; + Makespan = cpy.Makespan; + G = cpy.G; + H = cpy.H; // The conflictTimes, conflictCounts and sumConflictCounts are only copied later if necessary. - this.minGoalTimeStep = cpy.minGoalTimeStep; - this.minGoalCost = cpy.minGoalCost; - this.allAgentsState = new AgentState[cpy.allAgentsState.Length]; - for (int i = 0; i < allAgentsState.Length; i++) + MinGoalTimeStep = cpy.MinGoalTimeStep; + MinGoalCost = cpy.MinGoalCost; + AllAgentsState = new AgentState[cpy.AllAgentsState.Length]; + for (int i = 0; i < AllAgentsState.Length; i++) { - this.allAgentsState[i] = new AgentState(cpy.allAgentsState[i]); + AllAgentsState[i] = new AgentState(cpy.AllAgentsState[i]); // Shallow copy - it's still the same lastMove inside the AgentState, until we set a new lastMove there. } - if (cpy.currentMoves != null) + if (cpy.CurrentMoves != null) // cpy is an intermediate node - this.currentMoves = new Dictionary(dictionary: cpy.currentMoves); + CurrentMoves = new Dictionary(dictionary: cpy.CurrentMoves); else // cpy is a concrete node - this.currentMoves = new Dictionary(capacity: cpy.allAgentsState.Length); - this.goalCost = NOT_SET; - this.goalSingleCosts = null; - this.singlePlans = null; - this.HBonus = 0; - this.mddNode = cpy.mddNode; - this.prevStep = cpy; + CurrentMoves = new Dictionary(capacity: cpy.AllAgentsState.Length); + goalCost = NOT_SET; + goalSingleCosts = null; + singlePlans = null; + HBonus = 0; + MDDNode = cpy.MDDNode; + PrevStep = cpy; } /// @@ -146,19 +145,19 @@ public bool GoalTest() { // Check if this is a generalised goal node and its plan is long enough. // If we know the optimal solution, it doesn't matter if this is a real goal node or not, we can finish. - if (this.singlePlans != null) + if (singlePlans != null) { // Check if plans are long enough and costly enough - if (this.singlePlans.All(plan => plan.GetSize() - 1 >= this.minGoalTimeStep)) + if (singlePlans.All(plan => plan.GetSize() - 1 >= MinGoalTimeStep)) { if (Constants.costFunction == Constants.CostFunction.SUM_OF_COSTS) { - if (this.singlePlans.Sum(plan => plan.GetCost()) >= this.minGoalCost) + if (singlePlans.Sum(plan => plan.GetCost()) >= MinGoalCost) return true; } else if (Constants.costFunction == Constants.CostFunction.MAKESPAN || Constants.costFunction == Constants.CostFunction.MAKESPAN_THEN_SUM_OF_COSTS) { - if (this.singlePlans.Max(plan => plan.GetCost()) >= this.minGoalCost) + if (singlePlans.Max(plan => plan.GetCost()) >= MinGoalCost) return true; } else @@ -166,13 +165,13 @@ public bool GoalTest() } } - if (this.G < this.minGoalCost) + if (G < MinGoalCost) return false; - if (this.makespan < this.minGoalTimeStep) + if (Makespan < MinGoalTimeStep) return false; - return this.H == 0; // This assumes the heuristic is consistent, + return H == 0; // This assumes the heuristic is consistent, // or at least has the property of consistent heuristics that only the goal has h==0. // SIC really is a consistent heuristic, so this is fine for now. // TODO: Implement a proper goal test and use it when h==0. @@ -187,15 +186,15 @@ public bool GoalTest() /// public virtual void SetSolution(SinglePlan[] solution) { - this.singlePlans = SinglePlan.GetSinglePlans(this); // This node may be a partial solution itself, need to start from the real root. + singlePlans = SinglePlan.GetSinglePlans(this); // This node may be a partial solution itself, need to start from the real root. for (int i = 0; i < solution.Length; ++i) - this.singlePlans[i].ContinueWith(solution[i]); + singlePlans[i].ContinueWith(solution[i]); } public SinglePlan[] GetSinglePlans() { - if (this.singlePlans != null) - return this.singlePlans; + if (singlePlans != null) + return singlePlans; else return SinglePlan.GetSinglePlans(this); } @@ -207,8 +206,8 @@ public SinglePlan[] GetSinglePlans() /// public Plan GetPlan() { - if (this.singlePlans != null) - return new Plan(this.singlePlans); + if (singlePlans != null) + return new Plan(singlePlans); else return new Plan(this); } @@ -225,23 +224,23 @@ public Plan GetPlan() /// public int GetGoalCost() { - Trace.Assert(this.GoalTest(), "Only call for goal nodes!"); + Trace.Assert(GoalTest(), "Only call for goal nodes!"); if (goalCost == NOT_SET) // This is just a proper goal { if (Constants.costFunction == Constants.CostFunction.SUM_OF_COSTS) { - return this.G; + return G; } else if (Constants.costFunction == Constants.CostFunction.MAKESPAN || Constants.costFunction == Constants.CostFunction.MAKESPAN_THEN_SUM_OF_COSTS) { - return this.makespan; + return Makespan; } return 0; // To quiet the compiler } else // This is a generalised goal node - it stores the optimal path to the goal through it - return this.goalCost; + return goalCost; } /// @@ -252,7 +251,7 @@ public int GetGoalCost() /// public void SetGoalCost(int cost) { - this.goalCost = cost; + goalCost = cost; } /// @@ -262,12 +261,12 @@ public void SetGoalCost(int cost) public int[] GetSingleCosts() { - Trace.Assert(this.GoalTest(), "Only call for goal nodes!"); + Trace.Assert(GoalTest(), "Only call for goal nodes!"); if (goalSingleCosts == null) // This is just a proper goal - return allAgentsState.Select(agent => agent.g).ToArray(); + return AllAgentsState.Select(agent => agent.g).ToArray(); else - return this.goalSingleCosts; + return goalSingleCosts; } /// @@ -278,7 +277,7 @@ public int[] GetSingleCosts() /// public void SetSingleCosts(int[] costs) { - this.goalSingleCosts = costs; + goalSingleCosts = costs; } /// @@ -289,19 +288,19 @@ public void SetSingleCosts(int[] costs) public virtual int CompareTo(IBinaryHeapItem other) { WorldState that = (WorldState)other; - int thisF = this.F; + int thisF = F; int thatF = that.F; if (thisF < thatF) return -1; if (thisF > thatF) return 1; - return this.TieBreak(that); + return TieBreak(that); } public int TieBreak(WorldState that) { - bool thisIsGoal = this.GoalTest(); + bool thisIsGoal = GoalTest(); bool thatIsGoal = that.GoalTest(); if (thisIsGoal == true && thatIsGoal == false) // The elaborate form is necessary to keep the comparison consistent. Otherwise goalA that.primaryTieBreaker) + if (_primaryTieBreaker > that._primaryTieBreaker) return 1; // Prefer nodes with fewer conflicts - the probability that some of them are cardinal is lower - if (this.secondaryTieBreaker < that.secondaryTieBreaker) + if (_secondaryTieBreaker < that._secondaryTieBreaker) return -1; - if (this.secondaryTieBreaker > that.secondaryTieBreaker) + if (_secondaryTieBreaker > that._secondaryTieBreaker) return 1; // //M-Star: prefer nodes with smaller collision sets: - //if (this.collisionSets != null) // than M-Star is running + //if (collisionSets != null) // than M-Star is running //{ // // The collision sets change during collision set backpropagation and closed list hits. // // Backpropagation goes from a node's child to the node, so it's tempting to think @@ -342,17 +341,17 @@ public int TieBreak(WorldState that) // // Closed list hits can also happen while the node is waiting to be expanded. // // So the max rank can change while the node is in the open list - // // it can't be used for tie breaking :(. - // if (this.collisionSets.maxRank < that.collisionSets.maxRank) + // if (collisionSets.maxRank < that.collisionSets.maxRank) // return -1; - // if (that.collisionSets.maxRank > this.collisionSets.maxRank) + // if (that.collisionSets.maxRank > collisionSets.maxRank) // return 1; //} // f, collision sets, conflicts and internal conflicts being equal, prefer nodes with a larger g // - they're closer to the goal so less nodes would probably be generated by them on the way to it. - if (this.G < that.G) + if (G < that.G) return 1; - if (this.G > that.G) + if (G > that.G) return -1; return 0; @@ -363,19 +362,12 @@ public int TieBreak(WorldState that) /// public virtual void CalculateG() { - if (Constants.costFunction == Constants.CostFunction.SUM_OF_COSTS) + G = Constants.costFunction switch { - G = allAgentsState.Sum(agent => agent.g); - } - else if (Constants.costFunction == Constants.CostFunction.MAKESPAN || - Constants.costFunction == Constants.CostFunction.MAKESPAN_THEN_SUM_OF_COSTS) - { - G = makespan; // Let's hope makespan var is correct - } - else - { - throw new Exception($"Unsupported cost function {Constants.costFunction}"); - } + Constants.CostFunction.SUM_OF_COSTS => AllAgentsState.Sum(agent => agent.g), + Constants.CostFunction.MAKESPAN or Constants.CostFunction.MAKESPAN_THEN_SUM_OF_COSTS => Makespan,// Let's hope makespan var is correct + _ => throw new Exception($"Unsupported cost function {Constants.costFunction}"), + }; } /// @@ -383,20 +375,14 @@ public virtual void CalculateG() /// public virtual void Clear() { } - public virtual int F - { - get - { - return this.G + this.H; - } - } + public virtual int F => G + H; public int GetTargetH(int f) => f - G; public override string ToString() { - var builder = new System.Text.StringBuilder($"{generated} f:{F} makespan:{makespan} h:{H} g:{G} "); - foreach (AgentState temp in allAgentsState) + var builder = new System.Text.StringBuilder($"{Generated} f:{F} makespan:{Makespan} h:{H} g:{G} "); + foreach (AgentState temp in AllAgentsState) { builder.Append("|"); builder.Append(temp.lastMove); @@ -409,32 +395,26 @@ public override string ToString() /// Returns the last move of all the agents in this state. /// /// A list of Moves - public List GetAgentsMoves() - { - return this.allAgentsState.Select(state => state.lastMove).ToList(); - } + public List GetAgentsMoves() => [.. AllAgentsState.Select(state => state.lastMove)]; /// /// Returns the last move of the requested agent. /// /// /// - public Move GetSingleAgentMove(int index) - { - return allAgentsState[index].lastMove; - } + public Move GetSingleAgentMove(int index) => AllAgentsState[index].lastMove; /// /// BH_Item implementation /// /// - public int GetIndexInHeap() { return binaryHeapIndex; } + public int GetIndexInHeap() => _binaryHeapIndex; /// /// BH_Item implementation /// /// - public void SetIndexInHeap(int index) { binaryHeapIndex = index; } + public void SetIndexInHeap(int index) { _binaryHeapIndex = index; } /// /// Checks for internal conflicts @@ -442,12 +422,12 @@ public Move GetSingleAgentMove(int index) /// public bool isValid() { - for (int i = 0; i < this.allAgentsState.Length; i++) + for (int i = 0; i < AllAgentsState.Length; i++) { - for (int j = i+1; j < this.allAgentsState.Length; j++) + for (int j = i+1; j < AllAgentsState.Length; j++) { // Internal conflict - if (this.allAgentsState[i].lastMove.IsColliding(this.allAgentsState[j].lastMove)) + if (AllAgentsState[i].lastMove.IsColliding(AllAgentsState[j].lastMove)) return false; } } @@ -464,9 +444,9 @@ public override int GetHashCode() int ans = 0; unchecked { - for (int i = 0 ; i < allAgentsState.Length; i++) + for (int i = 0 ; i < AllAgentsState.Length; i++) { - ans += allAgentsState[i].GetHashCode() * Constants.PRIMES_FOR_HASHING[i % Constants.PRIMES_FOR_HASHING.Length]; + ans += AllAgentsState[i].GetHashCode() * Constants.PRIMES_FOR_HASHING[i % Constants.PRIMES_FOR_HASHING.Length]; } } return ans; @@ -483,7 +463,7 @@ public override bool Equals(object obj) if (obj == null) return false; WorldState that = (WorldState)obj; - return this.allAgentsState.SequenceEqual(that.allAgentsState); + return AllAgentsState.SequenceEqual(that.AllAgentsState); } /// @@ -494,20 +474,20 @@ public override bool Equals(object obj) /// public virtual void IncrementConflictCounts(ConflictAvoidanceTable CAT) { - for (int i = 0; i < this.allAgentsState.Length; i++) + for (int i = 0; i < AllAgentsState.Length; i++) { - this.allAgentsState[i].lastMove.IncrementConflictCounts(CAT, this.conflictCounts, this.conflictTimes); + AllAgentsState[i].lastMove.IncrementConflictCounts(CAT, ConflictCounts, ConflictTimes); } if (CAT.AvoidanceGoal == AvoidanceGoal.MINIMIZE_CONFLICTS) // For ID, the original rule - this.primaryTieBreaker = this.conflictCounts.Sum(pair => pair.Value); + _primaryTieBreaker = ConflictCounts.Sum(pair => pair.Value); else if (CAT.AvoidanceGoal == AvoidanceGoal.MINIMIZE_CONFLICTING_GROUPS) - this.primaryTieBreaker = this.conflictCounts.Keys.Count; + _primaryTieBreaker = ConflictCounts.Keys.Count; else if (CAT.AvoidanceGoal == AvoidanceGoal.MINIMIZE_CONFLICTING_GROUPS_THEN_CONFLICTS) // For CBS, minimizes the number of conflicting groups and then the number of conflicts with them { - this.primaryTieBreaker = this.conflictCounts.Keys.Count; - this.secondaryTieBreaker = this.conflictCounts.Sum(pair => pair.Value); + _primaryTieBreaker = ConflictCounts.Keys.Count; + _secondaryTieBreaker = ConflictCounts.Sum(pair => pair.Value); } else if (CAT.AvoidanceGoal == AvoidanceGoal.MINIMIZE_LARGEST_CONFLICTING_GROUP_THEN_NUMBER_OF_SUCH_GROUPS) // For ID, minimizes the size of the largest group we conflict with and then @@ -515,36 +495,36 @@ public virtual void IncrementConflictCounts(ConflictAvoidanceTable CAT) // non-max-size groups don't. // Kept mostly for reference. { - if (this.conflictCounts.Count != 0) + if (ConflictCounts.Count != 0) { - this.primaryTieBreaker = this.conflictCounts.Max(pair => CAT.AgentSizes[pair.Key]); - this.secondaryTieBreaker = this.conflictCounts.Where(pair => CAT.AgentSizes[pair.Key] == this.primaryTieBreaker).Count(); + _primaryTieBreaker = ConflictCounts.Max(pair => CAT.AgentSizes[pair.Key]); + _secondaryTieBreaker = ConflictCounts.Where(pair => CAT.AgentSizes[pair.Key] == _primaryTieBreaker).Count(); } else { - this.primaryTieBreaker = 0; - this.secondaryTieBreaker = 0; + _primaryTieBreaker = 0; + _secondaryTieBreaker = 0; } } else if (CAT.AvoidanceGoal == AvoidanceGoal.MINIMIZE_LARGEST_CONFLICTING_GROUP_THEN_MAXIMIZE_CONFLICT_COUNTS_WITH_OTHERS) // For ID, minimizes the size of the largest group we conflict with and then // maximizes the number of conflicts the two groups have with other groups { - if (this.conflictCounts.Count != 0) + if (ConflictCounts.Count != 0) { - this.primaryTieBreaker = this.conflictCounts.Max(pair => CAT.AgentSizes[pair.Key]); - this.secondaryTieBreaker = -(this.conflictCounts.Sum(pair => pair.Value) + - this.conflictCounts.Where(pair => CAT.AgentSizes[pair.Key] == this.primaryTieBreaker).Max(pair => CAT.AgentConflictCounts[pair.Key])); + _primaryTieBreaker = ConflictCounts.Max(pair => CAT.AgentSizes[pair.Key]); + _secondaryTieBreaker = -(ConflictCounts.Sum(pair => pair.Value) + + ConflictCounts.Where(pair => CAT.AgentSizes[pair.Key] == _primaryTieBreaker).Max(pair => CAT.AgentConflictCounts[pair.Key])); } else { - this.primaryTieBreaker = 0; - this.secondaryTieBreaker = 0; + _primaryTieBreaker = 0; + _secondaryTieBreaker = 0; } } else if (CAT.AvoidanceGoal == AvoidanceGoal.MINIMIZE_CONFLICTING_GROUP_SIZE_AND_COUNT) { - this.primaryTieBreaker = this.conflictCounts.Sum(pair => 1 << (CAT.AgentSizes[pair.Key] - 1)); + _primaryTieBreaker = ConflictCounts.Sum(pair => 1 << (CAT.AgentSizes[pair.Key] - 1)); } } @@ -559,14 +539,6 @@ public virtual (ProblemInstance, ISet) ToProblemInstance(ProblemI // Notice this is not a subproblem in the number of agents but // in the steps from the start. // It might even be harder if the steps were away from the goal. - return (initial.Subproblem(this.allAgentsState), new HashSet()); + return (initial.Subproblem(AllAgentsState), new HashSet()); } - - //public WorldState GetPlanStart(int agentIndex) - //{ - // WorldState node = this; - // while (node.individualMStarBookmarks[agentIndex] != 0) - // node = node.prevStep; - // return node; - //} } diff --git a/WorldStateForPartialExpansion.cs b/WorldStateForPartialExpansion.cs index d4bdc89..ab59a24 100644 --- a/WorldStateForPartialExpansion.cs +++ b/WorldStateForPartialExpansion.cs @@ -107,7 +107,7 @@ public override string ToString() public void calcSingleAgentDeltaFs(ProblemInstance problem, ValidityChecker isValid) { // Init - this.singleAgentDeltaFs = new byte[allAgentsState.Length][]; + this.singleAgentDeltaFs = new byte[AllAgentsState.Length][]; for (int i = 0; i < singleAgentDeltaFs.Length; i++) { this.singleAgentDeltaFs[i] = new byte[Constants.NUM_ALLOWED_DIRECTIONS]; @@ -118,28 +118,28 @@ public void calcSingleAgentDeltaFs(ProblemInstance problem, ValidityChecker isVa this.maxDeltaF = 0; // Set values - for (int i = 0; i < allAgentsState.Length; i++) + for (int i = 0; i < AllAgentsState.Length; i++) { - hBefore = problem.GetSingleAgentOptimalCost(allAgentsState[i]); + hBefore = problem.GetSingleAgentOptimalCost(AllAgentsState[i]); int singleAgentMaxLegalDeltaF = -1; - foreach (TimedMove check in allAgentsState[i].lastMove.GetNextMoves()) + foreach (TimedMove check in AllAgentsState[i].lastMove.GetNextMoves()) { - if (isValid(check, noMoves, this.makespan + 1, i, this, this) == false) // Is this move by itself invalid because of constraints or obstacles + if (isValid(check, noMoves, this.Makespan + 1, i, this, this) == false) // Is this move by itself invalid because of constraints or obstacles { singleAgentDeltaFs[i][(int)check.Direction] = byte.MaxValue; } else { - hAfter = problem.GetSingleAgentOptimalCost(allAgentsState[i].agent.agentNum, check); + hAfter = problem.GetSingleAgentOptimalCost(AllAgentsState[i].agent.agentNum, check); if (Constants.sumOfCostsVariant == Constants.SumOfCostsVariant.ORIG) { if (hBefore != 0) singleAgentDeltaFs[i][(int)check.Direction] = (byte)(hAfter - hBefore + 1); // h difference + g difference in this specific domain else if (hAfter != 0) // If agent moved from its goal we must count and add all the steps it was stationed at the goal, since they're now part of its g difference - singleAgentDeltaFs[i][(int)check.Direction] = (byte)(hAfter - hBefore + makespan - allAgentsState[i].arrivalTime + 1); + singleAgentDeltaFs[i][(int)check.Direction] = (byte)(hAfter - hBefore + Makespan - AllAgentsState[i].arrivalTime + 1); else singleAgentDeltaFs[i][(int)check.Direction] = 0; // This is a WAIT move at the goal. } @@ -163,7 +163,7 @@ public void calcSingleAgentDeltaFs(ProblemInstance problem, ValidityChecker isVa this.maxDeltaF += (byte) singleAgentMaxLegalDeltaF; } - fLookup = new DeltaFAchievable[allAgentsState.Length][]; + fLookup = new DeltaFAchievable[AllAgentsState.Length][]; for (int i = 0; i < fLookup.Length; i++) { fLookup[i] = new DeltaFAchievable[this.maxDeltaF + 1]; // Towards the last agents most of the row will be wasted (the last one can do delta F of 0 or 1), @@ -205,7 +205,7 @@ public bool hasChildrenForCurrentDeltaF(int agentNum=0) protected bool existsChildForF(int agentNum, ushort remainingTargetDeltaF) { // Stopping conditions: - if (agentNum == allAgentsState.Length) + if (agentNum == AllAgentsState.Length) { if (remainingTargetDeltaF == 0) return true; @@ -245,7 +245,7 @@ public void UpdateRemainingDeltaF(int agentIndex) { Trace.Assert(false, $"Remaining deltaF is ushort.MaxValue, a reserved value with special meaning. agentIndex={agentIndex}"); - byte lastMoveDeltaF = this.singleAgentDeltaFs[agentIndex][(int)this.allAgentsState[agentIndex].lastMove.Direction]; + byte lastMoveDeltaF = this.singleAgentDeltaFs[agentIndex][(int)this.AllAgentsState[agentIndex].lastMove.Direction]; if (lastMoveDeltaF != byte.MaxValue && this.remainingDeltaF >= lastMoveDeltaF) this.remainingDeltaF -= lastMoveDeltaF; else diff --git a/WorldStateWithOD.cs b/WorldStateWithOD.cs index 2a2b8a3..da7e3a2 100644 --- a/WorldStateWithOD.cs +++ b/WorldStateWithOD.cs @@ -46,16 +46,16 @@ public override (ProblemInstance, ISet) ToProblemInstance(Problem { // CBS doesn't handle partially expanded nodes well. // Use the last fully expanded node and add the additional moves as must conds: - state = this.prevStep; // Points to the last fully expanded node. + state = this.PrevStep; // Points to the last fully expanded node. } - ProblemInstance subproblem = initial.Subproblem(state.allAgentsState); // Can't use base's method because we're operating on a different object + ProblemInstance subproblem = initial.Subproblem(state.AllAgentsState); // Can't use base's method because we're operating on a different object var positiveConstraints = new HashSet(); if (this.agentTurn != 0) { for (int i = 0; i < this.agentTurn; ++i) { - positiveConstraints.Add(new CbsConstraint(this.allAgentsState[i].agent.agentNum, this.allAgentsState[i].lastMove)); + positiveConstraints.Add(new CbsConstraint(this.AllAgentsState[i].agent.agentNum, this.AllAgentsState[i].lastMove)); } } @@ -71,7 +71,7 @@ public override void SetSolution(SinglePlan[] solution) if (this.agentTurn == 0) this.singlePlans = SinglePlan.GetSinglePlans(this); else - this.singlePlans = SinglePlan.GetSinglePlans(this.prevStep); + this.singlePlans = SinglePlan.GetSinglePlans(this.PrevStep); // ToProblemInstance gives the last proper state as the problem to solve, // with must constraints to make the solution go through the steps already // taken from there. @@ -124,21 +124,21 @@ public override bool Equals(object obj) if (this.agentTurn == 0) // All agents have moved, safe to ignore direction information. return base.Equals(obj); - if (this.allAgentsState.Length != that.allAgentsState.Length) + if (this.AllAgentsState.Length != that.AllAgentsState.Length) return false; // Comparing the agent states: - for (int i = 0; i < this.allAgentsState.Length; ++i) + for (int i = 0; i < this.AllAgentsState.Length; ++i) { - if (this.allAgentsState[i].Equals(that.allAgentsState[i]) == false) + if (this.AllAgentsState[i].Equals(that.AllAgentsState[i]) == false) return false; if (i < this.agentTurn) // Agent has already moved in this step { bool mightCollideLater = false; - for (int j = this.agentTurn; j < this.allAgentsState.Length; j++) + for (int j = this.agentTurn; j < this.AllAgentsState.Length; j++) { - if (this.allAgentsState[i].lastMove.X == this.allAgentsState[j].lastMove.X && - this.allAgentsState[i].lastMove.Y == this.allAgentsState[j].lastMove.Y) // Can't just remove the direction and use IsColliding since the moves' time is different, so they'll never collide + if (this.AllAgentsState[i].lastMove.X == this.AllAgentsState[j].lastMove.X && + this.AllAgentsState[i].lastMove.Y == this.AllAgentsState[j].lastMove.Y) // Can't just remove the direction and use IsColliding since the moves' time is different, so they'll never collide { mightCollideLater = true; break; @@ -147,9 +147,9 @@ public override bool Equals(object obj) if (mightCollideLater == true) // Then check the direction too { - if (this.allAgentsState[i].lastMove.Direction != Direction.NO_DIRECTION && - that.allAgentsState[i].lastMove.Direction != Direction.NO_DIRECTION && - this.allAgentsState[i].lastMove.Direction != that.allAgentsState[i].lastMove.Direction) // Can't just use this.allAgentsState[i].lastMove.Equals(that.allAgentsState[i].lastMove) because TimedMoves don't ignore the time. + if (this.AllAgentsState[i].lastMove.Direction != Direction.NO_DIRECTION && + that.AllAgentsState[i].lastMove.Direction != Direction.NO_DIRECTION && + this.AllAgentsState[i].lastMove.Direction != that.AllAgentsState[i].lastMove.Direction) // Can't just use this.allAgentsState[i].lastMove.Equals(that.allAgentsState[i].lastMove) because TimedMoves don't ignore the time. return false; } } @@ -196,10 +196,10 @@ public override void IncrementConflictCounts(ConflictAvoidanceTable conflictAvoi { int lastAgentToMove = agentTurn - 1; if (agentTurn == 0) - lastAgentToMove = allAgentsState.Length - 1; + lastAgentToMove = AllAgentsState.Length - 1; - allAgentsState[lastAgentToMove].lastMove.IncrementConflictCounts(conflictAvoidance, - this.conflictCounts, this.conflictTimes); - this.primaryTieBreaker = this.conflictCounts.Sum(pair => pair.Value); + AllAgentsState[lastAgentToMove].lastMove.IncrementConflictCounts(conflictAvoidance, + this.ConflictCounts, this.ConflictTimes); + this._primaryTieBreaker = this.ConflictCounts.Sum(pair => pair.Value); } } From d977196f3045e452bd810b1e997b1d3914eaf2a7 Mon Sep 17 00:00:00 2001 From: Artem Kliatchkine Date: Fri, 9 May 2025 10:24:13 +0200 Subject: [PATCH 14/14] Parallel calculation of individual distances (minor performance gain on bigger grids). --- ProblemInstance.cs | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/ProblemInstance.cs b/ProblemInstance.cs index 74f28e6..952d0ab 100644 --- a/ProblemInstance.cs +++ b/ProblemInstance.cs @@ -3,6 +3,7 @@ using System.Linq; using System.IO; using System.Diagnostics; +using System.Threading.Tasks; namespace mapf; @@ -153,8 +154,8 @@ public void ComputeSingleAgentShortestPaths() _singleAgentOptimalCosts = new int[GetNumOfAgents()][]; _singleAgentOptimalMoves = new Move[GetNumOfAgents()][]; - // TODO: test on bigger grids and see if Parallel loop can be beneficial. - for (int agentId = 0; agentId < GetNumOfAgents(); agentId++) + // Parallel is only benefitial on bigger grids, but why not. + Parallel.For(0, GetNumOfAgents(), agentId => { // Run a single source shortest path algorithm from the _goal_ of the agent var shortestPathLengths = new int[NumLocations]; @@ -206,7 +207,7 @@ public void ComputeSingleAgentShortestPaths() _singleAgentOptimalCosts[agentId] = shortestPathLengths; _singleAgentOptimalMoves[agentId] = optimalMoves; - } + }); double endTime = watch.Elapsed.TotalMilliseconds; ShortestPathComputeTime = endTime - startTime; }