public static Orbit OrbitFromStateVectors(Vector3d pos, Vector3d vel, CelestialBody body, double UT) { Orbit ret = new Orbit(); ret.UpdateFromStateVectors(OrbitExtensions.SwapYZ(pos - body.position), OrbitExtensions.SwapYZ(vel), body, UT); if (double.IsNaN(ret.argumentOfPeriapsis)) { Vector3d vectorToAN = Quaternion.AngleAxis(-(float)ret.LAN, Planetarium.up) * Planetarium.right; Vector3d vectorToPe = OrbitExtensions.SwapYZ(ret.eccVec); double cosArgumentOfPeriapsis = Vector3d.Dot(vectorToAN, vectorToPe) / (vectorToAN.magnitude * vectorToPe.magnitude); //Squad's UpdateFromStateVectors is missing these checks, which are needed due to finite precision arithmetic: if (cosArgumentOfPeriapsis > 1) { ret.argumentOfPeriapsis = 0; } else if (cosArgumentOfPeriapsis < -1) { ret.argumentOfPeriapsis = 180; } else { ret.argumentOfPeriapsis = Math.Acos(cosArgumentOfPeriapsis); } } return(ret); }
public static Orbit OrbitFromStateVectors(Vector3d pos, Vector3d vel, CelestialBody body, double UT) { Orbit ret = new Orbit(); ret.UpdateFromStateVectors(OrbitExtensions.SwapYZ(pos - body.position), OrbitExtensions.SwapYZ(vel), body, UT); return(ret); }