Ellipsoid.hpp

Go to the documentation of this file.

// This file is part of INSTINCT, the INS Toolkit for Integrated
// Navigation Concepts and Training by the Institute of Navigation of
// the University of Stuttgart, Germany.
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at https://mozilla.org/MPL/2.0/.
 
 
#pragma once
 
#include <concepts>
#include "Navigation/Constants.hpp"
 
namespace NAV
{
 
template<std::floating_point Scalar>
[[nodiscard]] Scalar calcGeocentricRadius(const Scalar& latitude, const Scalar& R_E, const Scalar& e_squared = InsConst::WGS84::e_squared)
{
    return R_E * std::sqrt(std::pow(std::cos(latitude), 2) + std::pow((1.0 - e_squared) * std::sin(latitude), 2));
}
 
template<std::floating_point Scalar>
[[nodiscard]] Scalar calcEarthRadius_N(const Scalar& latitude, const Scalar& a = InsConst::WGS84::a, const Scalar& e_squared = InsConst::WGS84::e_squared)
{
    Scalar k = std::sqrt(1 - e_squared * std::pow(std::sin(latitude), 2));
 
    // North/South (meridian) earth radius [m]
    return a * (1 - e_squared) / std::pow(k, 3);
}
 
template<std::floating_point Scalar>
[[nodiscard]] Scalar calcEarthRadius_E(const Scalar& latitude, const Scalar& a = InsConst::WGS84::a, const Scalar& e_squared = InsConst::WGS84::e_squared)
{
    // East/West (prime vertical) earth radius [m]
    return a / std::sqrt(1 - e_squared * std::pow(std::sin(latitude), 2));
}
 
template<typename Derived>
[[nodiscard]] Eigen::Matrix3<typename Derived::Scalar> conversionMatrixCartesianCurvilinear(const Eigen::MatrixBase<Derived>& lla_position,
                                                                                            const typename Derived::Scalar& R_N, const typename Derived::Scalar& R_E)
{
    return Eigen::DiagonalMatrix<typename Derived::Scalar, 3>{ 1.0 / (R_N + lla_position(2)),
                                                               1.0 / ((R_E + lla_position(2)) * std::cos(lla_position(0))),
                                                               -1.0 };
}
 
template<std::floating_point Scalar>
[[nodiscard]] Scalar calcGreatCircleDistance(Scalar lat1, Scalar lon1, Scalar lat2, Scalar lon2)
{
    Scalar R = calcGeocentricRadius(lat1, calcEarthRadius_E(lat1));
    Scalar dLat = lat2 - lat1;
    Scalar dLon = lon2 - lon1;
    Scalar a = std::pow(std::sin(dLat / 2.0), 2) + std::cos(lat1) * std::cos(lat2) * std::pow(std::sin(dLon / 2.0), 2);
    Scalar c = 2.0 * std::atan2(std::sqrt(a), std::sqrt(1.0 - a));
    return R * c; // meters
}
 
template<std::floating_point Scalar>
[[nodiscard]] Scalar calcGeographicalDistance(Scalar lat1, Scalar lon1, Scalar lat2, Scalar lon2)
{
    if (lat1 == lat2 && lon1 == lon2)
    {
        return 0;
    }
    // First convert the latitudes 𝜙₁,𝜙₂ of the two points to reduced latitudes 𝛽₁,𝛽₂
    Scalar beta1 = std::atan((1 - InsConst::WGS84::f) * std::tan(lat1));
    Scalar beta2 = std::atan((1 - InsConst::WGS84::f) * std::tan(lat2));
 
    // Then calculate the central angle 𝜎 in radians between two points 𝛽₁,𝜆₁ and 𝛽₂,𝜆₂ on a sphere using the
    // Great-circle distance method (law of cosines or haversine formula), with longitudes 𝜆₁ and 𝜆₂ being the same on the sphere as on the spheroid.
    Scalar sigma = calcGreatCircleDistance(beta1, lon1, beta2, lon2)
                   / calcGeocentricRadius(lat1, calcEarthRadius_E(lat1));
 
    Scalar P = (beta1 + beta2) / 2;
    Scalar Q = (beta2 - beta1) / 2;
 
    Scalar X = (sigma - std::sin(sigma)) * std::pow((std::sin(P) * std::cos(Q)) / std::cos(sigma / 2), 2);
    Scalar Y = (sigma + std::sin(sigma)) * std::pow((std::cos(P) * std::sin(Q)) / std::sin(sigma / 2), 2);
 
    return InsConst::WGS84::a * (sigma - InsConst::WGS84::f / 2.0 * (X + Y));
}
 
} // namespace NAV