/// <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)
{
if (obj == null)
{
return(false);
}
CbsConstraint other = (CbsConstraint)obj;
if (this.agentNum != other.agentNum)
{
return(false);
}
Trace.Assert(this.queryInstance == false || other.queryInstance == false); // At most one of the instances is a query
Trace.Assert(this.queryInstance == false || this.move.direction != Move.Direction.NO_DIRECTION); // Must query regarding a specific direction
Trace.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
// TODO: Get rid of all of this using Nathan's advice.
{
return(this.move.Equals(other.move) && this.move.direction == other.move.direction);
}
}
/// <summary>
/// Kind of the opposite of Equals: checks that the moves are unequal or that not one of the other's agents appears in this.agents.
/// </summary>
/// <param name="other"></param>
/// <returns></returns>
public bool Allows(CbsConstraint other)
{
if (this.move.Equals(other.move) == false) // Minor behavior change: if exactly one move has a set direction, and they're otherwise equal the method used to return true.
{
return(true);
}
if (this.agentNum == other.agentNum)
{
return(false);
}
return(true);
}
public MDD(MDD other, CbsConstraint newConstraint)
: this(other)
{
List <MDDNode> toDelete = new List <MDDNode>();
foreach (MDDNode mddNode in this.levels[newConstraint.time])
{
if (newConstraint.move.Equals(mddNode.move))
{
toDelete.Add(mddNode);
}
}
Trace.Assert(toDelete.Count > 0);
foreach (MDDNode mddNode in toDelete)
{
mddNode.delete();
}
}
public int CompareTo(object item)
{
CbsConstraint other = (CbsConstraint)item;
return(this.move.time.CompareTo(other.move.time));
}
public virtual bool Expand(CbsNode node, CbsConflict conflict)
{
if (runner.ElapsedMilliseconds() > Constants.MAX_TIME)
{
return(false);
}
highLevelExpanded++;
if (conflict == null)
{
this.totalCost = node.g;
this.goalNode = node;
this.solution = node.CalculateJointPlan();
this.Clear();
return(true);
}
CbsNode toAdd;
CbsConstraint con2 = new CbsConstraint(conflict, instance, false);
CbsConstraint con1 = new CbsConstraint(conflict, instance, true);
byte stepLength = 0;
if (conflict.isVertexConflict)
{
stepLength = 1;
}
bool ok1 = false, ok2 = false;
if (node.f + conflict.timeStep + stepLength - node.PathLength(conflict.agentAIndex) <= fBound)
{
ok1 = true;
if (node.DoesMustConstraintAllow(con1))
{
toAdd = new CbsNode(node, con1, conflict.agentAIndex);
toAdd.SetMustConstraint(con2);
if (toAdd.Replan3b(conflict.agentAIndex, Math.Max(minDepth, conflict.timeStep)))
{
this.highLevelGenerated++;
if (toAdd.f <= fBound)
{
if (Expand(toAdd, toAdd.GetConflict()))
{
return(true);
}
}
else if (toAdd.f < nextF)
{
nextF = toAdd.f;
}
}
}
}
if (node.f + conflict.timeStep + stepLength - node.PathLength(conflict.agentBIndex) <= fBound)
{
ok2 = true;
if (node.DoesMustConstraintAllow(con2))
{
toAdd = new CbsNode(node, con2, conflict.agentBIndex);
toAdd.SetMustConstraint(con1);
if (toAdd.Replan3b(conflict.agentBIndex, Math.Max(minDepth, conflict.timeStep)))
{
this.highLevelGenerated++;
if (toAdd.f <= fBound)
{
if (Expand(toAdd, toAdd.GetConflict()))
{
return(true);
}
}
else if (toAdd.f < nextF)
{
nextF = toAdd.f;
}
}
}
}
if (ok1 && ok2)
{
toAdd = new CbsNode(node, con1, conflict.agentAIndex);
if (toAdd.Replan3b(conflict.agentAIndex, Math.Max(minDepth, conflict.timeStep)))
{
if (toAdd.f <= fBound)
{
toAdd = new CbsNode(toAdd, con2, conflict.agentBIndex);
if (toAdd.Replan(conflict.agentBIndex, Math.Max(minDepth, conflict.timeStep)))
// FIXME: Should this really use the regular Replan() or was this a typo?
{
this.highLevelGenerated++;
if (toAdd.f <= fBound)
{
if (Expand(toAdd, toAdd.GetConflict()))
{
return(true);
}
}
else if (toAdd.f < nextF)
{
nextF = toAdd.f;
}
}
}
}
}
return(false);
}