/// <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.sumOfCostsVariant == Constants.SumOfCostsVariant.ORIG)
{
if (this.AtGoal())
{
this.g = this.arrivalTime;
}
else
{
this.g = this.lastMove.time;
}
}
else if (Constants.sumOfCostsVariant == Constants.SumOfCostsVariant.WAITING_AT_GOAL_ALWAYS_FREE)
{
if ((atGoal && isWait) == false)
{
this.g += 1;
}
}
}
public new TimedMove GetMoveWithoutDirection()
{
TimedMove copy = new TimedMove(this);
copy.direction = Direction.NO_DIRECTION;
return(copy);
}
/// <summary>
/// Gets a combined list of the values mapped to the specified key in the combined dictionaries.
/// </summary>
/// <param name="key">The key to locate.</param>
/// <returns>A combined list of the values mapped to the specified key in the combined dictionaries.</returns>
/// <exception cref="System.ArgumentNullException">key is null.</exception>
/// <exception cref="System.Collections.Generic.KeyNotFoundException">key is not found.</exception>
public new IReadOnlyList <int> this[TimedMove key]
{
get
{
if (Data.Count > 1)
{
var ret = new List <int>();
foreach (ConflictAvoidanceTable cat in Data)
{
if (cat.ContainsKey(key))
{
ret.AddRange(cat[key]);
}
}
if (ret.Count == 0)
{
throw new KeyNotFoundException();
}
return(ret);
}
else
{
return(Data.First()[key]);
}
}
}
public bool ViolatesMustConstraint(byte agent, TimedMove move)
{
if (this.agentNum != agent)
{
return(false);
}
return(this.move.Equals(move) == false);
}
public AgentState(AgentState copy)
{
this.agent = copy.agent;
this.h = copy.h;
this.arrivalTime = copy.arrivalTime;
this.lastMove = copy.lastMove; //new TimedMove(copy.lastMove); // Can we just do this.lastMove = copy.lastMove? I think we can now, since MoveTo replaces the move
//this.prev = copy;
this.g = copy.g;
}
/// <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);
}
return(true);
}
/// <summary>
/// Returns whether the specified key exists in any of the combined dictionaries
/// </summary>
/// <param name="key">The key to look for</param>
/// <returns></returns>
public new bool ContainsKey(TimedMove key)
{
foreach (ConflictAvoidanceTable item in Data)
{
if (item.ContainsKey(key))
{
return(true);
}
}
return(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);
}
}
/// <summary>
/// Update conflict counts according to what happens after the plan finishes -
/// needed if the plan is shorter than one of the previous plans and collides
/// with it while at the goal.
/// It's cheaper to do it this way than to force the solver the go more deeply.
/// The conflict counts are saved at the group's representative.
/// </summary>
protected void IncrementConflictCountsAtGoal(IndependenceDetectionAgentsGroup group, ConflictAvoidanceTable CAT)
{
for (int i = 0; i < group.allAgentsState.Length; ++i)
{
var afterGoal = new TimedMove(group.allAgentsState[i].agent.Goal.x, group.allAgentsState[i].agent.Goal.y, Move.Direction.Wait, time: 0);
for (int time = group.GetPlan().GetSize(); time < CAT.GetMaxPlanSize(); time++)
{
afterGoal.time = time;
afterGoal.IncrementConflictCounts(CAT,
this.conflictCountsPerGroup[group.groupNum],
this.conflictTimesPerGroup[group.groupNum]);
}
}
}
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)
{
coexistingNodesFromOtherMdds = new HashSet <MDDNode> [numOfAgents];
for (int i = 0; i < numOfAgents; i++)
{
coexistingNodesFromOtherMdds[i] = new HashSet <MDDNode>(5); // Each level is small
}
}
}
private bool isValidMove(TimedMove move)
{
if (this.problem.IsValid(move) == false)
{
return(false);
}
if (move.IsColliding(this.reservationTable))
{
return(false);
}
this.queryMove.setup(move.x, move.y, Move.Direction.NO_DIRECTION);
if (parked.ContainsKey(this.queryMove) && parked[this.queryMove] <= move.time)
{
return(false);
}
return(true);
}
private bool singleAgentAStar(AgentState agent)
{
AgentState.EquivalenceOverDifferentTimes = false;
BinaryHeap <AgentState> openList = new BinaryHeap <AgentState>(); // TODO: Safe to use OpenList here instead?
HashSet <AgentState> closedList = new HashSet <AgentState>();
agent.h = this.problem.GetSingleAgentOptimalCost(agent);
openList.Add(agent);
AgentState node;
this.initialEstimate += agent.h;
TimedMove queryTimedMove = new TimedMove();
while (openList.Count > 0)
{
if (this.runner.ElapsedMilliseconds() > Constants.MAX_TIME)
{
return(false);
}
node = openList.Remove();
if (node.h == 0)
{
bool valid = true;
for (int i = node.lastMove.time; i <= maxPathCostSoFar; i++)
{
queryTimedMove.setup(node.lastMove.x, node.lastMove.y, Move.Direction.NO_DIRECTION, i);
if (reservationTable.Contains(queryTimedMove))
{
valid = false;
}
}
if (valid)
{
this.paths[agent.agent.agentNum] = new SinglePlan(node);
reservePath(node);
totalcost += node.lastMove.time;
parked.Add(new Move(node.lastMove.x, node.lastMove.y, Move.Direction.NO_DIRECTION), node.lastMove.time);
return(true);
}
}
expandNode(node, openList, closedList);
expanded++;
}
return(false);
}
} // Nonsense values until Init, just allocate move
public CbsConstraint(CbsConflict conflict, ProblemInstance instance, bool agentA)
{
Move move;
int agentNum;
if (agentA)
{
move = conflict.agentAmove;
agentNum = instance.agents[conflict.agentAIndex].agent.agentNum;
}
else
{
move = conflict.agentBmove;
agentNum = instance.agents[conflict.agentBIndex].agent.agentNum;
}
this.agentNum = (byte)agentNum;
this.move = new TimedMove(move, conflict.timeStep);
if (conflict.isVertexConflict)
{
this.move.direction = Move.Direction.NO_DIRECTION;
}
}
/// <summary>
/// Note: 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>An optimal plan for the agent, ignoring all others</returns>
public SinglePlan GetSingleAgentOptimalPlan(AgentState agentState)
{
LinkedList <Move> moves = new LinkedList <Move>();
int agentNum = agentState.agent.agentNum;
TimedMove current = agentState.lastMove; // The starting position
int time = current.time;
while (true)
{
moves.AddLast(current);
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);
}
return(new SinglePlan(moves, agentNum));
}
public AgentState(int pos_X, int pos_Y, Agent agent)
{
this.lastMove = new TimedMove(pos_X, pos_Y, Move.Direction.NO_DIRECTION, 0);
this.agent = agent;
}
public void Init(int agentNum, TimedMove move)
{
this.agentNum = (byte)agentNum;
this.move = move;
}
public CbsConstraint(int agentNum, TimedMove move)
{
this.Init(agentNum, move);
}
/// <summary>
/// Check if the given move collides with this move.
/// This includes:
/// 0. Same time
/// 1. Head on collision
/// 2. When other moves target the same location.
/// </summary>
/// <param name="other"></param>
/// <returns></returns>
public bool IsColliding(TimedMove other)
{
return(IsColliding(other.x, other.y, other.direction, other.time));
}
/// <summary>
///
/// </summary>
/// <param name="cpy"></param>
public void setup(TimedMove cpy)
{
base.setup(cpy);
this.time = cpy.time;
}
public TimedMove(TimedMove cpy) : base(cpy)
{
this.time = cpy.time;
}
