/// <summary>
/// Cubic interpolation routine from Nocedal and Wright
/// </summary>
/// <param name="a">first point, with value and derivative</param>
/// <param name="b">second point, with value and derivative</param>
/// <returns>local minimum of interpolating cubic polynomial</returns>
private static Float CubicInterp(StepValueDeriv a, StepValueDeriv b)
{
Float t1 = a.Deriv + b.Deriv - 3 * (a.Value - b.Value) / (a.Step - b.Step);
Float t2 = Math.Sign(b.Step - a.Step) * MathUtils.Sqrt(t1 * t1 - a.Deriv * b.Deriv);
Float num = b.Deriv + t2 - t1;
Float denom = b.Deriv - a.Deriv + 2 * t2;
return(b.Step - (b.Step - a.Step) * num / denom);
}
private Float FindMinimum(DiffFunc1D func, Float initValue, Float initDeriv)
{
Contracts.CheckParam(initDeriv < 0, nameof(initDeriv), "Cannot search in direction of ascent!");
StepValueDeriv lo = new StepValueDeriv(func, 0, initValue, initDeriv);
StepValueDeriv hi = new StepValueDeriv(func, _step);
// bracket minimum
while (hi.Deriv < 0)
{
Swap(ref lo, ref hi);
if (lo.Step >= MaxStep)
{
return(MaxStep);
}
hi.Step = lo.Step * 2;
}
Float window = 1;
StepValueDeriv mid = new StepValueDeriv(func);
for (int numSteps = 1; ; ++numSteps)
{
Float interp = CubicInterp(lo, hi);
if (window <= MinWindow || numSteps == MaxNumSteps)
{
return(interp);
}
// insure minimal progress to narrow interval
Float minProgressStep = _minProgress * (hi.Step - lo.Step);
Float maxMid = hi.Step - minProgressStep;
if (interp > maxMid)
{
interp = maxMid;
}
Float minMid = lo.Step + minProgressStep;
if (interp < minMid)
{
interp = minMid;
}
mid.Step = interp;
if (mid.Deriv == 0 || mid.Step == lo.Step || mid.Step == hi.Step)
{
return(mid.Step);
}
if (mid.Deriv < 0)
{
Swap(ref lo, ref mid);
}
else
{
Swap(ref hi, ref mid);
}
if (lo.Step >= MaxStep)
{
return(MaxStep);
}
window = (hi.Step - lo.Step) / hi.Step;
}
}
