planetrelutils.hpp

/******************************************************************************
* Copyright (c) ATTX LLC 2024. All Rights Reserved.
*
* This software and associated documentation (the "Software") are the 
* proprietary and confidential information of ATTX, LLC. The Software is 
* furnished under a license agreement between ATTX and the user organization 
* and may be used or copied only in accordance with the terms of the agreement.
* Refer to 'license/attx_license.adoc' for standard license terms.
*
* EXPORT CONTROL NOTICE: THIS SOFTWARE MAY INCLUDE CONTENT CONTROLLED UNDER THE
* INTERNATIONAL TRAFFIC IN ARMS REGULATIONS (ITAR) OR THE EXPORT ADMINISTRATION 
* REGULATIONS (EAR99). No part of the Software may be used, reproduced, or 
* transmitted in any form or by any means, for any purpose, without the express 
* written permission of ATTX, LLC.
******************************************************************************/
/*
Planet relative state Utils header file
---------------------------------------
The planet relative state utilities header file contains
utiltities for converting between planet-based spherical 
coordinates and planet-centered rotating frame coordinates.
 
Author: Alex Reynolds
 
Update Log
----------
May 9, 2024, Gabriel A. Heredia Acevedo
 
Update tools to support planet (celestial) relative states model
*/
 
#ifndef PLANET_RELATIVE_STATE_UTILS_HPP
#define PLANET_RELATIVE_STATE_UTILS_HPP
 
#include <vector>
 
#include "core/macros.h"
#include "core/clockwerkerrors.h"
#include "core/CartesianVector.hpp"
#include "six_dof_dynamics/Frame.hpp"
#include <iomanip>
 
namespace clockwerk {
 
    /// @brief Function to calculate detic LLA from PCR state
    /// @param pos__pcr The position of the spacecraft in PCR frame
    /// @param r_planet_equatorial The equatorial radius of the planet
    /// @param flattening The flattening value of the planet
    /// @param lat_rad Implicit return of latitude in radians
    /// @param lon_rad Implicit return of longitude in radians
    /// @param alt Implicit return of altitude
    template <typename T>
    int pcrDeticToLla(CartesianVector<T, 3> pos__pcr, T r_planet_equatorial, T flattening, T& lat_rad, T& lon_rad, T& alt);
    /// @brief Wrapper around pcrDeticToLLA for bulk conversion of x, y, z coordinates
    /// @param pcr_x Vector of x PCR frame values 
    /// @param pcr_y Vector of y PCR frame values 
    /// @param pcr_z Vector of z PCR frame values 
    /// @param a Semimajor axis of the planet. Defaults to WGS84
    /// @param flattening Flattening of the planet. Defaults to WGS84
    /// @return Vector of vectors of coordinatess containing lat and lon in radians and altitude
    /// @note NOT embedded safe! Does not error check.
    std::vector<std::vector<double>> pcr2lla(const std::vector<double> &pcr_x,
                                            const std::vector<double> &pcr_y,
                                            const std::vector<double> &pcr_z,
                                            double a=6378137.0,
                                            double flattening=(1.0/298.257223563));
 
    /// @brief Function to return the PCR position of an object from centric lat, lon, alt
    /// @param r_planet The radius of the planet relative to which LLA is measured
    /// @param lat_rad The centric latitude in radians
    /// @param lon_rad The longitude in radians
    /// @param alt The centric altitude in the same units as the radius
    /// @return The PCR position of the object
    template <typename T>
    CartesianVector<T, 3> llaCentricToPCR(T r_planet, T lat_rad, T lon_rad, T alt);
 
    /// @brief Function to calculate the East, North, Up frame attitude from centric lat and lon
    /// @param lat_rad The centric latitude in radians
    /// @param lon_rad The longitude in radians
    /// @return DCM corresponding to ENU frame attitude relative to PCR
    template <typename T>
    DCM<T> enuFromLatLonCentric(T lat_rad, T lon_rad);
 
