public void CartToKep(Vector3D pos, Vector3D vel)//This doesn't work, feel free to try and fix it.
        {
            this.pos = pos;
            this.vel = vel;
            en = vel.LengthSquared() / 2 - planet.u / pos.Length();
            a = Math.Abs(planet.u / (2 * en));
            h = Vector3D.Cross(pos, vel);
            E = Vector3D.Cross(vel, h) / planet.u - (pos / pos.Length());
            Console.WriteLine(E);
            e = E.Length();
            I = Math.Acos(h.GetDim(2) / h.Length());
            N = new Vector3D(-h.GetDim(1), h.GetDim(0), 0);
            if (N.GetDim(1) >= 0)
            {
                q = Math.Acos(N.GetDim(0) / N.Length());
                q = double.IsNaN(q) ? 0 : q;
            }
            else
            {
                q = 2 * Math.PI - Math.Acos(N.GetDim(0) / N.Length());
                q = double.IsNaN(q) ? 2*Math.PI : q;
            }

            n = Math.Sqrt(planet.u / Math.Pow(a, 3));
            if (q != 0)
            {
                w = E.GetDim(2) >= 0 ? Math.Acos(Vector3D.Dot(N, E) / (N.Length() * E.Length())) : Math.PI * 2 - Math.Acos(Vector3D.Dot(N, E) / (N.Length() * E.Length()));

            }
            else
            {
                double W=Math.Atan2(E.GetDim(1),E.GetDim(0));
                w = h.GetDim(2) < 0 ? 2*Math.PI-W:W;


            }

            if (double.IsNaN(w)) w = 0;
            if (w < 0) w += Math.PI * 2;
            epoch = DateTime.Now;
            if (E.Length() != 0)
            {
                v = Vector3D.Dot(pos, vel) >= 0 ? Math.Acos(Vector3D.Dot(E, pos) / (E.Length() * pos.Length())) : Math.PI * 2 - Math.Acos(Vector3D.Dot(E, pos) / (E.Length() * pos.Length()));
            }
            else
            {
                if (I != 0)
                {
                    v = Vector3D.Dot(N, vel) <= 0 ? Math.Acos(Vector3D.Dot(N, pos) / (N.Length() * pos.Length())) : Math.PI * 2 - Math.Acos(Vector3D.Dot(N, pos) / (N.Length() * pos.Length()));
                    Console.WriteLine(N);
                }
                else
                {
                    v = vel.GetDim(0) <= 0 ? Math.Acos(pos.GetDim(0) / (pos.Length())) : Math.PI * 2 - Math.Acos(pos.GetDim(0) / (pos.Length()));
                }
            }
            if (double.IsNaN(v)) v = 0;
            lastTa = v;
            double sinE = sin(a) * Math.Sqrt(1 - Math.Pow(e, 2)) / (1 + e * cos(a));
            double cosE = (e + cos(a)) / (1 + e * cos(a));
            double Ec = Math.Atan2(sinE, cosE);
            EC = 2 * Math.Atan(Math.Tan(v / 2) / Math.Sqrt(1 + e / 1 - e));
            EC = EC < 0 ? EC + Math.PI * 2 : EC;
            if (double.IsNaN(EC)) EC = 0;
            M = EC - e * Math.Sin(EC);
            lastM = M;
            Period = Math.PI * 2 * Math.Sqrt(Math.Pow(a,3)/planet.u);
        }