예제 #1
0
        /// <summary>
        /// Single impulse transfer from an ellipitical, non-coplanar parking orbit to an arbitrary hyperbolic v-infinity target.
        ///
        /// Ocampo, C., & Saudemont, R. R. (2010). Initial Trajectory Model for a Multi-Maneuver Moon-to-Earth Abort Sequence.
        /// Journal of Guidance, Control, and Dynamics, 33(4), 1184–1194.
        /// </summary>
        ///
        /// <param name="mu">Gravitational parameter of central body.</param>
        /// <param name="r0">Reference position on the parking orbit.</param>
        /// <param name="v0">Reference velocity on the parking orbit.</param>
        /// <param name="v_inf">Target hyperbolic v-infinity vector.</param>
        /// <param name="vneg">Velocity on the parking orbit before the burn.</param>
        /// <param name="vpos">Velocity on the hyperboliic ejection orbit after the burn.</param>
        /// <param name="r">Position of the burn.</param>
        /// <param name="dt">Coasting time on the parking orbit from the reference to the burn.</param>
        ///
        public static void singleImpulseHyperbolicBurn(double mu, Vector3d r0, Vector3d v0, Vector3d v_inf,
                                                       ref Vector3d vneg, ref Vector3d vpos, ref Vector3d r, ref double dt, bool debug = false)
        {
            double   rot, dv;
            BrentFun f = delegate(double testrot, object ign) {
                double   dt   = 0;
                Vector3d vneg = new Vector3d();
                Vector3d vpos = new Vector3d();
                Vector3d r    = new Vector3d();
                singleImpulseHyperbolicBurn(mu, r0, v0, v_inf, ref vneg, ref vpos, ref r, ref dt, (float)testrot, debug);
                return((vpos - vneg).magnitude);
            };

            Brent.Minimize(f, -30, 30, 1e-6, out rot, out dv, null);
            singleImpulseHyperbolicBurn(mu, r0, v0, v_inf, ref vneg, ref vpos, ref r, ref dt, (float)rot, debug);
        }
