/// <summary>
/// Search for an optimal solution using the Simple Independence Detection algorithm from Trevor Standley's paper.
/// </summary>
/// <param name="runner"></param>
/// <returns></returns>
public bool SimpleID(Run runner)
{
while (true)
{
IndependenceDetectionConflict conflict = FindFirstConflict();
// If there are no conflicts - can finish the run
if (conflict == null)
{
break;
}
allGroups.Remove(conflict.group1);
allGroups.Remove(conflict.group2);
IndependenceDetectionAgentsGroup compositeGroup = this.JoinGroups(conflict);
++merges;
// Solve composite group with A*
bool solved = compositeGroup.Solve(runner, conflictAvoidanceTable);
if (solved == false)
{
this.totalCost = compositeGroup.solutionCost;
return(false);
}
allGroups.AddFirst(compositeGroup);
}
return(true);
}
void UpdateConflictCounts(IndependenceDetectionAgentsGroup group)
{
conflictCountsPerGroup[group.groupNum] = group.conflictCounts;
conflictTimesPerGroup[group.groupNum] = group.conflictTimes;
// Update conflict counts with what happens after the plan finishes
this.IncrementConflictCountsAtGoal(group, conflictAvoidanceTable);
// Update conflictCountsPerGroup and conflictTimesPerGroup for all other groups
for (int i = 0; i < this.conflictCountsPerGroup.Length; ++i)
{
if (this.conflictCountsPerGroup[i] == null || i == group.groupNum)
{
continue;
}
if (group.conflictCounts.ContainsKey(i))
{
this.conflictCountsPerGroup[i][group.groupNum] = group.conflictCounts[i];
this.conflictTimesPerGroup[i][group.groupNum] = group.conflictTimes[i];
}
else
{
this.conflictCountsPerGroup[i].Remove(group.groupNum);
this.conflictTimesPerGroup[i].Remove(group.groupNum);
}
}
CountConflicts();
}
public override bool Equals(object obj)
{
if (obj == null)
{
return(false);
}
IndependenceDetectionAgentsGroup other = (IndependenceDetectionAgentsGroup)obj;
return(allAgentsState.SequenceEqual(other.allAgentsState));
}
/// <summary>
/// Joins this and another group to a single group with all of the agents together.
/// </summary>
/// <param name="other"></param>
/// <returns>A new AgentsGroup object with the agents from both this and the other group</returns>
public IndependenceDetectionAgentsGroup Join(IndependenceDetectionAgentsGroup other)
{
AgentState[] joinedAgentStates = new AgentState[allAgentsState.Length + other.allAgentsState.Length];
this.allAgentsState.CopyTo(joinedAgentStates, 0);
other.allAgentsState.CopyTo(joinedAgentStates, this.allAgentsState.Length);
if (this.groupSolver.GetType() != typeof(CostTreeSearchSolverOldMatching))
{
Array.Sort(joinedAgentStates, (x, y) => x.agent.agentNum.CompareTo(y.agent.agentNum)); // TODO: Technically could be a merge. FIXME: Is this necessary at all?
}
return(new IndependenceDetectionAgentsGroup(this.instance, joinedAgentStates, this.singleAgentSolver, this.groupSolver));
}
/// <summary>
/// Update conflict counts according to what happens after the plan finishes -
/// needed if the plan is shorter than one of the previous plans and collides
/// with it while at the goal.
/// It's cheaper to do it this way than to force the solver the go more deeply.
/// The conflict counts are saved at the group's representative.
/// </summary>
protected void IncrementConflictCountsAtGoal(IndependenceDetectionAgentsGroup group, ConflictAvoidanceTable CAT)
{
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);
for (int time = group.GetPlan().GetSize(); time < CAT.GetMaxPlanSize(); time++)
{
afterGoal.time = time;
afterGoal.IncrementConflictCounts(CAT,
this.conflictCountsPerGroup[group.groupNum],
this.conflictTimesPerGroup[group.groupNum]);
}
}
}
private IndependenceDetectionConflict ChooseFirstConflict()
{
int groupRepA = -1; // To quiet the compiler
int groupRepB = -1; // To quiet the compiler
int time = int.MaxValue;
for (int i = 0; i < this.conflictTimesPerGroup.Length; i++)
{
if (conflictCountsPerGroup[i] == null)
{
continue;
}
foreach (var otherGroupNumAndConflictTimes in this.conflictTimesPerGroup[i])
{
if (otherGroupNumAndConflictTimes.Value[0] < time)
{
time = otherGroupNumAndConflictTimes.Value[0];
groupRepA = i;
groupRepB = otherGroupNumAndConflictTimes.Key;
}
}
}
IndependenceDetectionAgentsGroup groupA = null, groupB = null;
foreach (var group in this.allGroups)
{
if (group.groupNum == groupRepA)
{
groupA = group;
}
else if (group.groupNum == groupRepB)
{
groupB = group;
}
}
return(new IndependenceDetectionConflict(groupA, groupB, time));
}
/// <summary>
/// Chooses the first agent to be the one that maximizes the number of agents it conflicts with internally divided by 2^(group_size-1).
/// Then chooses an agent among the agents it conflicts with using the same formula.
/// Then chooses their first conflict.
///
/// Choosing the agent that conflicts the most is a greedy strategy.
/// Had replanning promised to resolve all conflicts, it would've been better to choose according to the minimum vertex cover.
///
/// Assumes all agents are initially on the same timestep (no OD).
///
/// TODO: Prefer conflicts where one of the conflicting agents is at their goal, to reduce the danger of task blow-up
/// by enabling partial expansion. On the other hand, partial expansion is only possible in basic CBS.
/// </summary>
private IndependenceDetectionConflict ChooseConflictOfMostConflictingSmallestAgents()
{
int groupRepA = -1; // To quiet the compiler
int groupRepB = -1; // To quiet the compiler
int time = int.MaxValue;
Func <int, double> formula = i => this.conflictCountsPerGroup[i] != null ?
this.countsOfGroupsThatConflict[i] / ((double)(1 << (this.GetGroupSize(i) - 1)))
: -1;
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.
IEnumerable <int> conflictsWithGroupNums = this.conflictCountsPerGroup[chosenGroupNum].Keys;
int chosenConflictingGroupNum = conflictsWithGroupNums.MaxByKeyFunc(formula);
groupRepA = chosenGroupNum;
groupRepB = chosenConflictingGroupNum;
time = this.conflictTimesPerGroup[chosenGroupNum][chosenConflictingGroupNum][0]; // Choosing the earliest conflict between them - the choice doesn't matter for ID, but this is consistent with CBS' strategy
IndependenceDetectionAgentsGroup groupA = null, groupB = null;
foreach (var group in this.allGroups)
{
if (group.groupNum == groupRepA)
{
groupA = group;
}
else if (group.groupNum == groupRepB)
{
groupB = group;
}
}
return(new IndependenceDetectionConflict(groupA, groupB, time));
}
/// <summary>
/// Search for an optimal solution using the Independence Detection algorithm in Standley's paper,
/// which utilises a CAT.
/// </summary>
/// <param name="runner"></param>
/// <returns></returns>
public bool ImprovedID(Run runner)
{
while (true)
{
if (this.debug)
{
Debug.WriteLine($"{this.totalConflictCount} conflicts");
}
IndependenceDetectionConflict conflict = ChooseConflict();
// If there are no conflicts - can return the current plan
if (conflict == null)
{
break;
}
if (this.debug)
{
Debug.WriteLine($"{this.allGroups.Count} groups: {String.Join(", ", this.allGroups)}");
Debug.Write("Group single agent costs: ");
foreach (var group in this.allGroups)
{
Debug.Write($"{{{String.Join(" ", group.GetCosts())}}}, ");
}
Debug.WriteLine("");
for (int j = 0; j < this.conflictTimesPerGroup.Length; j++)
{
if (this.conflictTimesPerGroup[j] != null)
{
Debug.Write($"Group {j} conflict times: ");
foreach (var pair in this.conflictTimesPerGroup[j])
{
Debug.Write($"{pair.Key}:[{String.Join(",", pair.Value)}], ");
}
Debug.WriteLine("");
}
}
var plan = this.CalculateJointPlan();
if (plan.GetSize() < 200)
{
Debug.WriteLine(plan);
}
else
{
Debug.WriteLine($"Plan is too long to print ({plan.GetSize()} steps)");
}
Debug.WriteLine($"Chose {conflict}");
}
// Try to resolve the current conflict by re-planning one of the groups' path
if (this.resolutionAttemptedFirstGroup.Contains(conflict) == false) // We haven't already tried to resolve this conflict
// without merging the groups by replanning the first group's path
{
// Prevent trying to resolve this conflict this way again
this.resolutionAttemptedFirstGroup.Add(conflict);
// Add the plan of group2 to the illegal moves table and re-plan group1 with equal cost
if ((conflict.time < conflict.group1.GetPlan().GetSize() - 1) ||
(conflict.group1.Size() > 1)) // Otherwise the conflict is while a single agent
// is at its goal, no chance of an alternate path
// with the same cost that avoids the conflict - TODO: If it's an edge conflict while entering the goal it may be resolvable
{
if (this.debug)
{
Debug.WriteLine($"Trying to find an alternative path that avoids the conflict for {conflict.group1}.");
//Debug.WriteLine($"Old plan:\n{conflict.group1.GetPlan()}");
}
conflict.group1.removeGroupFromCAT(conflictAvoidanceTable);
bool resolved = conflict.group1.ReplanUnderConstraints(conflict.group2.GetPlan(), runner, this.conflictAvoidanceTable);
++resolutionAttempts;
if (resolved == true)
{
if (this.debug)
{
//Debug.WriteLine($"Found an alternative path that avoids the conflict for group 1: {conflict.group1.GetPlan()}");
Debug.WriteLine($"Found an alternative path that avoids the conflict for {conflict.group1}");
}
UpdateConflictCounts(conflict.group1);
conflict.group1.addGroupToCAT(conflictAvoidanceTable);
continue;
}
else
{
conflict.group1.addGroupToCAT(conflictAvoidanceTable);
}
if (this.debug)
{
Debug.WriteLine($"Couldn't find an alternative path that avoids the conflict for {conflict.group1}");
}
}
else
{
if (this.debug)
{
Debug.WriteLine($"Not trying to find an alternative path that avoids the conflict for {conflict.group1} because " +
"the group contains a single agent and the conflict happens after it reaches its goal.");
}
}
}
else
{
if (this.debug)
{
Debug.WriteLine($"Not trying to find an alternative path that avoids the conflict for {conflict.group1} - " +
"we've already tried to in the past.");
}
}
if (this.resolutionAttemptedSecondGroup.Contains(conflict) == false) // We haven't already tried to resolve this conflict
// without merging the groups by replanning the second group's path
{
// Prevent trying to resolve this conflict this way again
this.resolutionAttemptedSecondGroup.Add(conflict);
// Add the plan of group1 to the illegal moves table and re-plan group2 with equal cost
if ((conflict.time < conflict.group2.GetPlan().GetSize() - 1) ||
(conflict.group2.Size() > 1))
{
if (this.debug)
{
Debug.WriteLine($"Trying to find an alternative path that avoids the conflict for {conflict.group2}");
//Debug.WriteLine($"Old plan: {conflict.group2.GetPlan()}");
}
conflict.group2.removeGroupFromCAT(conflictAvoidanceTable);
bool resolved = conflict.group2.ReplanUnderConstraints(conflict.group1.GetPlan(), runner, this.conflictAvoidanceTable);
++resolutionAttempts;
if (resolved == true)
{
if (this.debug)
{
//Debug.WriteLine($"Found an alternative path that avoids the conflict for group 2: {conflict.group2.GetPlan()}");
Debug.WriteLine($"Found an alternative path that avoids the conflict for {conflict.group2}");
}
UpdateConflictCounts(conflict.group2);
conflict.group2.addGroupToCAT(conflictAvoidanceTable);
continue;
}
else
{
conflict.group2.addGroupToCAT(conflictAvoidanceTable);
}
if (this.debug)
{
Debug.WriteLine($"Couldn't find an alternative path that avoids the conflict for {conflict.group2}");
}
}
else
{
if (this.debug)
{
Debug.WriteLine($"Not trying to find an alternative path that avoids the conflict for {conflict.group2} because " +
"the group contains a single agent and the conflict happens after it reaches its goal.");
}
}
}
else
{
if (this.debug)
{
Debug.WriteLine($"Not trying to find an alternative path that avoids the conflict for {conflict.group2} - " +
"we've already tried to in the past.");
}
}
int group1Size = conflict.group1.Size();
int group1Cost = conflict.group1.solutionCost;
int group2Cost = conflict.group2.solutionCost;
// Groups are conflicting - need to join them to a single group
allGroups.Remove(conflict.group1);
allGroups.Remove(conflict.group2);
// Remove both groups from avoidance table
conflict.group1.removeGroupFromCAT(conflictAvoidanceTable);
conflict.group2.removeGroupFromCAT(conflictAvoidanceTable);
conflictCountsPerGroup[conflict.group1.groupNum] = null;
conflictTimesPerGroup[conflict.group1.groupNum] = null;
conflictCountsPerGroup[conflict.group2.groupNum] = null;
conflictTimesPerGroup[conflict.group2.groupNum] = null;
// Remove the old groups from the conflict counts - new counts will be put there after replanning
for (int i = 0; i < this.conflictCountsPerGroup.Length; i++)
{
this.conflictCountsPerGroup[i]?.Remove(conflict.group1.groupNum);
this.conflictCountsPerGroup[i]?.Remove(conflict.group2.groupNum);
this.conflictTimesPerGroup[i]?.Remove(conflict.group1.groupNum);
this.conflictTimesPerGroup[i]?.Remove(conflict.group2.groupNum);
}
if (this.debug)
{
Debug.WriteLine($"Merging the agent groups that participate in {conflict}.");
//Debug.WriteLine($"Group1 plan before the merge: {conflict.group1.GetPlan()}");
//Debug.WriteLine($"Group2 plan before the merge: {conflict.group2.GetPlan()}");
}
IndependenceDetectionAgentsGroup compositeGroup = this.JoinGroups(conflict);
++merges;
// Solve composite group with the underlying group solver
bool solved = compositeGroup.Solve(runner, conflictAvoidanceTable,
group1Cost, group2Cost, group1Size);
if (compositeGroup.solutionCost > maxSolutionCostFound)
{
maxSolutionCostFound = compositeGroup.solutionCost;
}
this.expanded += compositeGroup.expanded;
this.generated += compositeGroup.generated;
if (compositeGroup.allAgentsState.Length > this.maxGroupSize)
{
this.maxGroupSize = compositeGroup.allAgentsState.Length;
}
if (solved == false)
{
this.totalCost = Constants.NO_SOLUTION_COST;
return(false);
}
UpdateConflictCounts(compositeGroup);
// Add the new group to conflict avoidance table
compositeGroup.addGroupToCAT(conflictAvoidanceTable);
allGroups.AddFirst(compositeGroup);
}
return(true);
}
