File Equinoctial.cpp

File List > astrea > astro > astro > state > orbital_elements > instances > Equinoctial.cpp

Go to the documentation of this file

/*
 * The GNU Lesser General Public License (LGPL)
 *
 * Copyright (c) 2025 Jay Iuliano
 *
 * This file is part of Astrea.
 * Astrea is free software: you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
 * Astrea is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty
 * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. You should
 * have received a copy of the GNU General Public License along with Astrea. If not, see <https://www.gnu.org/licenses/>.
 */

#include <astro/state/orbital_elements/instances/Equinoctial.hpp>

#include <iomanip>
#include <iostream>

#include <mp-units/math.h>
#include <mp-units/systems/angular/math.h>
#include <mp-units/systems/si.h>
#include <mp-units/systems/si/math.h>

#include <math/interpolation.hpp>

#include <astro/state/orbital_elements/OrbitalElements.hpp>
#include <astro/state/orbital_elements/instances/Keplerian.hpp>
#include <astro/systems/AstrodynamicsSystem.hpp>
#include <astro/types/typedefs.hpp>
#include <astro/utilities/conversions.hpp>


using namespace mp_units;
using namespace mp_units::angular;
using angular::unit_symbols::deg;
using angular::unit_symbols::rad;
using si::unit_symbols::km;
using si::unit_symbols::s;

