Esempio n. 1
0
        public static void Solve(Vector3d R1, Vector3d R2, double dt, double gravParameter, bool shortway, out Vector3d V1, out Vector3d V2)
        {
            double muCB = gravParameter;

            double R1mag = R1.magnitude;
            double R2mag = R2.magnitude;

            double tm;

            if (shortway)
            {
                tm = 1.0;
            }
            else
            {
                tm = -1.0;
            }

            double cosDeltaTA = (Vector3d.Dot(R1, R2)) / (R1mag * R2mag);
            double sinDeltaTA = tm * Math.Sqrt(1 - Math.Pow(cosDeltaTA, 2));
            double deltaTA    = Math.Atan2(sinDeltaTA, cosDeltaTA);

            if (deltaTA < 0)
            {
                deltaTA = deltaTA + 2 * Math.PI;
            }

            double c = Math.Sqrt(Math.Pow(R1mag, 2) + Math.Pow(R2mag, 2) - 2 * R1mag * R2mag * cosDeltaTA);

            double s = (R1mag + R2mag + c) / 2;

            double epsilon = (R2mag - R1mag) / R1mag;

            double intemed1 = R2mag / R1mag;
            double TanSqr2w = (Math.Pow(epsilon, 2) / 4) / (Math.Sqrt(intemed1) + intemed1 * (2 + Math.Sqrt(intemed1)));

            double sinSqrDeltaTAOver4 = Math.Pow(Math.Sin(deltaTA / 4), 2);
            double cosSqrDeltaTAOver4 = Math.Pow(Math.Cos(deltaTA / 4), 2);
            double rop = Math.Sqrt(R1mag * R2mag) * (cosSqrDeltaTAOver4 + TanSqr2w);

            double l;

            if (shortway)
            {
                l = (sinSqrDeltaTAOver4 + TanSqr2w) / (sinSqrDeltaTAOver4 + TanSqr2w + Math.Cos(deltaTA / 2));
            }
            else
            {
                l = (cosSqrDeltaTAOver4 + TanSqr2w - Math.Cos(deltaTA / 2)) / (cosSqrDeltaTAOver4 + TanSqr2w);
            }

            double m = (muCB * Math.Pow(dt, 2)) / (8 * Math.Pow(rop, 3));

            double x        = l;
            double x_change = 1;

            double y     = 0;
            int    loops = 0;

            do
            {
                double ksi = ComputeKsi(x, 8);

                double h1 = (Math.Pow((l + x), 2) * (1 + 3 * x + ksi)) / ((1 + 2 * x + l) * (4 * x + ksi * (3 + x)));
                double h2 = (m * (x - l + ksi)) / ((1 + 2 * x + l) * (4 * x + ksi * (3 + x)));

                double[]           polyConsts = { -h2, 0, -(1 + h1), 1 };
                PolynomialFunction yEqnPoly   = new PolynomialFunction(polyConsts);

                //const double relativeAccuracy = 1.0e-12;
                //const double absoluteAccuracy = 1.0e-12;
                //BracketingNthOrderBrentSolver solver = new BracketingNthOrderBrentSolver(relativeAccuracy, absoluteAccuracy, 10);
                //y = solver.solve(10000, yEqnPoly, -10000, 10000, 0.0, AllowedSolution.ANY_SIDE);

                //replaced Arrowstar's above four commented lines with this solver:  -The_Duck
                //Use an initial guess of 10; NewtonSolver will get stuck with an initial guess of zero.
                y = NewtonSolver.Solve(yEqnPoly, 10, 1.0e-12, 50);

                double x_new = Math.Sqrt(Math.Pow((1 - l) / 2, 2) + m / Math.Pow(y, 2)) - (1 + l) / 2;
                x_change = Math.Abs(x - x_new);
                x        = x_new;
                loops++;
            } while (x_change > Math.Pow(10, -6) && loops < 30);

            double a = (muCB * Math.Pow(dt, 2)) / (16 * Math.Pow(rop, 2) * x * Math.Pow(y, 2));

            double       f     = 0;
            double       g     = 0;
            double       g_dot = 0;
            const double small = 1e-5;

            if (a > small)
            {
                double sinBetaEOver2 = Math.Sqrt((s - c) / (2 * a));
                double betaE         = 2 * Math.Asin(sinBetaEOver2);
                if (deltaTA > Math.PI)
                {
                    betaE = -betaE;
                }

                double amin = s / 2;
                double tmin = Math.Sqrt(Math.Pow(amin, 3) / muCB) * (Math.PI - betaE + Math.Sin(betaE));

                double alphaE = 2 * Math.Asin(Math.Sqrt(s / (2 * a)));
                if (dt > tmin)
                {
                    alphaE = 2 * Math.PI - alphaE;
                }

                double deltaE = alphaE - betaE;

                f     = 1 - (a / R1mag) * (1 - Math.Cos(deltaE));
                g     = dt - Math.Sqrt(Math.Pow(a, 3) / muCB) * (deltaE - Math.Sin(deltaE));
                g_dot = 1 - (a / R2mag) * (1 - Math.Cos(deltaE));
            }
            else if (a < -small)
            {
                //Asinh asinh=new Asinh();
                //double alphaH = 2*asinh.value(Math.Sqrt(s/(-2*a)));
                //double betaH = 2*asinh.value(Math.Sqrt((s-c)/(-2*a)));
                //Porting Arrowstar's above three lines to: -The_Duck
                double alphaH = 2 * MuUtils.Asinh(Math.Sqrt(s / (-2 * a)));
                double betaH  = 2 * MuUtils.Asinh(Math.Sqrt((s - c) / (-2 * a)));

                double deltaH = alphaH - betaH;

                f     = 1 - (a / R1mag) * (1 - Math.Cosh(deltaH));
                g     = dt - Math.Sqrt(-Math.Pow(a, 3) / muCB) * (Math.Sinh(deltaH) - deltaH);
                g_dot = 1 - (a / R2mag) * (1 - Math.Cosh(deltaH));
            }
            else
            {
                //List<Vector3d> VArr = null;
                //commented out the above line from Arrowstar's original, since it seems to do nothing and throws a compiler error -The_Duck
            }

            V1 = (R2 - (f * R1)) / g;
            V2 = (g_dot * R2 - R1) / g;
        }