예제 #2
0
파일: ODE.cs 프로젝트: vinix38/MechJeb2
        // Dormand Prince 5(4)7FM ODE integrator (aka DOPRI5)
        //
        // We prefer this over Bogacki–Shampine in order to obtain the free 4th order interpolant.
        //
        // f       - the ODE function to integrate
        // o       - object paramter which is passed through to the ODE function
        // y0      - array of starting y0 values
        // n       - dimensionality of the problem
        // xtbl    - array of x values to evaluate at (2 minimum for start + end)
        // ytbl    - output jagged array of y values at the x evaluation points (n x 2 minimum)
        // eps     - accuracy
        // h       - starting step-size (can be zero for automatic guess)
        // xlist   - output list of all intermediate x values (user allocates, can be null or omitted)
        // ylist   - output list of all intermediate y values (user allocates, can be null or omitted)
        // hmin    - minimum h step (may be violated on the last step)
        // hmax    - maximum h step
        // maxiter - maximum number of steps
        // EvtFuns - array of event functions
        //
        // ref: https://github.com/scipy/scipy/blob/master/scipy/integrate/dop/dopri5.f
        //
        public static void RKDP547FM(ODEFun f, object o, double[] y0, int n, Span <double> xtbl, Span <double[]> ytbl, double eps, double hstart, List <double> xlist = null, List <double []> ylist = null, double hmin = 0, double hmax = 0, int maxiter = 0, EvtFun[] EvtFuns = null)
        {
            Span <double> k1 = stackalloc double[n];
            Span <double> k2 = stackalloc double[n];
            Span <double> k3 = stackalloc double[n];
            Span <double> k4 = stackalloc double[n];
            Span <double> k5 = stackalloc double[n];
            Span <double> k6 = stackalloc double[n];
            Span <double> k7 = stackalloc double[n];
            // accumulator
            Span <double> a = stackalloc double[n];
            // error
            Span <double> err = stackalloc double[n];
            double        h   = hstart;
            double        x   = xtbl[0];
            Span <double> y   = stackalloc double[n];

            if (ylist != null && xlist != null)
            {
                ylist.Clear();
                xlist.Clear();
            }

            y0.CopyTo(y);

            // auto-guess starting value of h based on smallest dx in xtbl
            if (h == 0)
            {
                double v = Math.Abs(xtbl[1] - xtbl[0]);
                for (int i = 1; i < xtbl.Length - 1; i++)
                {
                    v = Math.Min(v, Math.Abs(xtbl[i + 1] - xtbl[i]));
                }
                h = 0.001 * v;
            }

            // xtbl controls the direcction of integration, the sign of h is not relevant
            h = Math.Sign(xtbl[1] - xtbl[0]) * Math.Abs(h);

            // copy initial conditions to ybtl output
            int j = 0;

            for (int i = 0; i < n; i++)
            {
                ytbl[i][j] = y[i];
            }
            j++;

            // copy initial conditions to full xlist/ylist output
            if (xlist != null && ylist != null)
            {
                double[] ydup2 = new double[n];
                y.CopyTo(ydup2);
                ylist.Add(ydup2);
                xlist.Add(x);
            }

            bool   fsal = false;
            bool   at_hmin;
            double h_restart = 0;
            double niter     = 0;

            while (j < xtbl.Length)
            {
                double xf = xtbl[j];

                while ((h > 0) ? x <xf : x> xf)
                {
                    at_hmin = false;
                    if (hmax > 0 && Math.Abs(h) > hmax)
                    {
                        h = hmax * Math.Sign(h);
                    }
                    if (hmin > 0 && Math.Abs(h) < hmin)
                    {
                        h       = hmin * Math.Sign(h);
                        at_hmin = true;
                    }

                    // we may violate hmin in order to exactly hit the boundary conditions
                    if (Math.Abs(h) > Math.Abs(xf - x))
                    {
                        h = xf - x;
                    }

                    if (fsal)
                    {
                        // FIXME: tricks to make this copy go away?
                        for (int i = 0; i < n; i++)
                        {
                            k1[i] = k7[i];
                        }
                    }
                    else
                    {
                        f(y, x, k1, n, o);
                    }

                    for (int i = 0; i < n; i++)
                    {
                        a[i] = y[i] + h * (a21 * k1[i]);
                    }
                    f(a, x + c2 * h, k2, n, o);

                    for (int i = 0; i < n; i++)
                    {
                        a[i] = y[i] + h * (a31 * k1[i] + a32 * k2[i]);
                    }
                    f(a, x + c3 * h, k3, n, o);

                    for (int i = 0; i < n; i++)
                    {
                        a[i] = y[i] + h * (a41 * k1[i] + a42 * k2[i] + a43 * k3[i]);
                    }
                    f(a, x + c4 * h, k4, n, o);

                    for (int i = 0; i < n; i++)
                    {
                        a[i] = y[i] + h * (a51 * k1[i] + a52 * k2[i] + a53 * k3[i] + a54 * k4[i]);
                    }
                    f(a, x + c5 * h, k5, n, o);

                    for (int i = 0; i < n; i++)
                    {
                        a[i] = y[i] + h * (a61 * k1[i] + a62 * k2[i] + a63 * k3[i] + a64 * k4[i] + a65 * k5[i]);
                    }
                    f(a, x + c6 * h, k6, n, o);

                    for (int i = 0; i < n; i++)
                    {
                        a[i] = y[i] + h * (a71 * k1[i] + a73 * k3[i] + a74 * k4[i] + a75 * k5[i] + a76 * k6[i]);
                    }
                    f(a, x + c7 * h, k7, n, o);

                    for (int i = 0; i < n; i++)
                    {
                        err[i] = k1[i] * (b1 - b1p) + k3[i] * (b3 - b3p) + k4[i] * (b4 - b4p) + k5[i] * (b5 - b5p) + k6[i] * (b6 - b6p) + k7[i] * (b7 - b7p);
                    }

                    double error = 0;
                    for (int i = 0; i < n; i++)
                    {
                        // FIXME: look at dopri fortran code to see how they generate this
                        error = Math.Max(error, Math.Abs(err[i]));
                    }
                    double s = 0.84 * Math.Pow(eps / error, 1.0 / 5.0);

                    int evt = -1;

                    if (error < eps || at_hmin || h_restart != 0)
                    {
                        int sign = 0;

                        if (EvtFuns != null)
                        {
                            for (int i = 0; i < EvtFuns.Length; i++)
                            {
                                EvtFun e  = EvtFuns[i];
                                double e1 = e(y, x, n, o);
                                double e2 = e(a, x + h, n, o);
                                if (e1 * e2 < 0)
                                {
                                    evt = i;
                                    // sign is passed to Brent to ensure we select a value on the other side of the event
                                    sign = Math.Sign(e2);
                                    break;
                                }
                            }
                        }

                        if (evt < 0 || h_restart != 0)
                        {
                            fsal = true; // advancing so use k7 for k1 next time
                            for (int i = 0; i < n; i++)
                            {
                                y[i] = a[i]; // FSAL
                            }
                            x = x + h;

                            if (xlist != null && ylist != null)
                            {
                                double[] ydup = new double[n];
                                for (int i = 0; i < n; i++)
                                {
                                    ydup[i] = y[i];
                                }
                                ylist.Add(ydup);
                                xlist.Add(x);
                            }
                            if (h_restart != 0)
                            {
                                h         = h_restart;
                                s         = 1;
                                h_restart = 0;
                            }
                        }
                        else
                        {
                            fsal = false;
                            s    = 1;
                            if (h_restart == 0)
                            {
                                h_restart = h;
                            }
                            Brent.Root((newh, a1, a2, a3, a4, o2) => EvtWrapper(EvtFuns[evt], newh, x, a1, a2, x + h, a3, a4, n, o2), 0, h, 1e-15, out h, out _, o, y, k1, a, k7, sign: sign);
                        }
                    }
                    else
                    {
                        fsal = false; // rewinding so k7 is no longer valid
                    }

                    if (s < 0.1)
                    {
                        s = 0.1;
                    }
                    if (s > 4)
                    {
                        s = 4;
                    }
                    h = h * s;

                    if (maxiter > 0 && niter++ >= maxiter)
                    {
                        throw new ArgumentException("maximum iterations exceeded");
                    }
                }

                for (int i = 0; i < n; i++)
                {
                    ytbl[i][j] = y[i];
                }

                j++;
            }
        }