namespace astrea {
namespace astro {

Equinoctial Equinoctial::LEO(const GravParam& mu) { return Equinoctial(Keplerian::LEO(), mu); }
Equinoctial Equinoctial::LMEO(const GravParam& mu) { return Equinoctial(Keplerian::LMEO(), mu); }
Equinoctial Equinoctial::GPS(const GravParam& mu) { return Equinoctial(Keplerian::GPS(), mu); }
Equinoctial Equinoctial::HMEO(const GravParam& mu) { return Equinoctial(Keplerian::HMEO(), mu); }
Equinoctial Equinoctial::GEO(const GravParam& mu) { return Equinoctial(Keplerian::GEO(), mu); }

Equinoctial::Equinoctial(const Keplerian& elements, const GravParam& mu)
{
    // Get r and v
    const auto& a      = elements.get_semimajor();
    const auto& ecc    = elements.get_eccentricity();
    const auto& inc    = elements.get_inclination();
    const auto& raan   = elements.get_right_ascension();
    const auto& argPer = elements.get_argument_of_perigee();
    const auto& theta  = elements.get_true_anomaly();

    // Semilatus rectum
    _semilatus = a * (1 - ecc * ecc);

    // Non-dimensionalized parameters
    _f = ecc * cos(argPer + raan);
    _g = ecc * sin(argPer + raan);
    _h = tan(inc / 2.0) * cos(raan);
    _k = tan(inc / 2.0) * sin(raan);

    // True longitude
    _trueLongitude = wrap_angle(raan + argPer + theta);
}

Equinoctial::Equinoctial(const Cartesian& elements, const GravParam& mu) :
    Equinoctial(Keplerian(elements, mu), mu)
{
}

Equinoctial::Equinoctial(const OrbitalElements& elements, const GravParam& mu)
{
    *this = elements.in_element_set<Equinoctial>(mu);
}

// Copy constructor
Equinoctial::Equinoctial(const Equinoctial& other) :
    _semilatus(other._semilatus),
    _f(other._f),
    _g(other._g),
    _h(other._h),
    _k(other._k),
    _trueLongitude(other._trueLongitude)
{
}

// Move constructor
Equinoctial::Equinoctial(Equinoctial&& other) noexcept :
    _semilatus(std::move(other._semilatus)),
    _f(std::move(other._f)),
    _g(std::move(other._g)),
    _h(std::move(other._h)),
    _k(std::move(other._k)),
    _trueLongitude(std::move(other._trueLongitude))
{
}

// Move assignment operator
Equinoctial& Equinoctial::operator=(Equinoctial&& other) noexcept
{
    if (this != &other) {
        _semilatus     = std::move(other._semilatus);
        _f             = std::move(other._f);
        _g             = std::move(other._g);
        _h             = std::move(other._h);
        _k             = std::move(other._k);
        _trueLongitude = std::move(other._trueLongitude);
    }
    return *this;
}

// Copy assignment operator
Equinoctial& Equinoctial::operator=(const Equinoctial& other) { return *this = Equinoctial(other); }

// Comparitors operators
bool Equinoctial::operator==(const Equinoctial& other) const
{
    return (
        _semilatus == other._semilatus && _f == other._f && _g == other._g && _h == other._h && _k == other._k &&
        _trueLongitude == other._trueLongitude
    );
}

bool Equinoctial::operator!=(const Equinoctial& other) const { return !(*this == other); }


// Mathmatical operators
Equinoctial Equinoctial::operator+(const Equinoctial& other) const
{
    return Equinoctial(
        _semilatus + other._semilatus,
        _f + other._f,
        _g + other._g,
        _h + other._h,
        _k + other._k,
        _trueLongitude + other._trueLongitude
    );
}
Equinoctial& Equinoctial::operator+=(const Equinoctial& other)
{
    _semilatus += other._semilatus;
    _f += other._f;
    _g += other._g;
    _h += other._h;
    _k += other._k;
    _trueLongitude += other._trueLongitude;
    return *this;
}

Equinoctial Equinoctial::operator-(const Equinoctial& other) const
{
    return Equinoctial(
        _semilatus - other._semilatus,
        _f - other._f,
        _g - other._g,
        _h - other._h,
        _k - other._k,
        _trueLongitude - other._trueLongitude
    );
}
Equinoctial& Equinoctial::operator-=(const Equinoctial& other)
{
    _semilatus -= other._semilatus;
    _f -= other._f;
    _g -= other._g;
    _h -= other._h;
    _k -= other._k;
    _trueLongitude -= other._trueLongitude;
    return *this;
}

Equinoctial Equinoctial::operator*(const Unitless& multiplier) const
{
    return Equinoctial(_semilatus * multiplier, _f * multiplier, _g * multiplier, _h * multiplier, _k * multiplier, _trueLongitude * multiplier);
}
Equinoctial& Equinoctial::operator*=(const Unitless& multiplier)
{
    _semilatus *= multiplier;
    _f *= multiplier;
    _g *= multiplier;
    _h *= multiplier;
    _k *= multiplier;
    _trueLongitude *= multiplier;
    return *this;
}

EquinoctialPartial Equinoctial::operator/(const Time& time) const
{
    return EquinoctialPartial(_semilatus / time, _f / time, _g / time, _h / time, _k / time, _trueLongitude / time);
}

Equinoctial Equinoctial::operator/(const Unitless& divisor) const
{
    return Equinoctial(_semilatus / divisor, _f / divisor, _g / divisor, _h / divisor, _k / divisor, _trueLongitude / divisor);
}

Equinoctial& Equinoctial::operator/=(const Unitless& divisor)
{
    _semilatus /= divisor;
    _f /= divisor;
    _g /= divisor;
    _h /= divisor;
    _k /= divisor;
    _trueLongitude /= divisor;
    return *this;
}


Equinoctial
    Equinoctial::interpolate(const Time& thisTime, const Time& otherTime, const Equinoctial& other, const GravParam& mu, const Time& targetTime) const
{
    const std::array<Time, 2> times = { thisTime, otherTime };
    const Distance interpSemimajor =
        math::fast_interpolate<Time, Distance>(times, { _semilatus, other.get_semilatus() }, targetTime);
    const Unitless interpEcc    = math::fast_interpolate<Time, Unitless>(times, { _f, other.get_f() }, targetTime);
    const Unitless interpInc    = math::fast_interpolate<Time, Unitless>(times, { _g, other.get_g() }, targetTime);
    const Unitless interpRaan   = math::fast_interpolate<Time, Unitless>(times, { _h, other.get_h() }, targetTime);
    const Unitless interpArgPer = math::fast_interpolate<Time, Unitless>(times, { _k, other.get_k() }, targetTime);
    const Angle interpTheta = math::fast_interpolate<Time, Angle>(times, { _trueLongitude, other.get_true_longitude() }, targetTime);

    return Equinoctial(interpSemimajor, interpEcc, interpInc, interpRaan, interpArgPer, interpTheta);
}

std::vector<Unitless> Equinoctial::force_to_vector() const
{
    return { _semilatus / _semilatus.unit, _f, _g, _h, _k, _trueLongitude / _trueLongitude.unit };
}

Equinoctial Equinoctial::from_vector(const std::vector<Unitless>& vec)
{
    if (vec.size() != 6) {
        throw std::runtime_error("Input vector must have exactly 6 elements to convert to Equinoctial.");
    }
    return Equinoctial(vec[0] * detail::distance_unit, vec[1], vec[2], vec[3], vec[4], vec[5] * detail::angle_unit);
}

Equinoctial EquinoctialPartial::operator*(const Time& time) const
{
    return Equinoctial(_semilatusPartial * time, _fPartial * time, _gPartial * time, _hPartial * time, _kPartial * time, _trueLongitudePartial * time);
}

std::vector<Unitless> EquinoctialPartial::force_to_vector() const
{
    return { _semilatusPartial / _semilatusPartial.unit,
             _fPartial / _fPartial.unit,
             _gPartial / _gPartial.unit,
             _hPartial / _hPartial.unit,
             _kPartial / _kPartial.unit,
             _trueLongitudePartial / _trueLongitudePartial.unit };
}

std::ostream& operator<<(std::ostream& os, Equinoctial const& elements)
{
    os << "[";
    os << elements.get_semilatus() << ", ";
    os << elements.get_f() << ", ";
    os << elements.get_g() << ", ";
    os << elements.get_h() << ", ";
    os << elements.get_k() << ", ";
    os << elements.get_true_longitude();
    os << "] (Equinoctial)";
    return os;
}

std::ostream& operator<<(std::ostream& os, EquinoctialPartial const& elements)
{
    os << "[";
    os << elements._semilatusPartial << ", ";
    os << elements._fPartial << ", ";
    os << elements._gPartial << ", ";
    os << elements._hPartial << ", ";
    os << elements._kPartial << ", ";
    os << elements._trueLongitudePartial;
    os << "] (EquinoctialPartial)";
    return os;
}

} // namespace astro
} // namespace astrea