diff --git a/src/orbital_mechanics.cpp b/src/orbital_mechanics.cpp index 10a03d0..fb1d124 100644 --- a/src/orbital_mechanics.cpp +++ b/src/orbital_mechanics.cpp @@ -240,10 +240,44 @@ OrbitalElements cartesian_to_orbital_elements(Vec3 position, Vec3 velocity, doub double r_mag = vec3_magnitude(r_vec); double r_dot_e = vec3_dot(r_vec, e_vec); - // Near-circular: e ≈ 0, true anomaly undefined + // Ascending node vector: n = k × h_vec (k is unit Z vector) + Vec3 n_vec = {0.0, 0.0, 1.0}; + Vec3 n = vec3_cross(n_vec, h_vec); + double n_mag = vec3_magnitude(n); + + // True anomaly: angle from periapsis to position double true_anomaly; if (e < 1e-10) { - true_anomaly = 0.0; + // Circular orbit: no periapsis direction. Use argument of latitude + // (position angle in orbital plane) as true anomaly, with omega=0. + // For nearly-coplanar orbits, the ascending node is numerically + // unstable. Use the inclination to decide which reference to use. + double true_anomaly_from_position; + double sin_i = (h > 1e-10) ? n_mag / h : 1.0; + if (sin_i > 1e-6 && n_mag > 1e-10) { + // Well-defined ascending node: compute argument of latitude + double x_AN = n.x / n_mag; + double y_AN = n.y / n_mag; + // y_AN in orbital plane = (h × n) / |h × n| + double h_cross_n_x = h_vec.y * 0.0 - h_vec.z * n.y; + double h_cross_n_y = h_vec.z * n.x - h_vec.x * 0.0; + double h_cross_n_z = h_vec.x * n.y - h_vec.y * n.x; + double hcn_mag = sqrt(h_cross_n_x*h_cross_n_x + h_cross_n_y*h_cross_n_y + h_cross_n_z*h_cross_n_z); + if (hcn_mag > 1e-10) { + h_cross_n_x /= hcn_mag; + h_cross_n_y /= hcn_mag; + h_cross_n_z /= hcn_mag; + } + double r_xAN = r_vec.x * x_AN + r_vec.y * y_AN; + double r_yAN = r_vec.x * h_cross_n_x + r_vec.y * h_cross_n_y + r_vec.z * h_cross_n_z; + true_anomaly_from_position = atan2(r_yAN, r_xAN); + } else { + // Nearly coplanar: ascending node is numerically unstable. + // Use X-axis as reference. For coplanar orbits this gives + // the argument of latitude = atan2(y, x). + true_anomaly_from_position = atan2(r_vec.y, r_vec.x); + } + true_anomaly = normalize_angle(true_anomaly_from_position); } else { double cos_nu = r_dot_e / (r_mag * e); cos_nu = fmax(-1.0, fmin(1.0, cos_nu)); @@ -278,11 +312,6 @@ OrbitalElements cartesian_to_orbital_elements(Vec3 position, Vec3 velocity, doub i = 0.0; } - // Ascending node vector: n = k × h_vec (k is unit Z vector) - Vec3 n_vec = {0.0, 0.0, 1.0}; - Vec3 n = vec3_cross(n_vec, h_vec); - double n_mag = vec3_magnitude(n); - // Longitude of ascending node from n vector double Omega; if (n_mag > 1e-10) { @@ -375,7 +404,6 @@ OrbitalElements propagate_orbital_elements(const OrbitalElements& elements, doub OrbitalElements result = elements; result.true_anomaly = 2.0 * atan(sqrt((1.0 + e) / (1.0 - e)) * tan(E_new / 2.0)); - return result; } else { // e >= 1.0 (hyperbolic) double n = sqrt(mu / pow(-a, 3.0));