FramePrimitives.hpp

Go to the documentation of this file.

// Vulcan Coordinate Frame
// Orthonormal basis vector representation for coordinate frames
#pragma once
 
#include <vulcan/coordinates/EarthModel.hpp>
#include <vulcan/core/VulcanTypes.hpp>
 
#include <janus/math/Linalg.hpp>
#include <janus/math/Quaternion.hpp>
#include <janus/math/Rotations.hpp>
#include <janus/math/Trig.hpp>
 
namespace vulcan {
 
// =============================================================================
// CoordinateFrame - Basis Vector Representation
// =============================================================================

template <typename Scalar> struct CoordinateFrame {
    Vec3<Scalar> x_axis; 
    Vec3<Scalar> y_axis; 
    Vec3<Scalar> z_axis; 
    Vec3<Scalar> origin; 
 
    // =========================================================================
    // Constructors
    // =========================================================================

    CoordinateFrame()
        : x_axis(Vec3<Scalar>::UnitX()), y_axis(Vec3<Scalar>::UnitY()),
          z_axis(Vec3<Scalar>::UnitZ()), origin(Vec3<Scalar>::Zero()) {}

    CoordinateFrame(const Vec3<Scalar> &x, const Vec3<Scalar> &y,
                    const Vec3<Scalar> &z, const Vec3<Scalar> &o)
        : x_axis(x), y_axis(y), z_axis(z), origin(o) {}
 
    // =========================================================================
    // Core Transformations
    // =========================================================================

    [[nodiscard]] Vec3<Scalar> from_ecef(const Vec3<Scalar> &v) const {
        Vec3<Scalar> result;
        result(0) = janus::dot(v, x_axis);
        result(1) = janus::dot(v, y_axis);
        result(2) = janus::dot(v, z_axis);
        return result;
    }

    [[nodiscard]] Vec3<Scalar> to_ecef(const Vec3<Scalar> &v) const {
        return x_axis * v(0) + y_axis * v(1) + z_axis * v(2);
    }

    [[nodiscard]] Vec3<Scalar>
    position_from_ecef(const Vec3<Scalar> &pos_ecef) const {
        Vec3<Scalar> relative = pos_ecef - origin;
        return from_ecef(relative);
    }

    [[nodiscard]] Vec3<Scalar>
    position_to_ecef(const Vec3<Scalar> &pos_local) const {
        return to_ecef(pos_local) + origin;
    }
 
    // =========================================================================
    // Direction Cosine Matrix
    // =========================================================================

    [[nodiscard]] Mat3<Scalar> dcm() const {
        Mat3<Scalar> R;
        R.col(0) = x_axis;
        R.col(1) = y_axis;
        R.col(2) = z_axis;
        return R;
    }

    [[nodiscard]] Mat3<Scalar> dcm_inverse() const { return dcm().transpose(); }
 
    // =========================================================================
    // Quaternion Representation
    // =========================================================================

    [[nodiscard]] janus::Quaternion<Scalar> quaternion() const {
        // The DCM columns are our basis vectors in ECEF
        // DCM = [x_axis | y_axis | z_axis] transforms local -> ECEF
        // We want ECEF -> local, which is DCM transpose
        // Quaternion from rotation matrix expects ECEF -> local convention
        return janus::Quaternion<Scalar>::from_rotation_matrix(dcm_inverse());
    }

    static CoordinateFrame
    from_quaternion(const janus::Quaternion<Scalar> &q,
                    const Vec3<Scalar> &o = Vec3<Scalar>::Zero()) {
        // q.to_rotation_matrix() gives R such that v_local = R * v_ecef
        // We need basis vectors in ECEF, so we take R^T columns
        Mat3<Scalar> R = q.to_rotation_matrix();
        Mat3<Scalar> R_T = R.transpose();
        return CoordinateFrame(R_T.col(0), R_T.col(1), R_T.col(2), o);
    }
 
    // =========================================================================
    // Validation
    // =========================================================================

    [[nodiscard]] bool is_valid(double tol = 1e-9) const {
        if constexpr (std::is_floating_point_v<Scalar>) {
            // Check unit lengths
            double norm_x = x_axis.norm();
            double norm_y = y_axis.norm();
            double norm_z = z_axis.norm();
            if (std::abs(norm_x - 1.0) > tol)
                return false;
            if (std::abs(norm_y - 1.0) > tol)
                return false;
            if (std::abs(norm_z - 1.0) > tol)
                return false;
 
            // Check orthogonality
            double dot_xy = x_axis.dot(y_axis);
            double dot_yz = y_axis.dot(z_axis);
            double dot_xz = x_axis.dot(z_axis);
            if (std::abs(dot_xy) > tol)
                return false;
            if (std::abs(dot_yz) > tol)
                return false;
            if (std::abs(dot_xz) > tol)
                return false;
 
            return true;
        } else {
            // For symbolic types, validation is not meaningful
            return true;
        }
    }
 
    // =========================================================================
    // Static Factory Methods
    // =========================================================================

    static CoordinateFrame ecef() { return CoordinateFrame(); }

    static CoordinateFrame eci(Scalar gmst) {
        // ECI is ECEF rotated by -gmst about Z
        // ECI X-axis points to vernal equinox, which is at angle -gmst from
        // Greenwich
        Scalar c = janus::cos(gmst);
        Scalar s = janus::sin(gmst);
 
        Vec3<Scalar> x_eci, y_eci, z_eci;
        x_eci << c, -s, Scalar(0);
        y_eci << s, c, Scalar(0);
        z_eci << Scalar(0), Scalar(0), Scalar(1);
 
        return CoordinateFrame(x_eci, y_eci, z_eci, Vec3<Scalar>::Zero());
    }

    static CoordinateFrame ned(Scalar lon, Scalar lat) {
        Scalar sin_lat = janus::sin(lat);
        Scalar cos_lat = janus::cos(lat);
        Scalar sin_lon = janus::sin(lon);
        Scalar cos_lon = janus::cos(lon);
 
        // North: -sin(lat)*cos(lon), -sin(lat)*sin(lon), cos(lat)
        Vec3<Scalar> north;
        north << -sin_lat * cos_lon, -sin_lat * sin_lon, cos_lat;
 
        // East: -sin(lon), cos(lon), 0
        Vec3<Scalar> east;
        east << -sin_lon, cos_lon, Scalar(0);
 
        // Down: -cos(lat)*cos(lon), -cos(lat)*sin(lon), -sin(lat)
        Vec3<Scalar> down;
        down << -cos_lat * cos_lon, -cos_lat * sin_lon, -sin_lat;
 
        return CoordinateFrame(north, east, down, Vec3<Scalar>::Zero());
    }

    static CoordinateFrame enu(Scalar lon, Scalar lat) {
        Scalar sin_lat = janus::sin(lat);
        Scalar cos_lat = janus::cos(lat);
        Scalar sin_lon = janus::sin(lon);
        Scalar cos_lon = janus::cos(lon);
 
        // East: -sin(lon), cos(lon), 0
        Vec3<Scalar> east;
        east << -sin_lon, cos_lon, Scalar(0);
 
        // North: -sin(lat)*cos(lon), -sin(lat)*sin(lon), cos(lat)
        Vec3<Scalar> north;
        north << -sin_lat * cos_lon, -sin_lat * sin_lon, cos_lat;
 
        // Up: cos(lat)*cos(lon), cos(lat)*sin(lon), sin(lat)
        Vec3<Scalar> up;
        up << cos_lat * cos_lon, cos_lat * sin_lon, sin_lat;
 
        return CoordinateFrame(east, north, up, Vec3<Scalar>::Zero());
    }
};
 
// =============================================================================
// Free Functions for Frame Operations
// =============================================================================

template <typename Scalar>
Vec3<Scalar> transform_vector(const Vec3<Scalar> &v,
                              const CoordinateFrame<Scalar> &from,
                              const CoordinateFrame<Scalar> &to) {
    // from → ECEF → to
    Vec3<Scalar> v_ecef = from.to_ecef(v);
    return to.from_ecef(v_ecef);
}

template <typename Scalar>
Vec3<Scalar> transform_position(const Vec3<Scalar> &pos,
                                const CoordinateFrame<Scalar> &from,
                                const CoordinateFrame<Scalar> &to) {
    // from → ECEF → to (with origin handling)
    Vec3<Scalar> pos_ecef = from.position_to_ecef(pos);
    return to.position_from_ecef(pos_ecef);
}
 
} // namespace vulcan