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