public AgentState(AgentState copy)
{
this.agent = copy.agent;
this.h = copy.h;
this.arrivalTime = copy.arrivalTime;
this.lastMove = new TimedMove(copy.lastMove); // Can we just do this.lastMove = copy.lastMove? I think we can now, since MoveTo replaces the move
}
public HashSet <CFMCbsConstraint> GetConstraints()
{
var constraints = new HashSet <CFMCbsConstraint>();
CFMCbsNode current = this;
CFMCbsConstraint currentConstraint = null;
while (current.depth > 0) // The root has no constraints
{
if (current.constraint != null && // Next check not enough if "surprise merges" happen (merges taken from adopted child)
current.prev.conflict != null && // Can only happen for temporary lookahead nodes the were created and then later the parent adopted a goal node
this.agentsGroupAssignment[current.prev.conflict.agentAIndex] !=
this.agentsGroupAssignment[current.prev.conflict.agentBIndex]) // Ignore constraints that deal with conflicts between
// agents that were later merged. They're irrelevant
// since merging fixes all conflicts between merged agents.
// Nodes that only differ in such irrelevant conflicts will have the same single agent paths.
// Dereferencing current.prev is safe because current isn't the root.
// Also, merging creates a non-root node with a null constraint, and this helps avoid adding the null to the answer.
{
currentConstraint = current.constraint;
}
TimedMove currentMove = current.constraint.move;
CFMCbsConstraint newConstraint = new CFMCbsConstraint(currentConstraint.agentNum, currentMove.x, currentMove.y, currentMove.direction, currentMove.time);
constraints.Add(newConstraint);
current = current.prev;
}
return(constraints);
}
// Summary:
// Gets the element that has the specified key in the read-only dictionary.
//
// Parameters:
// key:
// The key to locate.
//
// Returns:
// The element that has the specified key in the read-only dictionary.
//
// Exceptions:
// System.ArgumentNullException:
// key is null.
//
// System.Collections.Generic.KeyNotFoundException:
// The property is retrieved and key is not found.
public List <int> this[TimedMove key]
{
get
{
List <int> ans = null;
if (this.timedMovesToAgentNumList.ContainsKey(key))
{
ans = new List <int>(this.timedMovesToAgentNumList[key].Count + 1);
ans.AddRange(this.timedMovesToAgentNumList[key]);
}
queryMove.setup(key);
if (this.atGoalWaitsToTimeAndAgentNum.ContainsKey(queryMove))
{
var timeAndAgentNum = this.atGoalWaitsToTimeAndAgentNum[queryMove];
if (key.time >= timeAndAgentNum.Item1)
{
if (ans == null)
{
ans = new List <int>(1);
}
ans.Add(timeAndAgentNum.Item2);
}
}
if (ans != null)
{
return(ans);
}
else
{
return(ConflictAvoidanceTable.emptyList);
}
}
}
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));
}
private bool closed
(
MAM_AgentState child
)
{
TimedMove timedChildMove = child.lastMove;
Move childMove = new Move(timedChildMove.x, timedChildMove.y, Move.Direction.NO_DIRECTION);
if (!closedList.ContainsKey(childMove)) // Child is not in the closed list
{
closedList.Add(childMove, new Dictionary <int, Dictionary <int, MAM_AgentState> >());
Dictionary <int, MAM_AgentState> moveAgentDiffTimes = new Dictionary <int, MAM_AgentState>();
moveAgentDiffTimes.Add(timedChildMove.time, child);
closedList[childMove].Add(child.agentIndex, moveAgentDiffTimes);
return(false);
}
else if (!closedList[childMove].Keys.Contains(child.agentIndex)) // Child is not in the closed list for this agent
{
Dictionary <int, MAM_AgentState> moveAgentDiffTimes = new Dictionary <int, MAM_AgentState>();
moveAgentDiffTimes.Add(timedChildMove.time, child);
closedList[childMove].Add(child.agentIndex, moveAgentDiffTimes);
return(false);
}
else if (!closedList[childMove][child.agentIndex].Keys.Contains(timedChildMove.time)) // Child is in the closed list for this agent, but not for this time
{
Dictionary <int, MAM_AgentState> moveAgentDiffTimes = closedList[childMove][child.agentIndex];
moveAgentDiffTimes.Add(timedChildMove.time, child);
//closedList[childMove].Add(child.agentIndex, moveAgentDiffTimes);
return(false);
}
return(true);
}
/// <summary>
/// Updates the conflicts member according to given CATs. Both tables may be null.
/// </summary>
/// <param name="ID_CAT"></param>
/// <param name="CBS_CAT"></param>
public void SetConflicts(Dictionary <TimedMove, List <int> > ID_CAT, Dictionary <TimedMove, List <int> > CBS_CAT)
{
TimedMove queryMove = new TimedMove();
if (this.prevStep == null)
{
return;
}
for (int i = 0; i < allSteps.Length; i++)
{
// TODO: Kill this code dup. The ConflictAvoidanceTable class takes care of it.
queryMove.setup(allSteps[i].move.x, allSteps[i].move.y, Move.Direction.NO_DIRECTION, allSteps[i].move.time);
if (ID_CAT != null && ID_CAT.ContainsKey(queryMove))
{
conflicts++;
}
if (CBS_CAT != null && CBS_CAT.ContainsKey(queryMove))
{
conflicts++;
}
queryMove.direction = allSteps[i].move.direction;
queryMove.setOppositeMove();
if (ID_CAT != null && ID_CAT.ContainsKey(queryMove))
{
conflicts++;
}
if (CBS_CAT != null && CBS_CAT.ContainsKey(queryMove))
{
conflicts++;
}
}
}
public new TimedMove GetMoveWithoutDirection()
{
TimedMove copy = new TimedMove(this);
copy.direction = Direction.NO_DIRECTION;
return(copy);
}
/// <summary>
/// Updates the agent's last move with the given move and sets arrivalTime (at goal) if necessary.
/// </summary>
public void MoveTo(TimedMove move)
{
this.lastMove = move;
bool isWait = move.direction == Move.Direction.Wait;
bool atGoal = this.AtGoal();
// If performed a non WAIT move and reached the agent's goal - store the arrival time
if (atGoal && (isWait == false))
{
this.arrivalTime = move.time;
}
if (Constants.Variant == Constants.ProblemVariant.ORIG)
{
if (this.AtGoal())
{
this.g = this.arrivalTime;
}
else
{
this.g = this.lastMove.time;
}
}
else if (Constants.Variant == Constants.ProblemVariant.NEW)
{
if ((atGoal && isWait) == false)
{
this.g += 1;
}
}
}
public void setConflicts(HashSet <TimedMove> ID_CAT, HashSet_U <TimedMove> CBS_CAT)
{
TimedMove m2 = new TimedMove();
if (this.prevStep == null)
{
return;
}
for (int i = 0; i < allSteps.Length; i++)
{
m2.setup(allSteps[i].getX(), allSteps[i].getY(), Move.Direction.NO_DIRECTION, getDepth());
if (ID_CAT != null && ID_CAT.Contains(m2))
{
conflicts++;
}
if (CBS_CAT != null && CBS_CAT.Contains(m2))
{
conflicts++;
}
m2.direction = Move.getDirection(allSteps[i].getX(), allSteps[i].getY(), prevStep.allSteps[i].getX(), prevStep.allSteps[i].getY());
m2.setOppositeMove();
if (ID_CAT != null && ID_CAT.Contains(m2))
{
conflicts++;
}
if (CBS_CAT != null && CBS_CAT.Contains(m2))
{
conflicts++;
}
}
}
private bool compareTimedMoves(TimedMove A, TimedMove B)
{
if (A.x == B.x && A.y == B.y && A.time == B.time)
{
return(true);
}
return(false);
}
public bool ViolatesMustConstraint(byte agent, TimedMove move)
{
if (this.agentNum != agent)
{
return(false);
}
return(this.move.Equals(move) == false);
}
/// <summary>
/// Updates the agent's last move with the given move and sets arrivalTime (at goal) if necessary.
/// </summary>
public void MoveTo(TimedMove move)
{
this.lastMove = move;
// If performed a non WAIT move and reached the agent's goal - store the arrival time
if ((move.direction != Move.Direction.Wait) && (this.AtGoal()))
{
this.arrivalTime = move.time;
}
}
/// <summary>
/// The returned plan wasn't constructed considering a CAT, so it's possible there's an alternative plan with the same cost and less collisions.
/// </summary>
/// <param name="agentState"></param>
/// <returns></returns>
public SinglePlan GetSingleAgentOptimalPlan(AgentState agentState,
out Dictionary <int, int> conflictCountPerAgent, out Dictionary <int, List <int> > conflictTimesPerAgent, out Dictionary <int, List <int> > conflictTimesBiasPerAgent, int conflictRange = 0)
{
LinkedList <Move> moves = new LinkedList <Move>();
int agentNum = agentState.agent.agentNum;
var conflictCounts = new Dictionary <int, int>();
var conflictTimes = new Dictionary <int, List <int> >();
var conflictTimesBias = new Dictionary <int, List <int> >();
var conflictProbability = new Dictionary <int, List <double> >();
IReadOnlyDictionary <TimedMove, List <int> > CAT;
if (this.parameters.ContainsKey(CBS_LocalConflicts.CAT)) // TODO: Add support for IndependenceDetection's CAT
{
CAT = ((IReadOnlyDictionary <TimedMove, List <int> >) this.parameters[CBS_LocalConflicts.CAT]);
}
else
{
CAT = new Dictionary <TimedMove, List <int> >();
}
//for(int tempTime = 0 ; tempTime < agentState.)
TimedMove current = agentState.lastMove; // The starting position
int time = current.time;
int timeWithoutDelays = 0;
while (true)
{
moves.AddLast(current);
if (current.direction != Move.Direction.NO_DIRECTION &&
current.direction != Move.Direction.Wait)
{
timeWithoutDelays++;
}
// Count conflicts:
current.UpdateConflictCounts(CAT, conflictCounts, conflictTimes, conflictTimesBias, conflictRange);
if (agentState.agent.Goal.Equals(current))
{
break;
}
// Get next optimal move
time++;
Move optimal = this.singleAgentOptimalMoves[agentNum][this.GetCardinality(current)];
current = new TimedMove(optimal, time);
}
conflictCountPerAgent = conflictCounts;
conflictTimesPerAgent = conflictTimes;
conflictTimesBiasPerAgent = conflictTimesBias;
return(new SinglePlan(moves, agentNum));
}
public MAM_AgentState
(
int pos_X,
int pos_Y,
int agentIndex,
int time
)
{
this.lastMove = new TimedMove(pos_X, pos_Y, Move.Direction.NO_DIRECTION, time);
this.agentIndex = agentIndex;
this.heuristics = new List <double>();
}
//
// Summary:
// Gets the value that is associated with the specified key.
//
// Parameters:
// key:
// The key to locate.
//
// value:
// When this method returns, the value associated with the specified key, if
// the key is found; otherwise, the default value for the type of the value
// parameter. This parameter is passed uninitialized.
//
// Returns:
// true if the object that implements the System.Collections.Generic.IReadOnlyDictionary<TKey,TValue>
// interface contains an element that has the specified key; otherwise, false.
//
// Exceptions:
// System.ArgumentNullException:
// key is null.
public bool TryGetValue(TimedMove key, out List <int> value)
{
if (this.ContainsKey(key))
{
value = this[key];
return(true);
}
else
{
value = null;
return(false);
}
}
public MDDNode(TimedMove move, int numOfAgents, MDD father)
{
this.move = move;
this.father = father;
setUntil = 0;
children = new LinkedList <MDDNode>();
parents = new LinkedList <MDDNode>();
coexistLinkedList = new LinkedList <MDDNode> [numOfAgents];
for (int i = 0; i < numOfAgents; i++)
{
coexistLinkedList[i] = new LinkedList <MDDNode>();
}
}
public MMStarConstraint(int agentNum, int posX, int posY, Move.Direction direction, int timeStep)
{
this.move = new TimedMove(posX, posY, direction, timeStep);
this.agentNum = agentNum;
if (direction == Move.Direction.NO_DIRECTION)
{
this.vertexConflict = true;
}
else
{
this.vertexConflict = false;
}
}
/// <summary>
/// TODO: Get rid of the else
/// </summary>
/// <param name="time"></param>
/// <returns></returns>
public Move GetLocationAt(int time)
{
if (time < this.locationAtTimes.Count)
{
return(this.locationAtTimes[time]);
}
else
{
var rest = new TimedMove(this.locationAtTimes[this.locationAtTimes.Count - 1], time);
rest.direction = Move.Direction.Wait;
return(rest);
}
}
public MAM_AgentState
(
MAM_AgentState copy
)
{
this.agentIndex = copy.agentIndex;
this.lastMove = copy.lastMove;
this.g = copy.g;
this.h = copy.h;
this.f = copy.f;
this.prev = copy.prev;
this.heuristics = new List <double>(copy.heuristics);
this.numOfAgentsInBestHeuristic = copy.numOfAgentsInBestHeuristic;
}
public MDDNode(TimedMove move, int numOfAgents, MDD mdd, bool supportPruning = true)
{
this.move = move;
this.mdd = mdd;
children = new LinkedList <MDDNode>();
parents = new LinkedList <MDDNode>();
if (supportPruning)
{
coexistLinkedList = new LinkedList <MDDNode> [numOfAgents];
for (int i = 0; i < numOfAgents; i++)
{
coexistLinkedList[i] = new LinkedList <MDDNode>();
}
}
}
private void printTimedMove(TimedMove mv1, TimedMove mv2, int Agent)
{
if (mv1 == null)
{
Console.WriteLine("Agent: " + Agent + ", At A: null, At B: " + timedMoveString(mv2));
}
else if (mv2 == null)
{
Console.WriteLine("Agent: " + Agent + ", At A: " + timedMoveString(mv1) + ", At B: null");
}
else if (!compareTimedMoves(mv1, mv2))
{
Console.WriteLine("Agent: " + Agent + ", At A: " + timedMoveString(mv1) + ", At B: " + timedMoveString(mv2));
}
}
private Dictionary <int, Dictionary <int, TimedMove> > conflictAvoidanceToDictionary(IReadOnlyDictionary <TimedMove, List <int> > conflictAvoidance, int maxPlan)
{
int count = 0;
Dictionary <int, Dictionary <int, TimedMove> > newDic = new Dictionary <int, Dictionary <int, TimedMove> >();
foreach (KeyValuePair <TimedMove, List <int> > t in conflictAvoidance)
{
count++;
}
//if (count > 29)
// Console.WriteLine("");
if (conflictAvoidance.Keys == null)
{
return(newDic);
}
foreach (KeyValuePair <TimedMove, List <int> > t in conflictAvoidance)
{
foreach (int agentIndex in t.Value)
{
if (!newDic.Keys.Contains(agentIndex))
{
newDic[agentIndex] = new Dictionary <int, TimedMove>();
}
if (!newDic[agentIndex].Keys.Contains(t.Key.time))
{
newDic[agentIndex].Add(t.Key.time, t.Key);
}
}
}
foreach (int agent in newDic.Keys)
{
int currentMax = 0;
TimedMove lastMove = null;
foreach (int time in newDic[agent].Keys)
{
if (currentMax <= time)
{
currentMax = time;
lastMove = newDic[agent][time];
}
}
for (int i = currentMax + 1; i < maxPlan; i++)
{
newDic[agent].Add(i, new TimedMove(lastMove));
}
}
return(newDic);
}
/// <summary>
/// Also checks if the move is illegal
/// </summary>
/// <param name="toCheck"></param>
/// <returns></returns>
public bool IsValid(TimedMove toCheck)
{
if (IsValidTile(toCheck.x, toCheck.y) == false)
{
return(false);
}
if (parameters.ContainsKey("ID-reserved"))
{
var reserved = (HashSet <TimedMove>)parameters["ID-reserved"];
return(toCheck.IsColliding(reserved) == false);
}
return(true);
}
/// <summary>
/// Also checks if the move is illegal
/// </summary>
/// <param name="toCheck"></param>
/// <returns></returns>
public bool IsValid(TimedMove toCheck)
{
if (IsValidTile(toCheck.x, toCheck.y) == false)
{
return(false);
}
if (parameters.ContainsKey(IndependenceDetection.ILLEGAL_MOVES_KEY))
{
var reserved = (HashSet <TimedMove>)parameters[IndependenceDetection.ILLEGAL_MOVES_KEY];
return(toCheck.IsColliding(reserved) == false);
} // FIXME: Should this be here?
return(true);
}
} // Nonsense values until Init, just allocate move
public CbsConstraint(CbsConflict conflict, ProblemInstance instance, bool agentA, Run.ConstraintPolicy constraintPolicy = Run.ConstraintPolicy.Single, int constraintRange = 0)
{
Move move;
int agentNum;
int minTime;
this.constaintRange = Math.Abs(conflict.timeStepAgentB - conflict.timeStepAgentA);
if (constraintPolicy == Run.ConstraintPolicy.Range)
{
minTime = Math.Min(conflict.timeStepAgentA, conflict.timeStepAgentB);
}
else if (constraintPolicy == Run.ConstraintPolicy.DoubleRange)
{
minTime = conflict.timeStepAgentA;
}
else
{
if (agentA)
{
minTime = conflict.timeStepAgentA;
}
else
{
minTime = conflict.timeStepAgentB;
}
}
if (agentA)
{
move = conflict.agentAmove;
agentNum = instance.m_vAgents[conflict.agentAIndex].agent.agentNum;
this.move = new TimedMove(move, minTime);
}
else
{
move = conflict.agentBmove;
agentNum = instance.m_vAgents[conflict.agentBIndex].agent.agentNum;
this.move = new TimedMove(move, minTime);
}
this.agentNum = (byte)agentNum;
if (conflict.vertex)
{
this.move.direction = Move.Direction.NO_DIRECTION;
}
}
public void AddPlanToCAT(IDictionary <TimedMove, List <int> > addTo, int until)
{
for (int i = 0; i <= until; i++)
{
TimedMove step = new TimedMove(this.GetLocationAt(i), i); // TODO: Avoid creating new objects. Make the method return correctly timed moves.
if (addTo.ContainsKey(step))
{
addTo[step].Add(this.agentIndex);
}
else
{
var agentList = new List <int>();
agentList.Add(this.agentIndex);
addTo.Add(step, agentList);
}
}
}
private LinkedList <List <Move> > copyLinkedList(LinkedList <List <Move> > toCopy)
{
LinkedList <List <Move> > newLinkedList = new LinkedList <List <Move> >();
LinkedListNode <List <Move> > node = toCopy.First;
while (node != null)
{
List <Move> newList = new List <Move>();
foreach (Move mv in node.Value)
{
Move newMove = new TimedMove((TimedMove)mv);
newList.Add(newMove);
}
newLinkedList.AddLast(newList);
node = node.Next;
}
return(newLinkedList);
}
// Summary:
// Determines whether the read-only dictionary contains an element that has
// the specified key.
//
// Parameters:
// key:
// The key to locate.
//
// Returns:
// true if the read-only dictionary contains an element that has the specified
// key; otherwise, false.
//
// Exceptions:
// System.ArgumentNullException:
// key is null.
public bool ContainsKey(TimedMove key)
{
if (this.timedMovesToAgentNumList.ContainsKey(key))
{
return(true);
}
queryMove.setup(key);
if (this.atGoalWaitsToTimeAndAgentNum.ContainsKey(queryMove))
{
var timeAndAgentNum = this.atGoalWaitsToTimeAndAgentNum[queryMove];
if (key.time >= timeAndAgentNum.Item1)
{
return(true);
}
}
return(false);
}
/// <summary>
/// The returned plan wasn't constructed considering a CAT, so it's possible there's an alternative plan with the same cost and less collisions.
/// </summary>
/// <param name="agentState"></param>
/// <returns></returns>
public SinglePlan GetSingleAgentOptimalPlan(AgentState agentState,
out Dictionary <int, int> conflictCountPerAgent, out Dictionary <int, List <int> > conflictTimesPerAgent)
{
LinkedList <Move> moves = new LinkedList <Move>();
int agentNum = agentState.agent.agentNum;
var conflictCounts = new Dictionary <int, int>();
var conflictTimes = new Dictionary <int, List <int> >();
IReadOnlyDictionary <TimedMove, List <int> > CAT;
if (this.parameters.ContainsKey(CBS.CAT)) // TODO: Add support for IndependenceDetection's CAT
{
CAT = ((IReadOnlyDictionary <TimedMove, List <int> >) this.parameters[CBS.CAT]);
}
else
{
CAT = new Dictionary <TimedMove, List <int> >();
}
TimedMove current = agentState.lastMove; // The starting position
int time = current.time;
while (true)
{
moves.AddLast(current);
// Count conflicts:
current.UpdateConflictCounts(CAT, conflictCounts, conflictTimes);
if (agentState.agent.Goal.Equals(current))
{
break;
}
// Get next optimal move
time++;
Move optimal = this.singleAgentOptimalMoves[agentNum][this.GetCardinality(current)];
current = new TimedMove(optimal, time);
}
conflictCountPerAgent = conflictCounts;
conflictTimesPerAgent = conflictTimes;
return(new SinglePlan(moves, agentNum));
}
/// <summary>
/// Check if the move is valid, i.e. not colliding into walls or other agents.
/// This method is here instead of in ProblemInstance to enable algorithmic tweaks.
/// </summary>
/// <param name="possibleMove">The move to check if possible</param>
/// <returns>true, if the move is possible.</returns>
protected bool IsValid(TimedMove possibleMove, HashSet <TimedMove> currentMoves, int makespan, int agentNum)
{
// Check if the proposed move is reserved in the plan of another agent.
// This is used in Trevor's IndependenceDetection.
if (this.illegalMoves != null)
{
if (possibleMove.IsColliding(illegalMoves))
{
return(false);
}
} // FIXME: Also checked in instance.IsValid later.
if (this.constraintList != null)
{
queryConstraint.Init(agentNum, possibleMove);
if (this.constraintList.Contains(queryConstraint))
{
return(false);
}
}
if (this.mustConstraints != null && makespan < this.mustConstraints.Length && // There may be a constraint on the timestep of the generated node
this.mustConstraints[makespan] != null)
{
if (this.mustConstraints[makespan].Any <CbsConstraint>(
con => con.ViolatesMustConstraint((byte)agentNum, possibleMove)))
{
return(false);
}
}
// If the tile is not free (out of the grid or with an obstacle)
if (this.instance.IsValid(possibleMove) == false)
{
return(false);
}
// Check against all the agents that have already moved to see if current move collides with their move
return(possibleMove.IsColliding(currentMoves) == false);
}
