FrameLocal.hpp

Go to the documentation of this file.

// Vulcan Local Frames
// Convenience functions for local tangent plane reference frames
#pragma once
 
#include <vulcan/coordinates/FramePrimitives.hpp>
#include <vulcan/coordinates/Geodetic.hpp>
 
#include <janus/math/Trig.hpp>
 
namespace vulcan {
 
// =============================================================================
// Local Geodetic Horizon Frames
// =============================================================================

template <typename Scalar>
CoordinateFrame<Scalar> local_ned(Scalar lon, Scalar lat_gd) {
    return CoordinateFrame<Scalar>::ned(lon, lat_gd);
}

template <typename Scalar>
CoordinateFrame<Scalar> local_enu(Scalar lon, Scalar lat_gd) {
    return CoordinateFrame<Scalar>::enu(lon, lat_gd);
}
 
// =============================================================================
// Local Geocentric Horizon Frame
// =============================================================================

template <typename Scalar>
CoordinateFrame<Scalar> local_geocentric(Scalar lon, Scalar lat_gc) {
    Scalar sin_lat = janus::sin(lat_gc);
    Scalar cos_lat = janus::cos(lat_gc);
    Scalar sin_lon = janus::sin(lon);
    Scalar cos_lon = janus::cos(lon);
 
    // North: perpendicular to radial direction, in meridian plane, pointing
    // north
    //   = -sin(lat_gc)*cos(lon), -sin(lat_gc)*sin(lon), cos(lat_gc)
    Vec3<Scalar> north;
    north << -sin_lat * cos_lon, -sin_lat * sin_lon, cos_lat;
 
    // East: tangent to parallel of latitude
    //   = -sin(lon), cos(lon), 0
    Vec3<Scalar> east;
    east << -sin_lon, cos_lon, Scalar(0);
 
    // Down: toward Earth center (negative of radial unit vector)
    //   = -cos(lat_gc)*cos(lon), -cos(lat_gc)*sin(lon), -sin(lat_gc)
    Vec3<Scalar> down;
    down << -cos_lat * cos_lon, -cos_lat * sin_lon, -sin_lat;
 
    return CoordinateFrame<Scalar>(north, east, down, Vec3<Scalar>::Zero());
}

template <typename Scalar>
CoordinateFrame<Scalar> local_geocentric_at(const Vec3<Scalar> &r_ecef) {
    Spherical<Scalar> geo = ecef_to_spherical(r_ecef);
    return local_geocentric(geo.lon, geo.lat_gc);
}

template <typename Scalar>
CoordinateFrame<Scalar>
local_ned_at(const Vec3<Scalar> &r_ecef,
             const EarthModel &m = EarthModel::WGS84()) {
    LLA<Scalar> lla = ecef_to_lla(r_ecef, m);
    return local_ned(lla.lon, lla.lat);
}

template <typename Scalar>
CoordinateFrame<Scalar>
local_enu_at(const Vec3<Scalar> &r_ecef,
             const EarthModel &m = EarthModel::WGS84()) {
    LLA<Scalar> lla = ecef_to_lla(r_ecef, m);
    return local_enu(lla.lon, lla.lat);
}
 
// =============================================================================
// Rail Launch Frame
// =============================================================================

template <typename Scalar>
CoordinateFrame<Scalar>
local_rail(const LLA<Scalar> &lla_origin, const Scalar &azimuth,
           const Scalar &elevation,
           [[maybe_unused]] const EarthModel &m = EarthModel::WGS84()) {
    // Start with NED axes at origin
    const Scalar sin_lat = janus::sin(lla_origin.lat);
    const Scalar cos_lat = janus::cos(lla_origin.lat);
    const Scalar sin_lon = janus::sin(lla_origin.lon);
    const Scalar cos_lon = janus::cos(lla_origin.lon);
 
    // NED basis vectors in ECEF
    Vec3<Scalar> north;
    north << -sin_lat * cos_lon, -sin_lat * sin_lon, cos_lat;
 
    Vec3<Scalar> east;
    east << -sin_lon, cos_lon, Scalar(0);
 
    Vec3<Scalar> down;
    down << -cos_lat * cos_lon, -cos_lat * sin_lon, -sin_lat;
 
    // Trig for azimuth and elevation
    const Scalar sin_az = janus::sin(azimuth);
    const Scalar cos_az = janus::cos(azimuth);
    const Scalar sin_el = janus::sin(elevation);
    const Scalar cos_el = janus::cos(elevation);
 
    // Step 1: Rotate by azimuth in horizontal plane
    // Horizontal forward direction = cos(az)*North + sin(az)*East
    Vec3<Scalar> horiz_fwd = cos_az * north + sin_az * east;
 
    // Horizontal right direction = -sin(az)*North + cos(az)*East
    Vec3<Scalar> horiz_right = -sin_az * north + cos_az * east;
 
    // Step 2: Pitch up by elevation
    // Rail X-axis: cos(el)*horiz_fwd - sin(el)*down (up is -down)
    Vec3<Scalar> rail_x = cos_el * horiz_fwd - sin_el * down;
 
    // Rail Y-axis: remains horiz_right (perpendicular to pitch rotation)
    Vec3<Scalar> rail_y = horiz_right;
 
    // Rail Z-axis: sin(el)*horiz_fwd + cos(el)*down
    Vec3<Scalar> rail_z = sin_el * horiz_fwd + cos_el * down;
 
    return CoordinateFrame<Scalar>(rail_x, rail_y, rail_z,
                                   Vec3<Scalar>::Zero());
}

template <typename Scalar>
CoordinateFrame<Scalar>
local_rail_at(const Vec3<Scalar> &r_ecef, const Scalar &azimuth,
              const Scalar &elevation,
              const EarthModel &m = EarthModel::WGS84()) {
    LLA<Scalar> lla = ecef_to_lla(r_ecef, m);
    return local_rail(lla, azimuth, elevation, m);
}
 
// =============================================================================
// CDA Frame (Cross-range, Down-range, Altitude) — Trajectory-Relative
// =============================================================================

template <typename Scalar>
CoordinateFrame<Scalar>
local_cda(const LLA<Scalar> &lla_origin, const Scalar &bearing,
          [[maybe_unused]] const EarthModel &m = EarthModel::WGS84()) {
    // Start with local ENU frame at origin
    const Scalar sin_lat = janus::sin(lla_origin.lat);
    const Scalar cos_lat = janus::cos(lla_origin.lat);
    const Scalar sin_lon = janus::sin(lla_origin.lon);
    const Scalar cos_lon = janus::cos(lla_origin.lon);
 
    // ENU basis vectors in ECEF
    Vec3<Scalar> east;
    east << -sin_lon, cos_lon, Scalar(0);
 
    Vec3<Scalar> north;
    north << -sin_lat * cos_lon, -sin_lat * sin_lon, cos_lat;
 
    Vec3<Scalar> up;
    up << cos_lat * cos_lon, cos_lat * sin_lon, sin_lat;
 
    // Rotate about Up axis by bearing angle
    // Down-range = cos(bearing) * North + sin(bearing) * East
    // Cross-range = -sin(bearing) * North + cos(bearing) * East (right-hand)
    const Scalar sin_b = janus::sin(bearing);
    const Scalar cos_b = janus::cos(bearing);
 
    Vec3<Scalar> downrange = cos_b * north + sin_b * east;
    Vec3<Scalar> crossrange = -sin_b * north + cos_b * east;
 
    // CDA frame: D (downrange) as X, C (crossrange) as Y, A (altitude/up) as Z
    return CoordinateFrame<Scalar>(downrange, crossrange, up,
                                   Vec3<Scalar>::Zero());
}

template <typename Scalar>
CoordinateFrame<Scalar>
local_cda_at(const Vec3<Scalar> &r_ecef, const Scalar &bearing,
             const EarthModel &m = EarthModel::WGS84()) {
    LLA<Scalar> lla = ecef_to_lla(r_ecef, m);
    return local_cda(lla, bearing, m);
}
 
// =============================================================================
// CDA Coordinate Conversions
// =============================================================================

template <typename Scalar>
Vec3<Scalar> ecef_to_cda(const Vec3<Scalar> &r_ecef, const LLA<Scalar> &lla_ref,
                         const Scalar &bearing,
                         const EarthModel &m = EarthModel::WGS84()) {
    // Get CDA frame at reference point
    CoordinateFrame<Scalar> cda_frame = local_cda(lla_ref, bearing, m);
 
    // Get reference point in ECEF
    Vec3<Scalar> r_ref = lla_to_ecef(lla_ref, m);
 
    // Compute offset vector
    Vec3<Scalar> delta = r_ecef - r_ref;
 
    // Project onto CDA axes (frame axes are columns of rotation matrix)
    // CDA = R^T * delta where R = [downrange | crossrange | up]
    Vec3<Scalar> cda;
    cda(0) = delta.dot(cda_frame.x_axis); // Down-range
    cda(1) = delta.dot(cda_frame.y_axis); // Cross-range
    cda(2) = delta.dot(cda_frame.z_axis); // Altitude
 
    return cda;
}

template <typename Scalar>
Vec3<Scalar> cda_to_ecef(const Vec3<Scalar> &cda, const LLA<Scalar> &lla_ref,
                         const Scalar &bearing,
                         const EarthModel &m = EarthModel::WGS84()) {
    // Get CDA frame at reference point
    CoordinateFrame<Scalar> cda_frame = local_cda(lla_ref, bearing, m);
 
    // Get reference point in ECEF
    Vec3<Scalar> r_ref = lla_to_ecef(lla_ref, m);
 
    // Convert CDA to ECEF offset
    // delta = R * cda where R = [downrange | crossrange | up]
    Vec3<Scalar> delta = cda(0) * cda_frame.x_axis + cda(1) * cda_frame.y_axis +
                         cda(2) * cda_frame.z_axis;
 
    return r_ref + delta;
}
 
} // namespace vulcan