public CFMCbsNode(int numberOfAgents, CFM_CBS cbs, ushort[] agentsGroupAssignment = null)
{
this.cbs = cbs;
mamPlan = null;
mamCost = -1;
allSingleAgentCosts = new int[numberOfAgents];
countsOfInternalAgentsThatConflict = new int[numberOfAgents];
this.nodeConflicts = null;
if (agentsGroupAssignment == null)
{
this.agentsGroupAssignment = new ushort[numberOfAgents];
for (ushort i = 0; i < numberOfAgents; i++)
{
this.agentsGroupAssignment[i] = i;
}
}
else
{
this.agentsGroupAssignment = agentsGroupAssignment.ToArray <ushort>();
}
agentNumToIndex = new Dictionary <int, int>();
for (int i = 0; i < numberOfAgents; i++)
{
agentNumToIndex[this.cbs.GetProblemInstance().m_vAgents[i].agentIndex] = i;
}
depth = 0;
replanSize = 1;
agentAExpansion = ExpansionState.NOT_EXPANDED;
agentBExpansion = ExpansionState.NOT_EXPANDED;
this.prev = null;
this.constraint = null;
this.solver = solver;
this.singleAgentSolver = singleAgentSolver;
}
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>
/// Child from merge action constructor. FIXME: Code dup with previous constructor.
/// </summary>
/// <param name="father"></param>
/// <param name="mergeGroupA"></param>
/// <param name="mergeGroupB"></param>
public CFMCbsNode(CFMCbsNode father, int mergeGroupA, int mergeGroupB)
{
mamPlan = null;
mamCost = -1;
this.allSingleAgentCosts = father.allSingleAgentCosts.ToArray <int>();
this.countsOfInternalAgentsThatConflict = father.countsOfInternalAgentsThatConflict.ToArray <int>();
this.nodeConflicts = null;
this.agentsGroupAssignment = father.agentsGroupAssignment.ToArray <ushort>();
this.agentNumToIndex = father.agentNumToIndex;
this.prev = father;
this.constraint = null;
this.depth = (ushort)(this.prev.depth + 1);
this.agentAExpansion = ExpansionState.NOT_EXPANDED;
this.agentBExpansion = ExpansionState.NOT_EXPANDED;
this.replanSize = 1;
this.solver = father.solver;
this.singleAgentSolver = father.singleAgentSolver;
this.cbs = father.cbs;
}
/// <summary>
/// Checks that the agentNum is equal, and compares the move.
/// If one of the constraints is a query, an instance only created and used to quickly search for a move in a set of constraints,
/// the direction is ignored if the other constraint is a vertex constraint.
/// </summary>
/// <param name="obj"></param>
/// <returns></returns>
public override bool Equals(object obj)
{
CFMCbsConstraint other = (CFMCbsConstraint)obj;
if (this.agentNum != other.agentNum)
{
return(false);
}
Debug.Assert(this.queryInstance == false || other.queryInstance == false); // At most one of the instances is a query
Debug.Assert(this.queryInstance == false || this.move.direction != Move.Direction.NO_DIRECTION); // Must query regarding a specific direction
Debug.Assert(other.queryInstance == false || other.move.direction != Move.Direction.NO_DIRECTION); // Must query regarding a specific direction
if (this.queryInstance || other.queryInstance) // This way if the constraint is a vertex constraint than it will be equal to a query containing a move from any direction to that position,
// and if it is an edge constraint than it will only be equal to queries containing a move from that specific direction to that position.
{
return(this.move.Equals(other.move));
}
else // A vertex constraint is different to an edge constraint for the same agentNum and position.
// Must check the direction explicitly because vertex constraints have no direction and moves with no direction
// compare equal to moves with any direction
{
return(this.move.Equals(other.move) && this.move.direction == other.move.direction);
}
}
public int CompareTo(object item)
{
CFMCbsConstraint other = (CFMCbsConstraint)item;
return(this.move.time.CompareTo(other.move.time));
}
public MMStarConstraint(CFMCbsConstraint constraint)
: this(constraint.agentNum, constraint.move.x, constraint.move.y, constraint.move.direction, constraint.move.time)
{
}
/// <summary>
/// Create Constraints from conflict
/// </summary>
/// <param name="node"></param>
/// <param name="doLeftChild"></param>
/// <param name="closedListHitChildCost"></param>
/// <returns></returns>
protected CFMCbsNode ConstraintExpand(CFMCbsNode node, bool doLeftChild, out int closedListHitChildCost)
{
CFMCbsConflict conflict = node.GetConflict();
int conflictingAgentIndex = doLeftChild? conflict.agentAIndex : conflict.agentBIndex;
CFMCbsNode.ExpansionState expansionsState = doLeftChild ? node.agentAExpansion : node.agentBExpansion;
CFMCbsNode.ExpansionState otherChildExpansionsState = doLeftChild ? node.agentBExpansion : node.agentAExpansion;
string agentSide = doLeftChild? "left" : "right";
int groupSize = node.GetGroupSize(conflictingAgentIndex);
closedListHitChildCost = -1;
if (expansionsState != CFMCbsNode.ExpansionState.EXPANDED)
// Agent expansion already skipped in the past or not forcing it from its goal - finally generate the child:
{
if (debug)
{
Debug.WriteLine("Generating " + agentSide + " child");
}
if (doLeftChild)
{
node.agentAExpansion = CFMCbsNode.ExpansionState.EXPANDED;
}
else
{
node.agentBExpansion = CFMCbsNode.ExpansionState.EXPANDED;
}
var newConstraint = new CFMCbsConstraint(conflict, instance, doLeftChild);
CFMCbsNode child = new CFMCbsNode(node, newConstraint, conflictingAgentIndex);
if (closedList.ContainsKey(child) == false)
{
bool success = child.Solve();
if (success == false)
{
return(null); // A timeout probably occured
}
return(child);
}
else
{
this.closedListHits++;
closedListHitChildCost = this.closedList[child].totalCost;
if (debug)
{
Debug.WriteLine("Child already in closed list!");
}
}
}
else
{
if (debug)
{
Debug.WriteLine("Child already generated before");
}
}
return(null);
}
