Geodetic.hpp

Go to the documentation of this file.

// Vulcan Geodetic Utilities
// ECEF <-> Geodetic conversions using Vermeille (2004) closed-form algorithm
#pragma once
 
#include <vulcan/coordinates/EarthModel.hpp>
#include <vulcan/core/VulcanTypes.hpp>
 
#include <janus/math/Arithmetic.hpp>
#include <janus/math/Logic.hpp>
#include <janus/math/Trig.hpp>
 
namespace vulcan {
 
// =============================================================================
// LLA - Geodetic Coordinates
// =============================================================================

template <typename Scalar> struct LLA {
    Scalar lon; 
    Scalar lat; 
    Scalar alt; 
 
    LLA() : lon(Scalar(0)), lat(Scalar(0)), alt(Scalar(0)) {}
    LLA(Scalar lon_, Scalar lat_, Scalar alt_)
        : lon(lon_), lat(lat_), alt(alt_) {}
};
 
// =============================================================================
// Spherical - Geocentric Coordinates
// =============================================================================

template <typename Scalar> struct Spherical {
    Scalar lon;    
    Scalar lat_gc; 
    Scalar radius; 
 
    Spherical() : lon(Scalar(0)), lat_gc(Scalar(0)), radius(Scalar(0)) {}
    Spherical(Scalar lon_, Scalar lat_gc_, Scalar radius_)
        : lon(lon_), lat_gc(lat_gc_), radius(radius_) {}
};
 
// =============================================================================
// ECEF to LLA - Vermeille (2004) Closed-Form Algorithm
// =============================================================================

template <typename Scalar>
LLA<Scalar> ecef_to_lla(const Vec3<Scalar> &r,
                        const EarthModel &m = EarthModel::WGS84()) {
    const Scalar x = r(0);
    const Scalar y = r(1);
    const Scalar z = r(2);
 
    const double a = m.a;
    const double e2 = m.e2;
    const double e4 = e2 * e2;
 
    // Compute intermediate values
    // p = (x² + y²) / a²
    // q = (1 - e²) z² / a²
    const Scalar p = (x * x + y * y) / (a * a);
    const Scalar q = (1.0 - e2) * z * z / (a * a);
 
    // r = (p + q - e⁴) / 6
    const Scalar r_val = (p + q - e4) / 6.0;
 
    // Evolute parameters
    // s = e⁴ p q / (4 r³)
    const Scalar r_cubed = r_val * r_val * r_val;
    const Scalar s = e4 * p * q / (4.0 * r_cubed);
 
    // t = ∛(1 + s + √(s(2+s)))
    // Note: For numerical stability when s is small, this still works
    const Scalar s_term = s * (2.0 + s);
    const Scalar t = janus::pow(1.0 + s + janus::sqrt(s_term), 1.0 / 3.0);
 
    // u = r (1 + t + 1/t)
    const Scalar u = r_val * (1.0 + t + 1.0 / t);
 
    // v = √(u² + e⁴ q)
    const Scalar v = janus::sqrt(u * u + e4 * q);
 
    // w = e² (u + v - q) / (2v)
    const Scalar w = e2 * (u + v - q) / (2.0 * v);
 
    // k = √(u + v + w²) - w
    const Scalar k = janus::sqrt(u + v + w * w) - w;
 
    // D = k √(x² + y²) / (k + e²)
    const Scalar xy_dist = janus::sqrt(x * x + y * y);
    const Scalar D = k * xy_dist / (k + e2);
 
    // Compute geodetic latitude
    // φ = 2 atan2(z, D + √(D² + z²))
    const Scalar lat = 2.0 * janus::atan2(z, D + janus::sqrt(D * D + z * z));
 
    // Compute altitude
    // h = (k + e² - 1) / k · √(D² + z²)
    const Scalar alt = (k + e2 - 1.0) / k * janus::sqrt(D * D + z * z);
 
    // Compute longitude with pole handling
    // At poles (xy_dist ≈ 0), longitude is undefined; we set it to 0
    constexpr double eps = 1e-15;
    const Scalar is_pole = xy_dist < eps;
    const Scalar lon = janus::where(is_pole, Scalar(0.0), janus::atan2(y, x));
 
    return LLA<Scalar>(lon, lat, alt);
}
 
// =============================================================================
// LLA to ECEF - Closed-Form Conversion
// =============================================================================

template <typename Scalar>
Vec3<Scalar> lla_to_ecef(const LLA<Scalar> &lla,
                         const EarthModel &m = EarthModel::WGS84()) {
    const Scalar sin_lat = janus::sin(lla.lat);
    const Scalar cos_lat = janus::cos(lla.lat);
    const Scalar sin_lon = janus::sin(lla.lon);
    const Scalar cos_lon = janus::cos(lla.lon);
 
    const double a = m.a;
    const double e2 = m.e2;
 
    // Radius of curvature in the prime vertical
    // N = a / √(1 - e² sin²φ)
    const Scalar N = a / janus::sqrt(1.0 - e2 * sin_lat * sin_lat);
 
    // ECEF coordinates
    // x = (N + h) cos(φ) cos(λ)
    // y = (N + h) cos(φ) sin(λ)
    // z = (N(1 - e²) + h) sin(φ)
    Vec3<Scalar> r;
    r(0) = (N + lla.alt) * cos_lat * cos_lon;
    r(1) = (N + lla.alt) * cos_lat * sin_lon;
    r(2) = (N * (1.0 - e2) + lla.alt) * sin_lat;
 
    return r;
}
 
// =============================================================================
// ECEF to Spherical (Geocentric) Coordinates
// =============================================================================

template <typename Scalar>
Spherical<Scalar> ecef_to_spherical(const Vec3<Scalar> &r) {
    const Scalar x = r(0);
    const Scalar y = r(1);
    const Scalar z = r(2);
 
    // Radius (distance from Earth center)
    const Scalar radius = janus::sqrt(x * x + y * y + z * z);
 
    // Geocentric latitude (angle from equatorial plane to position vector)
    const Scalar lat_gc = janus::asin(z / radius);
 
    // Longitude with pole handling
    const Scalar xy_dist = janus::sqrt(x * x + y * y);
    constexpr double eps = 1e-15;
    const Scalar is_pole = xy_dist < eps;
    const Scalar lon = janus::where(is_pole, Scalar(0.0), janus::atan2(y, x));
 
    return Spherical<Scalar>(lon, lat_gc, radius);
}
 
// =============================================================================
// Spherical (Geocentric) to ECEF Conversion
// =============================================================================

template <typename Scalar>
Vec3<Scalar> spherical_to_ecef(const Spherical<Scalar> &geo) {
    const Scalar sin_lat = janus::sin(geo.lat_gc);
    const Scalar cos_lat = janus::cos(geo.lat_gc);
    const Scalar sin_lon = janus::sin(geo.lon);
    const Scalar cos_lon = janus::cos(geo.lon);
 
    Vec3<Scalar> r;
    r(0) = geo.radius * cos_lat * cos_lon;
    r(1) = geo.radius * cos_lat * sin_lon;
    r(2) = geo.radius * sin_lat;
 
    return r;
}
 
// =============================================================================
// Convenience Functions
// =============================================================================

template <typename Scalar>
Scalar geodetic_to_geocentric_lat(Scalar lat_gd,
                                  const EarthModel &m = EarthModel::WGS84()) {
    // tan(φ_gc) = (1 - e²) tan(φ_gd)
    return janus::atan((1.0 - m.e2) * janus::tan(lat_gd));
}

template <typename Scalar>
Scalar geocentric_to_geodetic_lat(Scalar lat_gc,
                                  const EarthModel &m = EarthModel::WGS84()) {
    // tan(φ_gd) = tan(φ_gc) / (1 - e²)
    return janus::atan(janus::tan(lat_gc) / (1.0 - m.e2));
}

template <typename Scalar>
Scalar radius_of_curvature_N(Scalar lat,
                             const EarthModel &m = EarthModel::WGS84()) {
    const Scalar sin_lat = janus::sin(lat);
    return m.a / janus::sqrt(1.0 - m.e2 * sin_lat * sin_lat);
}

template <typename Scalar>
Scalar radius_of_curvature_M(Scalar lat,
                             const EarthModel &m = EarthModel::WGS84()) {
    const Scalar sin_lat = janus::sin(lat);
    const Scalar denom = 1.0 - m.e2 * sin_lat * sin_lat;
    return m.a * (1.0 - m.e2) / janus::pow(denom, 1.5);
}
 
} // namespace vulcan