Неверное положение при движении по орбите в Unity

Я работаю над модулем орбитальной физики уже несколько месяцев и достиг барьера. У меня есть два метода: один для преобразования элементов Кеплера в планетарные координаты, а другой — для определения элементов Кеплера из векторов состояния R и V . Метод PositionFromOrbit()распространяет тело по орбите, но оно начинает смещаться со своего начального места на орбите. Затем, через один период, он начинает прыгать с точки на точку на орбите. Вот пример:

Синий куб — ​​это текущая позиция объекта перед выполнением, а красный контур — расчетная позиция объекта в соответствии с его кеплеровскими элементами. Я вижу, что контур находится точно над апоцентром, но я не знаю, как и почему. Вот основные фрагменты, начиная с преобразования векторов состояния в элементы Кеплера:

public static KeplerOrbit OrbitFromState(Vector3Decimal R, Vector3Decimal V, decimal mu)
        {
            Vector3Decimal h = Vector3Decimal.Cross(R, V);
            Vector3Decimal n = Vector3Decimal.Cross(Vector3Decimal.forward, h);
            Vector3Decimal eVec = ((V.magnitude * V.magnitude - mu / R.magnitude) * R - Vector3Decimal.Dot(R, V) * V) / mu; // eccentricity vector
            decimal eMag = eVec.magnitude;
            decimal E = V.magnitude * V.magnitude / 2 - mu / R.magnitude;
            decimal a;
            decimal p;
            if(eVec.magnitude != 1)
            {
                a = -mu / (2 * E);
                p = a * (1 - eMag * eMag);
            }
            else
            {
                a = 0;
                p = h.magnitude * h.magnitude / mu;
            }
            decimal inc = ACos(h.z / h.magnitude);

            decimal O;
            if(inc == 0 || inc == Pi)
            {
                O = 0;
            }
            else
            {
                O = ACos(n.x / n.magnitude);
            }

            if(n.y < 0)
            {
                O = 2 * Pi - O;
            }

            decimal w;
            if (eVec.magnitude == 0)
            {
                w = 0;
            }
            else
            {
                if (inc == 0 || inc == Pi)
                {
                    w = ATan2(eVec.y, eVec.x);
                }
                else
                {
                    w = ACos(Vector3Decimal.Dot(n, eVec) / (n.magnitude * eVec.magnitude));
                }
            }
            if(eVec.z < 0)
            {
                w = 2 * Pi - w;
            }

            decimal nu;
            if(eVec.magnitude == 0)
            {
                if(inc == 0 || inc == Pi)
                {
                    nu = ACos(R.z / R.magnitude);
                }
                else
                {
                    nu = ACos(Vector3Decimal.Dot(n, R) / (n.magnitude * R.magnitude));
                }
            }
            else
            {
                nu = ACos(Vector3Decimal.Dot(eVec, R) / (eVec.magnitude * R.magnitude));
                if(Vector3Decimal.Dot(R,V) < 0)
                {
                    nu = 2 * Pi - nu;
                }
            }

            KeplerOrbit ret = new KeplerOrbit(E, a, eVec.magnitude, inc, O, w, nu);
            return ret;

        }

И вот метод, чтобы найти его положение:

public static Vector3Decimal PositionFromOrbit(KeplerOrbit orbit, decimal t, decimal mu)
        {
            decimal a = orbit.semi_major_axis;
            decimal e = orbit.eccentricity;
            decimal i = orbit.inclination;
            decimal O = orbit.longitude_of_ascent;
            decimal w = orbit.argument_of_periapsis;
            //decimal T = Period(orbit.semi_major_axis, mu);
            decimal t0 = TimeAtEccentricAnomaly(a, e, 0, mu);
            decimal Mt;
            if(t == t0)
            {
                t = t0;
                Mt = 0;
            }
            else
            {
                decimal deltaT = (t - t0);
                Mt = deltaT * Sqrt(mu / (a * a * a));
            }

            decimal E = Mt;
            decimal F = E - e * Sin(E) - Mt;
            int j = 0;
            int maxIter = 30;
            decimal delta = 0.000001m;
            while(Abs(F) > delta && j < maxIter)
            {
                //E = (E - F / (1 - e * Cos(E))) % (Pi * 2);
                E = (E - F / (1 - e * Cos(E))); // this is the line that fixed jittering.
                F = E - e * Sin(E) - Mt;
                j++;
            }

            decimal nu = 2 * ATan2(Sqrt(1 + e) * Sin(E / 2), Sqrt(1 - e) * Cos(E / 2));
            decimal rc = a * (1 - e * Cos(E));

            Vector3Decimal o = new Vector3Decimal(rc * Cos(nu), rc * Sin(nu), 0);
            Vector3Decimal odot = new Vector3Decimal(Sin(E), Sqrt(1 - e * e) * Cos(E), 0);
            odot = odot * (Sqrt(mu * a) / rc);
            decimal rx, ry, rz;
            rx = o.x; ry = o.y; rz = o.z;

            rx = (o.x * (Cos(w) * Cos(O) - Sin(w) * Cos(i) * Sin(O)) - o.y * (Sin(w) * Cos(O) + Cos(w) * Cos(i) * Sin(O)));
            ry = (o.x * (Cos(w) * Sin(O) + Sin(w) * Cos(i) * Cos(O)) + o.y * (Cos(w) * Cos(i) * Cos(O) - Sin(w) * Sin(O)));
            rz = (o.x * (Sin(w) * Sin(i)) + o.y * (Cos(w) * Sin(i)));

            Vector3Decimal r = new Vector3Decimal(rx, ry, rz);
            return r;

        }

Может ли кто-нибудь определить, где «я» или методы не справляются со своей задачей?

Мои начальные векторы: R = 15, -0,07, 3,36 V = -0,39, 5,14, -0,04

У меня была эта проблема в течение нескольких недель, и было бы большим прорывом, если бы она была исправлена. Я пытался добавить/вычесть аргумент перицентра из истинной аномалии, как нуб, но это сделало орбиту неточной. Я попытался связать положение со временем в перицентре, но это тоже не сработало. Я использовал три других метода, которые работали не так хорошо, как текущий.

Спасибо, один и все!

Редактировать: Обнаружен еще один симптом проблемы или другой: после плавной орбиты объект начинает прыгать по своей орбите. Он перемещается от перицентра к апоапсису и обратно, прежде чем начинает дрожать.

Редактировать: дрожание было решено благодаря notovny, который рекомендовал удалить % (2 * Pi)при решении эксцентрической аномалии.