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); }