protected void addToGlobalConflictCount(CbsConflict conflict)
{
if (conflict != null)
{
globalConflictsCounter[Math.Max(conflict.agentAIndex, conflict.agentBIndex)][Math.Min(conflict.agentAIndex, conflict.agentBIndex)]++;
}
}
} // 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;
}
}
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);
}
