Dictionary <Move, Tuple <int, int> > atGoalWaitsToTimeAndAgentNum = new Dictionary <Move, Tuple <int, int> >(); // No need for a list of agent nums because goals can't collide :)
public void AddPlan(SinglePlan plan)
{
int planSize = plan.GetSize();
for (int i = 0; i < planSize; i++)
{
Move temp = plan.GetLocationAt(i);
TimedMove step;
if (temp.GetType() == typeof(TimedMove))
{
step = (TimedMove)temp;
}
else
{
step = new TimedMove(temp, i); // TODO: Avoid creating new objects when possible. Make the method return correctly timed moves.
}
if (this.timedMovesToAgentNumList.ContainsKey(step) == false)
{
this.timedMovesToAgentNumList[step] = new List <int>(1); // THIS IS ON THE HOT PATH! ~11% of time is passed on this line!
}
this.timedMovesToAgentNumList[step].Add(plan.agentNum);
}
Move lastMove = plan.GetLocationAt(planSize - 1);
Move goal = new Move(lastMove.x, lastMove.y, Move.Direction.Wait);
this.atGoalWaitsToTimeAndAgentNum.Add(goal, new Tuple <int, int>(planSize, plan.agentNum));
}
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(MDDStep 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>();
}
MDDStep current = finish;
while (current != null)
{
for (int i = 0; i < problem.Length; i++)
{
routes[i].AddFirst(new Move(current.allSteps[i].move));
}
current = current.prevStep;
}
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.
/// </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);
}
// TODO: Add GetCost and GetMakespan methods!
public SinglePlan[] GetSinglePlans()
{
SinglePlan[] ans = new SinglePlan[this.locationsAtTimes.First().Count];
for (int i = 0; i < ans.Length; i++)
{
ans[i] = new SinglePlan(this, i);
}
return(ans);
}
public SinglePlan[] GetSinglePlans()
{
if (this.singlePlans != null)
{
return(this.singlePlans);
}
else
{
return(SinglePlan.GetSinglePlans(this));
}
}
/// <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>
/// 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;
}
}
this.locationAtTimes.Add(newLocationAtTime);
}
}
/// <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
{
Debug.Assert(false, "Continuing a plan doesn't start from the same state");
}
}
this.locationAtTimes.Add(newLocationAtTime);
}
}
/// <summary>
/// Solves a given problem according to given constraints, sets the plans array (plan per agent).
/// This method ignores the agentsGroupAssignment and solves for each agent separately using the low level solver,
/// which is OK because it's only called for the root node.
/// But on the other hand, it makes merging the method with Replan more difficult.
/// Can this just call Replan consecutively please?
/// </summary>
/// <param name="depthToReplan"></param>
/// <returns></returns>
public bool Solve(int depthToReplan)
{
this.totalCost = 0;
var newInternalCAT = new Dictionary <TimedMove, List <int> >();
HashSet <CbsConstraint> newConstraints = this.GetConstraints(); // Probably empty as this is probably the root of the CT.
var internalCAT = (Dictionary_U <TimedMove, int>)problem.parameters[CBS_LocalConflicts.INTERNAL_CAT];
var constraints = (HashSet_U <CbsConstraint>)problem.parameters[CBS_LocalConflicts.CONSTRAINTS];
bool haveMustConstraints = problem.parameters.ContainsKey(CBS_LocalConflicts.MUST_CONSTRAINTS) == true &&
((List <CbsConstraint>)problem.parameters[CBS_LocalConflicts.MUST_CONSTRAINTS]).Count > 0;
Dictionary <int, int> agentsWithMustConstraints = null; // To quiet the compiler
if (haveMustConstraints)
{
agentsWithMustConstraints = ((List <CbsConstraint>)problem.parameters[CBS_LocalConflicts.MUST_CONSTRAINTS]).Select <CbsConstraint, int>(constraint => constraint.agent).Distinct().ToDictionary <int, int>(x => x); // ToDictionary because there's no ToSet...
}
Dictionary <int, int> agentsWithConstraints = null; // To quiet the compiler
constraints.Join(newConstraints);
internalCAT.Join(newInternalCAT);
// This mechanism of adding the constraints to the possibly pre-existing constraints allows having
// layers of CBS solvers, each one adding its own constraints and respecting those of the solvers above it.
bool haveConstraints = (constraints.Count != 0);
if (haveConstraints)
{
int maxConstraintTimeStep = constraints.Max <CbsConstraint>(constraint => constraint.time);
depthToReplan = Math.Max(depthToReplan, maxConstraintTimeStep); // Give all constraints a chance to affect the plan
agentsWithConstraints = constraints.Select <CbsConstraint, int>(constraint => constraint.agent).Distinct().ToDictionary <int, int>(x => x); // ToDictionary because there's no ToSet...
}
bool success = true;
for (int i = 0; i < problem.m_vAgents.Length; i++)
{
if (i > 0)
{
// Add existing plans to CAT
newInternalCAT.Clear();
int maxPlanSize = allSingleAgentPlans.Take <SinglePlan>(i).Max <SinglePlan>(singlePlan => singlePlan.GetSize());
for (int j = 0; j < i; j++)
{
allSingleAgentPlans[j].AddPlanToCAT(newInternalCAT, maxPlanSize);
}
}
// Solve for a single agent:
if ((haveConstraints == false ||
agentsWithConstraints.ContainsKey(i) == false) &&
(haveMustConstraints == false ||
agentsWithMustConstraints.ContainsKey(i) == false)) // Top-most CBS with no must constraints on this agent. Shortcut available (ignoring the CAT though)
{
allSingleAgentPlans[i] = new SinglePlan(problem.m_vAgents[i]); // All moves up to starting pos
allSingleAgentPlans[i].ContinueWith(this.problem.GetSingleAgentOptimalPlan(problem.m_vAgents[i]));
allSingleAgentCosts[i] = problem.m_vAgents[i].g + this.problem.GetSingleAgentOptimalCost(problem.m_vAgents[i]);
totalCost += (ushort)allSingleAgentCosts[i];
}
else
{
var subGroup = new List <AgentState>();
subGroup.Add(problem.m_vAgents[i]);
success = this.Replan(i, depthToReplan, newInternalCAT, subGroup);
if (!success) // Usually means a timeout occured.
{
break;
}
}
}
internalCAT.Seperate(newInternalCAT);
constraints.Seperate(newConstraints);
if (!success)
{
return(false);
}
this.FindConflict();
return(true);
}
public override bool Equals(object obj) // TODO: Implement GetHashCode!
{
SinglePlan other = (SinglePlan)obj;
return(this.agentNum == other.agentNum && this.locationAtTimes.SequenceEqual <Move>(other.locationAtTimes));
}
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;
}
public override bool Equals(object obj)
{
SinglePlan other = (SinglePlan)obj;
return(this.agentIndex == other.agentIndex && this.locationAtTimes.SequenceEqual <Move>(other.locationAtTimes));
}
public SinglePlan[] GetSinglePlans()
{
return(SinglePlan.GetSinglePlans(solution));
}
