/// <summary>
///
/// Assumes g of node was already calculated!
/// </summary>
/// <param name="s"></param>
/// <returns></returns>
public uint h(WorldState s)
{
int sicEstimate;
if (Constants.costFunction == Constants.CostFunction.SUM_OF_COSTS)
{
sicEstimate = (int)SumIndividualCosts.h(s, this.instance);
}
else if (Constants.costFunction == Constants.CostFunction.MAKESPAN || Constants.costFunction == Constants.CostFunction.MAKESPAN_THEN_SUM_OF_COSTS)
{
sicEstimate = (int)MaxIndividualCosts.h(s, this.instance);
}
else
{
throw new Exception($"Unsupported cost function {Constants.costFunction}");
}
if (sicEstimate == 0) // Only the goal has an estimate of zero
{
return(0);
}
int targetCost = s.g + sicEstimate + this.minAboveSic; // Ariel's idea - using SIC directly here to calc the target
// CBS gets an explicitly partially solved state - the agents' g may be greater than zero.
// So the cost CBS is going to calc is not of this node but of the initial problem instance,
// this is accounted for later too.
// (Notice node usually has a (possibly too low) h set already - inherited from the parent)
return(this.h(s, targetCost, sicEstimate));
}
/// <summary>
/// Computes a heuristic by running a bounded CBS search from the given node.
/// Assumes g of node was already calculated and h isn't zero.
/// </summary>
/// <param name="s"></param>
/// <param name="targetCost">Stop when the target cost is reached</param>
/// <param name="sicEstimate">For a debug assertion.</param>
/// <param name="lowLevelGeneratedCap">The number of low level nodes to generate</param>
/// <param name="milliCap">The process total millisecond count to stop at</param>
/// <param name="resume">Whether to resume the last search instead of solving the given node. Assumes the last search was from the same node as the given node.</param>
/// <returns></returns>
protected uint h(WorldState s, int targetCost, int sicEstimate = -1, int lowLevelGeneratedCap = -1,
int milliCap = int.MaxValue, bool resume = false)
{
double start = this.runner.ElapsedMilliseconds();
ProblemInstance sAsProblemInstance;
if (resume == false)
{
this.cbs.Clear();
sAsProblemInstance = s.ToProblemInstance(this.instance);
this.cbs.Setup(sAsProblemInstance,
Math.Max(s.makespan, // This forces must-constraints to be upheld when dealing with A*+OD nodes,
// at the cost of forcing every agent to move when a goal could be found earlier with all must constraints upheld.
s.minGoalTimeStep), // No point in finding shallower goal nodes
this.runner, null, null, null);
if (this.cbs.openList.Count > 0 && this.cbs.topMost)
{
if (sicEstimate == -1)
{
if (Constants.costFunction == Constants.CostFunction.SUM_OF_COSTS)
{
sicEstimate = (int)SumIndividualCosts.h(s, this.instance);
}
else if (Constants.costFunction == Constants.CostFunction.MAKESPAN || Constants.costFunction == Constants.CostFunction.MAKESPAN_THEN_SUM_OF_COSTS)
{
sicEstimate = (int)MaxIndividualCosts.h(s, this.instance);
}
else
{
throw new Exception($"Unsupported cost function {Constants.costFunction}");
}
}
Trace.Assert(((CbsNode)this.cbs.openList.Peek()).g - s.g == (int)sicEstimate,
"Total cost of CBS root not same as SIC + g");
// Notice we're subtracting s.g, not sAsProblemInstance.g.
// Must constraints we put may have forced some moves,
// and we shouldn't count them as part of the estimate.
}
}
else
{
sAsProblemInstance = this.cbs.GetProblemInstance();
}
if (lowLevelGeneratedCap == -1)
{
// Rough estimate of the branching factor:
lowLevelGeneratedCap = (int)Math.Pow(Constants.NUM_ALLOWED_DIRECTIONS, this.instance.agents.Length);
}
// Calc the h:
this.cbs.targetF = targetCost;
this.cbs.milliCap = milliCap;
this.cbs.lowLevelGeneratedCap = lowLevelGeneratedCap;
bool solved = this.cbs.Solve();
if (solved && this.reportSolution)
{
// We're always going to find a proper goal since we respected the node's minDepth
s.SetSolution(this.cbs.GetSinglePlans());
s.SetGoalCost(this.cbs.solutionCost); // We have to do it explicitly.
// We can't just change the node's g to g + cbs.g and its h to zero
// because approaches like BPMX or maximazing PDBs might "fix" the h back.
// So instead h is bumped to its maximum value when this method returns.
s.SetSingleCosts(this.cbs.GetSingleCosts());
this.nodesSolved++;
}
double end = this.runner.ElapsedMilliseconds();
this.totalRuntime += end - start;
this.nCalls++;
this.cbs.AccumulateStatistics();
this.cbs.ClearStatistics();
if (this.cbs.solutionCost < 0) // A timeout is legitimately possible if very little time was left to begin with,
// and a no solution failure may theoretically be possible too.
{
if (Constants.costFunction == Constants.CostFunction.SUM_OF_COSTS)
{
return(SumIndividualCosts.h(s, this.instance));
}
else if (Constants.costFunction == Constants.CostFunction.MAKESPAN || Constants.costFunction == Constants.CostFunction.MAKESPAN_THEN_SUM_OF_COSTS)
{
return(MaxIndividualCosts.h(s, this.instance));
}
else
{
throw new Exception($"Unsupported cost function {Constants.costFunction}");
}
}
Trace.Assert(this.cbs.solutionCost >= s.g,
$"CBS total cost {this.cbs.solutionCost} is smaller than starting problem's initial cost {s.g}."); // = is allowed since even though this isn't a goal node (otherwise this function won't be called),
// a non-goal node can have h==0 if a minimum depth is specified, and all agents have reached their
// goal in this node, but the depth isn't large enough.
uint cbsEstimate = (uint)(this.cbs.solutionCost - s.g);
// Discounting the moves the agents did before we started solving
// (This is easier than making a copy of each AgentState just to zero its lastMove.time)
this.totalImprovement += (int)(cbsEstimate - s.h); // Not computing difference from SIC to not over-count, since a node can be improved twice.
// Can be negative if the base heuristic was improved by:
// - Partial expansion
// - BPMX
if (validate)
{
// Brute-force validation of admissibility of estimate:
IHeuristicCalculator <WorldState> heuristic;
if (Constants.costFunction == Constants.CostFunction.SUM_OF_COSTS)
{
heuristic = new SumIndividualCosts();
}
else if (Constants.costFunction == Constants.CostFunction.MAKESPAN || Constants.costFunction == Constants.CostFunction.MAKESPAN_THEN_SUM_OF_COSTS)
{
heuristic = new MaxIndividualCosts();
}
else
{
throw new Exception($"Unsupported cost function {Constants.costFunction}");
}
heuristic.Init(this.instance, this.agentsToConsider);
var epeastarsic = new EPEA_Star(heuristic);
epeastarsic.Setup(sAsProblemInstance, s.makespan, runner);
bool epeastarsicSolved = epeastarsic.Solve();
if (epeastarsicSolved)
{
Trace.Assert(epeastarsic.totalCost - s.g >= this.cbs.solutionCost - s.g, "Inadmissible!!");
}
}
return(cbsEstimate);
}