/// <summary>
/// Search for an optimal solution using the Simple Independence Detection algorithm in Trevor Standley's paper.
/// </summary>
/// <param name="runner"></param>
/// <returns></returns>
public bool SimpleID(Run runner)
{
while (true)
{
Conflict conflict = FindConflictingGroups();
// If there are no conflicts - can finish the run
if (conflict == null)
{
break;
}
allGroups.Remove(conflict.group1);
allGroups.Remove(conflict.group2);
AgentsGroup compositeGroup = this.JoinGroups(conflict);
// Solve composite group with A*
bool solved = compositeGroup.Solve(runner);
if (solved == false)
{
this.totalCost = compositeGroup.solutionCost;
return(false);
}
allGroups.AddFirst(compositeGroup);
}
return(true);
}
/// <summary>
/// Joins this and another group to a single group with all of the agents together.
/// </summary>
/// <param name="other"></param>
/// <returns>A new Trevor_Group object with the agents from both this and the other group</returns>
public AgentsGroup Join(AgentsGroup other)
{
AgentState[] joinedAgentStates = new AgentState[allAgentsState.Length + other.allAgentsState.Length];
this.allAgentsState.CopyTo(joinedAgentStates, 0);
other.allAgentsState.CopyTo(joinedAgentStates, this.allAgentsState.Length);
Array.Sort <AgentState>(joinedAgentStates, (x, y) => x.agent.agentNum.CompareTo(y.agent.agentNum));
return(new AgentsGroup(this.instance, joinedAgentStates, this.singleAgentSolver, this.groupSolver));
}
/// <summary>
/// Run the A* algorithm with Standley's ID and OD improvements.
/// </summary>
/// <returns>true if optimal solution has been found</returns>
public bool Solve()
{
// Solve the single agent problems independently
LinkedListNode <AgentsGroup> agentGroupNode = this.allGroups.First;
maxDepth = 0;
while (agentGroupNode != null)
{
AgentsGroup group = agentGroupNode.Value;
group.instance.parameters[CONFLICT_AVOIDANCE] = conflictAvoidance;
group.Solve(runner);
if (group.solutionCost > maxDepth)
{
maxDepth = group.solutionCost;
}
// Add group to conflict avoidance table
group.addGroupToCA(conflictAvoidance, maxDepth);
this.expanded += group.expanded;
this.generated += group.generated;
agentGroupNode = agentGroupNode.Next;
}
//bool solved = this.SimpleIndependenceDetection(runner);
bool solved = this.IndependenceDetection(runner);
// Record found solution
if (solved == true)
{
// Store solution details
this.totalCost = 0;
foreach (AgentsGroup group in this.allGroups)
{
this.totalCost += group.solutionCost;
}
this.foundPlan = this.CalculateJointPlan();
}
else
{
this.foundPlan = null;
}
Console.WriteLine();
Console.WriteLine(this.allGroups.Count + " - Independent Groups");
Console.WriteLine(this.maxGroup + " - Size Of Largest ID Group");
Console.WriteLine();
return(solved);
}
/// <summary>
/// Joins this and another group to a single group with all of the agents together.
/// </summary>
/// <param name="other"></param>
/// <returns>A new Trevor_Group object with the agents from both this and the other group</returns>
public AgentsGroup Join(AgentsGroup other)
{
AgentState[] joinedAgentStates = new AgentState[allAgentsState.Length + other.allAgentsState.Length];
int i;
for (i = 0; i < allAgentsState.Length; i++)
{
joinedAgentStates[i] = allAgentsState[i];
}
for (int j = 0; j < other.allAgentsState.Length; j++)
{
joinedAgentStates[i + j] = other.allAgentsState[j];
}
return(new AgentsGroup(this.instance, joinedAgentStates, this.solver));
}
public override bool Equals(object obj) // TODO: Implement GetHashCode()
{
AgentsGroup other = (AgentsGroup)obj;
return(allAgentsState.SequenceEqual <AgentState>(other.allAgentsState));
}
/// <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)
{
Conflict conflict = FindConflictingGroups();
// If there are no conflicts - can finish the run
if (conflict == null)
{
break;
}
// Try to solve the current conflict by replanning one of the groups
if (this.allConflicts.Contains(conflict) == false)
{
// Add to all conflicts to prevent trying to replan this conflict again
this.allConflicts.Add(conflict);
// Add plan of group2 to illegal moves table and replan group1 with equal cost
if ((conflict.timeOfConflict < conflict.group1.GetPlan().GetSize() - 1) ||
(conflict.group1.allAgentsState.Length > 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
{
conflict.group1.removeGroupFromCAT(conflictAvoidance);
bool resolved = conflict.group1.ReplanUnderConstraints(conflict.group2.GetPlan(), runner);
conflict.group1.addGroupToCAT(conflictAvoidance, maxSolutionCostFound);
if (resolved == true)
{
continue;
}
}
// Add plan of group1 to illegal moves table and replan group2 with equal cost
if ((conflict.timeOfConflict < conflict.group2.GetPlan().GetSize() - 1) ||
(conflict.group2.allAgentsState.Length > 1))
{
conflict.group2.removeGroupFromCAT(conflictAvoidance);
bool resolved = conflict.group2.ReplanUnderConstraints(conflict.group1.GetPlan(), runner);
conflict.group2.addGroupToCAT(conflictAvoidance, maxSolutionCostFound);
if (resolved == true)
{
continue;
}
}
}
// 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(conflictAvoidance);
conflict.group2.removeGroupFromCAT(conflictAvoidance);
if (this.debug)
{
Debug.WriteLine("Merging " + conflict);
}
AgentsGroup compositeGroup = this.JoinGroups(conflict);
compositeGroup.instance.parameters[CONFLICT_AVOIDANCE] = conflictAvoidance;
// Solve composite group with A*
bool solved = compositeGroup.Solve(runner);
if (compositeGroup.solutionCost > maxSolutionCostFound)
{
maxSolutionCostFound = compositeGroup.solutionCost;
}
//add group to conflict avoidance table
compositeGroup.addGroupToCAT(conflictAvoidance, maxSolutionCostFound);
allGroups.AddFirst(compositeGroup);
this.expanded += compositeGroup.expanded;
this.generated += compositeGroup.generated;
if (compositeGroup.allAgentsState.Length > this.maxGroupSize)
{
this.maxGroupSize = compositeGroup.allAgentsState.Length;
}
if (solved == false)
{
this.totalCost = compositeGroup.solutionCost; // Should be some error code from Constants
return(false);
}
}
return(true);
}