    /// @brief Function to calculate the East, North, Up frame attitude from detic lat and lon
    /// @param lat_rad The detic latitude in radians
    /// @param lon_rad The longitude in radians
    /// @return DCM corresponding to ENU frame attitude relative to PCR
    template <typename T>
    DCM<T> enuFromLatLonDetic(T lat_rad, T lon_rad);
 
    /// @brief Function to calculate the North, East, Down frame attitude from centric lat and lon
    /// @param lat_rad The centric latitude in radians
    /// @param lon_rad The longitude in radians
    /// @return DCM corresponding to NED frame attitude relative to PCR
    template <typename T>
    DCM<T> nedFromLatLonCentric(T lat_rad, T lon_rad);
 
    /// @brief Function to calculate the North, East, Down frame attitude from detic lat and lon
    /// @param lat_rad The detic latitude in radians
    /// @param lon_rad The longitude in radians
    /// @return DCM corresponding to NED frame attitude relative to PCR
    template <typename T>
    DCM<T> nedFromLatLonDetic(T lat_rad, T lon_rad);
 
    /// @brief Function to calculate detic LLA from PCR state using the Heikkinen algorithm
    /// @param pos__pcr The position of the spacecraft in PCR frame
    /// @param r_planet_equatorial The equatorial radius of the planet
    /// @param flattening The flattening value of the planet
    /// @param lat_rad Implicit return of latitude in radians
    /// @param lon_rad Implicit return of longitude in radians
    /// @param alt Implicit return of altitude
    template <typename T>
    int heikkinenLla(CartesianVector<T, 3> pos__pcr, T r_planet_equatorial, T flattening, T& lat_rad, T& lon_rad, T& alt);
 
    /// @brief Function to calculate centric LLA from PCR state
    /// @param pos__pcr The position of the spacecraft in PCR frame
    /// @param r_planet The radius of the spherical planet
    /// @param lat_rad Implicit return of latitude in radians
    /// @param lon_rad Implicit return of longitude in radians
    /// @param alt Implicit return of altitude
    template <typename T>
    int pcrToLlaCentric(CartesianVector<T, 3> pos__pcr, T r_planet, T& lat_rad, T& lon_rad, T& alt);
 
    template <typename T>
    int pcrDeticToLla(CartesianVector<T, 3> pos__pcr, T r_planet_equatorial, T flattening, T& lat_rad, T& lon_rad, T& alt) {
        T nav_e2 = (2.0 - flattening)*flattening; // also e^2
 
        // Get our longitude from y and x pcr position
        lon_rad = atan2(pos__pcr[1], pos__pcr[0]);
 
        // Make initial lat and alt guesses based on spherical earth.
        T rhosqrd = pos__pcr[0]*pos__pcr[0] + pos__pcr[1]*pos__pcr[1];
        T rho;
        int _error = safeSqrt(rhosqrd, rho);
        if(_error) {return _error;}
 
        T templat = atan2(pos__pcr[2], rho);
        T tempalt = sqrt(rhosqrd + pos__pcr[2]*pos__pcr[2]) - r_planet_equatorial;
        T rhoerror = 1000.0;
        T zerror = 1000.0;
 
        int iter = 0; // number of iterations
 
        // Newton's method iteration on templat and tempalt makes
        // the residuals on rho and z progressively smaller.  Loop
        // is implemented as a 'while' instead of a 'do' to simplify
        // porting to MATLAB
        //TODO: need a global tolerance parameter
        T slat, clat, sqrt_q, q, r_n, drdl, aa, bb, cc, dd, invdet;
        while((abs(rhoerror) > 1e-6) || (abs(zerror) > 1e-6)) {
            slat = sin(templat);
            clat = cos(templat);
            q = 1.0 - nav_e2*slat*slat;
            _error = safeSqrt(q, sqrt_q);
            if(_error) {return _error;}
            _error = safeDivide(r_planet_equatorial, sqrt_q, r_n);
            if(_error) {return _error;}
            _error = safeDivide(r_n*nav_e2*slat*clat , q, drdl);
            if(_error) {return _error;}
 
            rhoerror = (r_n + tempalt)*clat - rho;
            zerror = (r_n*(1.0 - nav_e2) + tempalt)*slat - pos__pcr[2];
 
            aa = drdl*clat - (r_n + tempalt)*slat;
            bb = clat;
            cc = (1.0 - nav_e2)*(drdl*slat + r_n*clat);
            dd = slat;
 
            _error = safeDivide(1.0, aa*dd - bb*cc, invdet);
            if(_error) {return _error;}
            templat = templat - invdet*(dd*rhoerror - bb*zerror);
            tempalt = tempalt - invdet*(-cc*rhoerror + aa*zerror);
 
            iter++;
 
            if (iter > 20){return ERROR_INVALID_RANGE;}
        }
 
        lat_rad = templat;
        alt = tempalt;
 
        return NO_ERROR;
    }
 
