Ejemplo n.º 1
0
        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);
        }
Ejemplo n.º 2
0
        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);
        }