public void T03_Constrained()
{
// a constrained minimization problem using a barrier function to minimize x^2 subject to x > 5
MinimumBracket bracket = Minimize.BracketInward(new BarrierFunction(1), 5 + (5.0 / 2) * IEEE754.DoublePrecision, 100, 100).First();
double value;
Assert.AreEqual(5, Minimize.Brent(new BarrierFunction(1e-10), bracket, out value), 1.2e-7);
Assert.AreEqual(25, value, 1.2e-6);
// test a constrained minimization problem using various methods
ConstrainedMinimizer minimizer;
double[] point;
foreach (ConstraintEnforcement method in (ConstraintEnforcement[])Enum.GetValues(typeof(ConstraintEnforcement)))
{
// test the same problem using the ConstrainedMinimizer
point = new double[2] {
5, 8
};
minimizer = new ConstrainedMinimizer(new ConstraintTestFunction())
{
ConstraintEnforcement = method
};
minimizer.SetBounds(0, 1, double.PositiveInfinity);
minimizer.SetBounds(1, 3, 9);
value = minimizer.Minimize(point);
switch (method)
{
case ConstraintEnforcement.InverseBarrier: // ~ 220 function calls and 180 gradient evaluations
Assert.AreEqual(9, value, 1.4e-8);
Assert.AreEqual(1, point[0], 2.4e-10);
Assert.AreEqual(3, point[1], 4.1e-10);
break;
case ConstraintEnforcement.LinearPenalty: // ~ 870 function calls and 120 gradient evaluations
Assert.AreEqual(9, value, 4.4e-11);
Assert.AreEqual(1, point[0], 2.5e-12);
Assert.AreEqual(3, point[1], 2.5e-12);
break;
case ConstraintEnforcement.LogBarrier: // ~ 140 function calls and 130 gradient evaluations
Assert.AreEqual(9, value, 5.1e-9);
Assert.AreEqual(1, point[0], 6e-12);
Assert.AreEqual(3, point[1], 1.7e-11);
break;
case ConstraintEnforcement.QuadraticPenalty: // ~ 670 function calls and 170 gradient evaluations
Assert.AreEqual(9, value, 9e-9);
Assert.AreEqual(1, point[0], 9e-10);
Assert.AreEqual(3, point[1], 4e-10);
break;
default: throw new NotImplementedException();
}
}
// test the constrained minimizer with a tricky problem -- finding the minimum of the rosenbrock banana constrained to an
// arbitrary line that doesn't pass through the minimum of the original problem
minimizer = new ConstrainedMinimizer(new RosenbrockBanana());
minimizer.AddConstraint(new LineConstraint(-1.5, -1.5, 0, 1.5, 0));
point = new double[2] {
-1.2, 2
};
value = minimizer.Minimize(point);
Assert.AreEqual(2.4975, value, 2e-9);
Assert.AreEqual(-0.57955689989313142185, point[0], 8e-10);
Assert.AreEqual(0.34088620021373715629, point[1], 1e-9);
}
bool UpdateSolution()
{
double?speed = null, time = null, aob = null, radius = null;
if (!string.IsNullOrEmpty(txtSpeed.Text.Trim()))
{
double value;
if (!TryParseSpeed(txtSpeed.Text, unitSystem, out value))
{
ShowInvalidSpeed(txtSpeed.Text);
goto invalidData;
}
speed = value;
}
if (!string.IsNullOrEmpty(txtTime.Text.Trim()))
{
TimeSpan timeSpan;
bool relative;
if (!TryParseTime(txtTime.Text, out timeSpan, out relative))
{
ShowInvalidTime(txtTime.Text, false, false);
goto invalidData;
}
time = timeSpan.TotalSeconds;
}
if (!string.IsNullOrEmpty(txtAoB.Text.Trim()))
{
double value;
if (!TryParseAngle(txtAoB.Text.Trim(), out value))
{
ShowInvalidAngle(txtAoB.Text);
goto invalidData;
}
aob = value;
}
if (!string.IsNullOrEmpty(txtRadius.Text.Trim()))
{
double value;
if (!TryParseLength(txtRadius.Text, unitSystem, out value))
{
ShowInvalidLength(txtRadius.Text);
goto invalidData;
}
if (value != 0)
{
radius = value;
}
}
if (speed.HasValue && time.HasValue)
{
lblSolution.Text = "Remove speed or time.";
return(false);
}
if (aob.HasValue && !radius.HasValue)
{
lblSolution.Text = "Using AoB requires a radius.";
return(false);
}
if (radius.HasValue && !aob.HasValue)
{
lblSolution.Text = "Using a radius requires AoB.";
return(false);
}
Point2 targetPt;
Vector2 targetVel;
double targetCourse;
if (target != null)
{
targetPt = target.Position;
targetVel = target.GetEffectiveVelocity();
targetCourse = target.Direction;
}
else
{
double bearing, range;
if (string.IsNullOrEmpty(txtBearing.Text))
{
lblSolution.Text = "Enter a target bearing.";
return(false);
}
else if (!TryParseAngle(txtBearing.Text, out bearing))
{
ShowInvalidAngle(txtBearing.Text);
goto invalidData;
}
if (string.IsNullOrEmpty(txtRange.Text))
{
lblSolution.Text = "Enter a target range.";
return(false);
}
else if (!TryParseLength(txtRange.Text, unitSystem, out range))
{
ShowInvalidLength(txtRange.Text);
goto invalidData;
}
targetPt = new Vector2(0, range).Rotate(-bearing).ToPoint();
double targetSpeed;
if (string.IsNullOrEmpty(txtTargetSpeed.Text))
{
lblSolution.Text = "Enter a target speed.";
return(false);
}
else if (!TryParseSpeed(txtTargetSpeed.Text, unitSystem, out targetSpeed))
{
ShowInvalidSpeed(txtTargetSpeed.Text);
goto invalidData;
}
if (targetSpeed == 0)
{
targetCourse = 0;
}
else if (string.IsNullOrEmpty(txtCourse.Text))
{
lblSolution.Text = "Enter a target course.";
return(false);
}
else if (!TryParseAngle(txtCourse.Text, out targetCourse))
{
ShowInvalidAngle(txtCourse.Text);
goto invalidData;
}
targetVel = new Vector2(0, targetSpeed).Rotate(-targetCourse);
}
// if AoB was specified, then we're actually trying to intercept a single point on the radius circle, so make that are target point
// and use the standard point intercept algorithm
if (aob.HasValue)
{
targetPt += new Vector2(0, radius.Value).Rotate(-(targetCourse + aob.Value));
}
// if we've already satisfied the intercept criteria...
Vector2 o = unit == null ? new Vector2(targetPt) : targetPt - unit.Position;
if (o.LengthSqr <= (radius.HasValue && !aob.HasValue ? radius.Value * radius.Value : 0))
{
lblSolution.Text = "You are already there.";
return(false);
}
// if the target is not moving, any speed will work, so we'll just arbitrarily head there at 10 units of speed
if (targetVel.LengthSqr == 0)
{
Solution = o.GetNormal(MB.ConvertFromUnit(10, MB.GetAppropriateSpeedUnit(unitSystem)));
InterceptPoint = targetPt;
lblSolution.Text = ManeuveringBoard.GetAngleString(MB.SwapBearing(o.Angle)) + " (target stationary)";
return(true);
}
// if we have a single target point (i.e. the target itself, or one point on the radius circle), the intercept formula basically
// consists in solving a quadratic formula. if we're at P and the target is at T with velocity V, then we know that the interception
// point is at T+V*t, where t is time. if we take the vector from P to the intersection point (T+V*t-P), then the distance is
// |T+V*t-P|. dividing by the speed s gives us the time: t = |(T+V*t-P)|/s. so s*t = |T+V*t-P|. squaring both sides, replacing T-P
// with the helper O (i.e. translating P to the origin), and expanding the vector, we get (s*t)^2 = (Ox+Vx*t)^2 + (Oy+Vy*t)^2
if (!time.HasValue) // if the user didn't specify the time of intercept... (if they did, the problem's solved already)
{
if (!speed.HasValue)
{
// if the user specified no information, there are an infinite number of solutions. we'll choose the one that requires
// approximately the lowest intercept speed. if we solve the quadratic equation for speed instead of time, we get
// s = sqrt((Ox + Vx*t)^2 + (Oy + Vy*t)^2) / t. we need to minimize this equation where s >= 0 and t >= 0. there's probably
// some way to do this analytically, but it seems complicated, so we'll just minimize it numerically. we'll represent the time
// TODO: check out the inverse of this function. it may be more sloped and easier to optimize?
// TODO: consider a better method of rescaling (to get all the numbers about the same magnitude)
Func <double, double> speedFunc = t =>
{
if (t <= 0)
{
return(double.NaN);
}
t *= 3600; // we'll scale the time from seconds to hours because the minimizer is a bit more stable when params are around 1.0
double x = o.X + targetVel.X * t, y = o.Y + targetVel.Y * t;
return(Math.Sqrt(x * x + y * y) / t);
};
Func <double, double> derivative = t =>
{
if (t <= 0)
{
return(double.NaN);
}
t *= 3600;
double x = o.X + targetVel.X * t, y = o.Y + targetVel.Y * t;
return(-(o.X * x + o.Y * y) / (t * t * Math.Sqrt(x * x + y * y)));
};
ConstrainedMinimizer minimizer = new ConstrainedMinimizer(new DifferentiableMDFunction(speedFunc, derivative));
// the function tends to be very flat near the minimum, so it's hard to find it exactly. increase the gradient tolerance to
// prevent it from failing as often. we're going to be rounding the answer anyway, so it needn't be exact
minimizer.GradientTolerance = 1e-6;
// constrain the solution to be non-negative
minimizer.AddConstraint(new DifferentiableMDFunction(t => - speedFunc(t), t => - derivative(t)));
minimizer.SetBounds(0, 0, double.MaxValue); // constrain t to be non-negative
try
{
double[] point = new double[] { 1 };
double minSpeed = minimizer.Minimize(point);
// now we want to round the speed up to the next unit, to make it look nicer. we also want to increase the speed by a small
// amount because the time seems to increase without bound as the speed nears the minimum. for instance, a difference of
// 0.01 kn near the minimum might increase the time by 5 hours. speeds near the minimum also render the math below very
// inaccurate due to mismatching precision. so we'll add about a knot to the speed as well as round it up
SpeedUnit speedUnit = MB.GetAppropriateSpeedUnit(unitSystem);
speed = MB.ConvertFromUnit(Math.Ceiling(MB.ConvertToUnit(minSpeed + 0.5, speedUnit)), speedUnit); // 0.5 m/s ~= 1 kn
}
catch (MinimumNotFoundException)
{
lblSolution.Text = "Try setting parameters.";
return(false);
}
}
// if know the intercept speed, we take the above equation and factor out time. we end up with:
// t^2(Vx^2 + Vy^2 - s^2) + t*(2Ox*Vx + 2Oy*Vy) + Ox^2 + Oy^2 = 0. if we take A=(Vx^2 + Vy^2 - s^2), B=2(Ox*Vx + Oy*Vy), and
// C=Ox^2 + Oy^2, then we have the quadratic A*t^2 + B*t + C = 0 which we can solve with the quadratic formula.
// t = (-B +/- sqrt(B^2 - 4AC)) / 2A (and we can remove a factor of 2). if the discriminant is negative, there is no solution.
// otherwise, we take whichever solution gives the smallest non-negative time
double A = targetVel.X * targetVel.X + targetVel.Y * targetVel.Y - speed.Value * speed.Value, B = o.X * targetVel.X + o.Y * targetVel.Y, C = o.X * o.X + o.Y * o.Y;
// if A = 0, then the speeds are identical, and we get division by zero solving the quadratic. but if A = 0 then we just have
// B*t + C = 0 or t = -C/B. we know B can't be zero because we checked the relevant cases above
if (A == 0)
{
double t = -C / (2 * B); // we have to multiply B by 2 because we removed a factor of two above
if (t >= 0)
{
time = t;
}
else
{
goto noSolution;
}
}
else
{
double discriminant = B * B - A * C;
if (discriminant < 0)
{
goto noSolution;
}
double root = Math.Sqrt(discriminant), time1 = (-B + root) / A, time2 = (-B - root) / A;
if (time1 >= 0 && time1 <= time2)
{
time = time1;
}
else if (time2 >= 0)
{
time = time2;
}
else
{
goto noSolution;
}
}
}
// now that we know the time of intercept, we can calculate the intercept point and get the velocity we'd need to get there.
// the intercept point is T+V*t. the intercept vector is T+V*t-P = O+V*t. this vector has a length equal to the distance, but we
// want a length equal to the speed, so divide by time to get speed (e.g. 10 km in 2 hour = 5km/hour). but (O+V*t)/t = O/t+V
Solution = o / time.Value + targetVel;
InterceptPoint = targetPt + targetVel * time.Value;
haveSolution = true;
lblSolution.Text = ManeuveringBoard.GetAngleString(MB.SwapBearing(Solution.Angle)) + " at " +
MB.GetSpeedString(Solution.Length, unitSystem) + " for " + GetTimeString(time.Value);
return(true);
noSolution:
lblSolution.Text = "No intercept is possible.";
return(false);
invalidData:
lblSolution.Text = "Invalid data.";
return(false);
}