Wrong position during orbit propagation in Unity

Question

I've been working on an orbital physics module for months now, and have reached a barrier. I have two methods, one to convert from Kepler elements to planet-centric coordinates, and the other to determine the Kepler elements from the state vectors R and V. The PositionFromOrbit() method propagates the body around the orbit, but it starts displaced from it's initial place in the orbit. Then, after one period it starts jumping from point to point on the orbit. Here is an example:

The blue cube is the current position of the object before runtime, and the red outline is the calculated position of the object according to it's Keplerian elements. I can see that the outline is exactly on top of the apoapsis, but I don't know how or why. Here's the main snippets, starting with the conversion of state vectors to Keplerian elements:

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;

        }

And here's the method to find its position:

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;

        }

Can anyone spot where the I or the methods are failing at their task?

My starting vectors: R = 15, -0.07, 3.36 V = -0.39, 5.14, -0.04

I have had this issue for a few weeks now and it would be a great breakthrough if it was fixed. I have tried adding/subtracting the argument of periapsis from true anomaly like a noob but it made the orbit inaccurate. I tried relating the position to the time at periapsis and that also did not work. I adopted three other methods that didn't work as well as the current one.

Thank you, one and all!

Edit: Found another symptom of the issue or another: After a smooth orbit, the object begins to jump around on its orbit. It travels from periapsis to apoapsis and back before it starts to jitter.

Edit: The jittering has been solved, thanks to notovny, who reccommended I remove % (2 * Pi) when solving for the eccentric anomaly.

Answer 1

If you want high fidelity orbit propagation, you should store the orbit in Cartesian form because it's a non-singular orbit representation. It's also the simplest form to account for extra forces on the spacecraft because you only need to integrate $\vec F = m \vec a$ and apply it to the Cartesian state without trying to figure out how the given force changes eccentricity, semi-major axis, etc.

For the conversion from Keplerian to Cartesian representation, the algorithm is not that straight forward (if you want to account for all possible orbits). Here is a link to the exact function in GMAT C++ source code (148 C++ lines) and here is the "translation" in Nyx (80 lines in Rust, which I find clearer). To confirm that this translation is correct, here is the link to the validation of that function in Nyx.