public void Check(ProblemInstance problem)
{
SinglePlan[] singles = new SinglePlan[this.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())
{
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.
{
// Check all pairs of agents for a collision at the given time step
foreach ((int i1, var plan1) in singles.Enumerate())
{
foreach ((int i2, var plan2) in singles.Enumerate())
{
if (i1 < i2)
{
Trace.Assert(plan1.IsColliding(time, plan2) == false, $"Plans of agents {i1} and {i2} collide at time {time}!");
}
}
}
}
}
public static SinglePlan[] GetSinglePlans(WorldState goalState) // FIXME: Duplication with other methods.
{
LinkedList <Move>[] allroutes = new LinkedList <Move> [goalState.allAgentsState.Length];
for (int i = 0; i < allroutes.Length; i++)
{
allroutes[i] = new LinkedList <Move>();
}
WorldState currentNode = goalState;
while (currentNode != null)
{
for (int i = 0; i < allroutes.Length; i++)
{
allroutes[i].AddFirst(currentNode.GetSingleAgentMove(i));
}
currentNode = currentNode.prevStep;
}
SinglePlan[] ans = new SinglePlan[goalState.allAgentsState.Length];
for (int i = 0; i < ans.Length; i++)
{
ans[i] = new SinglePlan(allroutes[i], goalState.allAgentsState[i].agent.agentNum);
}
return(ans);
}
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 <Move> [problem.Length];
for (int i = 0; i < routes.Length; i++)
{
routes[i] = new LinkedList <Move>();
}
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));
}
current = current.prev;
}
var ans = new SinglePlan[problem.Length];
for (int i = 0; i < ans.Length; i++)
{
ans[i] = new SinglePlan(routes[i], i);
}
return(ans);
}
/// <summary>
/// Set the optimal solution of this node as a problem instance.
/// Currently only used by CbsHeuristicForAStar, if a solution was found while running the heuristic.
/// </summary>
/// <param name="solution"></param>
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.
for (int i = 0; i < solution.Length; ++i)
{
this.singlePlans[i].ContinueWith(solution[i]);
}
}
/// <summary>
/// Creates SinglePlans with agentIndex as agentNum. Not suitable for subproblems.
/// </summary>
/// <param name="allRoutes"></param>
/// <returns></returns>
public static SinglePlan[] GetSinglePlans(LinkedList <Move>[] allRoutes)
{
SinglePlan[] ans = new SinglePlan[allRoutes.Length];
for (int i = 0; i < ans.Length; i++)
{
ans[i] = new SinglePlan(allRoutes[i], i);
}
return(ans);
}
public override bool Equals(object obj)
{
if (obj == null)
{
return(false);
}
SinglePlan other = (SinglePlan)obj;
return(this.agentNum == other.agentNum && this.locationAtTimes.SequenceEqual <Move>(other.locationAtTimes));
}
public SinglePlan[] GetSinglePlans()
{
if (this.singlePlans != null)
{
return(this.singlePlans);
}
else
{
return(SinglePlan.GetSinglePlans(this));
}
}
public void removeGroupFromCAT(ConflictAvoidanceTable CAT)
{
if (this.plan == null)
{
return;
}
for (int i = 0; i < this.allAgentsState.Length; i++)
{
var singleAgentPlan = new SinglePlan(this.plan, i, this.groupNum);
CAT.RemovePlan(singleAgentPlan);
}
}
public void addGroupToCAT(ConflictAvoidanceTable CAT)
{
if (this.plan == null)
{
return;
}
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
CAT.AddPlan(singleAgentPlan);
}
}
/// <summary>
/// Check if this plan collides with another plan at a given time
/// </summary>
/// <param name="time">The time at which to check if the collision occured</param>
/// <param name="otherPlan">The plan to check against</param>
public bool IsColliding(int time, SinglePlan otherPlan)
{
Move thisLocation = this.GetLocationAt(time);
Move otherLocation = otherPlan.GetLocationAt(time);
if (thisLocation.IsColliding(otherLocation) == true) // IsColliding isn't virtual,
// so it doesn't matter whether the moves are actually TimedMoves
// with incorrect time
{
return(true);
}
return(false);
}
/// <summary>
/// Calculate the full plan for all the agents that has been found by the algorithm
/// </summary>
public Plan CalculateJointPlan()
{
var singlePlans = new SinglePlan[this.instance.GetNumOfAgents()];
foreach (var group in this.allGroups)
{
var groupPlan = group.GetPlan();
int i = 0;
foreach (var agentState in group.allAgentsState)
{
singlePlans[agentState.agent.agentNum] = new SinglePlan(groupPlan, i, agentState.agent.agentNum);
i++;
}
}
return(new Plan(singlePlans));
}
/// <summary>
/// Set the optimal solution of this node as a problem instance.
/// </summary>
/// <param name="solution"></param>
public override void SetSolution(SinglePlan[] solution)
{
if (this.agentTurn == 0)
{
this.singlePlans = SinglePlan.GetSinglePlans(this);
}
else
{
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.
for (int i = 0; i < solution.Length; ++i)
{
this.singlePlans[i].ContinueWith(solution[i]);
}
}
/// <summary>
/// Add actions of other plan after actions of plan.
/// If this plan ends where the other starts,
/// the first timestep of the other plan is skipped
/// </summary>
/// <param name="other"></param>
public void ContinueWith(SinglePlan other)
{
bool first = true;
foreach (Move newLocationAtTime in other.locationAtTimes)
{
if (first)
{
first = false;
if (this.locationAtTimes[this.locationAtTimes.Count - 1].Equals(newLocationAtTime))
{
continue;
}
else
{
Trace.Assert(false, "Continuing a plan doesn't start from the same state");
}
}
this.locationAtTimes.Add(newLocationAtTime);
}
}
public SinglePlan(SinglePlan cpy)
{
this.locationAtTimes = cpy.locationAtTimes.ToList <Move>(); // Behavior change: used to do a deep copy, with cloned moves.
this.agentNum = cpy.agentNum;
}