    template <typename T>
    CartesianVector<T, 3> llaCentricToPCR(T r_planet, T lat_rad, T lon_rad, T alt) {
        T r_tot = r_planet + alt;
        return CartesianVector<T, 3>({r_tot*cos(lat_rad)*cos(lon_rad),
                                    r_tot*cos(lat_rad)*sin(lon_rad),
                                    r_tot*sin(lat_rad)});
    }
 
    template <typename T>
    DCM<T> enuFromLatLonCentric(T lat_rad, T lon_rad) {
        return DCM<T>({{-sin(lon_rad), cos(lon_rad), 0.0},
                    {-cos(lon_rad)*sin(lat_rad), -sin(lon_rad)*sin(lat_rad), cos(lat_rad)},
                    {cos(lon_rad)*cos(lat_rad), sin(lon_rad)*cos(lat_rad), sin(lat_rad)}});
    }
 
    template <typename T>
    int heikkinenLla(CartesianVector<T, 3> pos__pcr, T r_planet_equatorial, T flattening, T& lat_rad, T& lon_rad, T& alt){
        int _error;
        T b = r_planet_equatorial*(1.0-flattening);
        T e_sqr  = (2.0 - flattening)*flattening; // also e^2
        T E_sqr  = e_sqr*r_planet_equatorial*r_planet_equatorial;
        T ep_sqr = E_sqr/b/b; // also e^2
        T z_sqr = std::pow(pos__pcr[2],2.0);
        T r;
        _error = safeSqrt(std::pow(pos__pcr[0],2.0)+std::pow(pos__pcr[1],2.0), r);
        if (_error) {return ERROR_NEGATIVE_SQRT;}
        T F = 54.0*b*b*z_sqr;
        T G = r*r+(1-e_sqr)*z_sqr-e_sqr*E_sqr;
        T c;
        _error = safeDivide(e_sqr*e_sqr*F*r*r,std::pow(G,3.0),c);
        if (_error) {return ERROR_DIVIDE_BY_ZERO;}
 
        T tmpRes0;
        T tmpRes1;
        _error = safeSqrt(c*c+2*c,tmpRes0);
        if (_error) {return ERROR_NEGATIVE_SQRT;}
        T s;
        _error = safeNroot(1.0+c+tmpRes0,3,s);
        if (_error) {return _error;} // Different types of error within function
        T P;
        _error = safeDivide(1.0,s,tmpRes1);
        if (_error) {return ERROR_DIVIDE_BY_ZERO;}
        _error = safeDivide(F,3.0*std::pow(s+tmpRes1+1.0,2.0)*G*G,P);
        if (_error) {return ERROR_DIVIDE_BY_ZERO;}
        T Q;
        _error = safeSqrt(1.0+2.0*e_sqr*e_sqr*P,Q);
        if (_error) {return ERROR_NEGATIVE_SQRT;}
 
        _error = safeDivide(P*(1.0-e_sqr)*z_sqr,Q*(1.0+Q),tmpRes0);
        if (_error) {return ERROR_DIVIDE_BY_ZERO;}
        T r_0;
        _error = safeDivide(1.0,Q,tmpRes1);
        if (_error) {return ERROR_DIVIDE_BY_ZERO;}
        _error =  safeSqrt(0.5*r_planet_equatorial*r_planet_equatorial*(1.0+tmpRes1)-tmpRes0-0.5*P*r*r,r_0);
        if (_error) {return ERROR_NEGATIVE_SQRT;}
        _error =  safeDivide(P*e_sqr*r,1.0+Q,tmpRes0);
        if (_error) {return ERROR_DIVIDE_BY_ZERO;}
        r_0 -= tmpRes0;
 
        tmpRes0 = std::pow(r-e_sqr*r_0,2.0);
 
        // U and V are, by definition, greater than or equal to zero numbers
        T U = std::sqrt(tmpRes0+z_sqr);
        T V = std::sqrt(tmpRes0+(1.0-e_sqr)*z_sqr);
        _error = safeDivide(b*b,r_planet_equatorial*V,tmpRes0);
        if (_error) {return ERROR_DIVIDE_BY_ZERO;}
 
        T z_0 = pos__pcr[2]*tmpRes0;
 
        lat_rad = atan2(pos__pcr[2] + ep_sqr*z_0, r);
        lon_rad = atan2(pos__pcr[1], pos__pcr[0]);
        alt     = U*(1.0-tmpRes0);
 
        return NO_ERROR;
    };
 
    template <typename T>
    int pcrToLlaCentric(CartesianVector<T, 3> pos__pcr, T r_planet, T& lat_rad, T& lon_rad, T& alt){
        int _error;
        double r_pos;
        double tmpVal0;
 
        _error  = pos__pcr.normSquared(r_pos);
        if (_error) {return _error;}
 
        _error  = safeSqrt(r_pos,alt);
        if (_error) {return ERROR_NEGATIVE_SQRT;}
 
        alt -= r_planet;
 
        lon_rad = atan2(pos__pcr[1],pos__pcr[0]);
 
        _error = safeSqrt(std::pow(pos__pcr[0],2.0)+std::pow(pos__pcr[1],2.0),tmpVal0);
        if (_error) {return ERROR_NEGATIVE_SQRT;}
 
        //TODO: atan2 needs a safe operator
        lat_rad = atan2(pos__pcr[2],tmpVal0);
 
        return NO_ERROR;
    };
 
    //TODO: This is a redundant definition to enuFromLatLonCentric. Both functions should be generalized into one!
    template <typename T>
    DCM<T> enuFromLatLonDetic(T lat_rad, T lon_rad) {
        return DCM<T>({{-sin(lon_rad), cos(lon_rad), 0.0},
                    {-cos(lon_rad)*sin(lat_rad), -sin(lon_rad)*sin(lat_rad), cos(lat_rad)},
                    {cos(lon_rad)*cos(lat_rad), sin(lon_rad)*cos(lat_rad), sin(lat_rad)}});
    }
 
    template <typename T>
    DCM<T> nedFromLatLonCentric(T lat_rad, T lon_rad) {
        return DCM<T>({{-sin(lat_rad)*cos(lon_rad), -sin(lat_rad)*sin(lon_rad), cos(lat_rad)},
                    {-sin(lon_rad), cos(lon_rad), 0.0},
                    {-cos(lat_rad)*cos(lon_rad), -cos(lat_rad)*sin(lon_rad), -sin(lat_rad)}});
    }
 
    //TODO: This is a redundant definition to nedFromLatLonCentric. Both functions should be generalized into one!
    template <typename T>
    DCM<T> nedFromLatLonDetic(T lat_rad, T lon_rad) {
        return DCM<T>({{-sin(lat_rad)*cos(lon_rad), -sin(lat_rad)*sin(lon_rad), cos(lat_rad)},
                    {-sin(lon_rad), cos(lon_rad), 0.0},
                    {-cos(lat_rad)*cos(lon_rad), -cos(lat_rad)*sin(lon_rad), -sin(lat_rad)}});
    }
 
}
 
#endif