frame.hpp

Go to the documentation of this file.

/*
 * Copyright (C) 2023  Christopher J. Howard
 *
 * This file is part of Antkeeper source code.
 *
 * Antkeeper source code is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Antkeeper source code 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 General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Antkeeper source code.  If not, see <http://www.gnu.org/licenses/>.
 */
 
#ifndef ANTKEEPER_PHYSICS_ORBIT_FRAME_HPP
#define ANTKEEPER_PHYSICS_ORBIT_FRAME_HPP
 
#include <engine/math/se3.hpp>
#include <cmath>
 
namespace physics {
namespace orbit {
 
namespace frame {
 
namespace pqw {
    
    template <class T>
    math::vec3<T> spherical(const math::vec3<T>& v)
    {
        const T xx_yy = v.x() * v.x() + v.y() * v.y();
        
        return math::vec3<T>
        {
            std::sqrt(xx_yy + v.z() * v.z()),
            std::atan2(v.z(), std::sqrt(xx_yy)),
            std::atan2(v.y(), v.x())
        };
    }
    
    template <class T>
    math::vec3<T> spherical(T ec, T a, T ea, T b)
    {
        const T x = a * (std::cos(ea) - ec);
        const T y = b * std::sin(ea);
        const T d = std::sqrt(x * x + y * y);
        const T ta = std::atan2(y, x);
        
        return math::vec3<T>{d, T(0), ta};
    }
    
    template <class T>
    math::vec3<T> spherical(T ec, T a, T ea)
    {
        const T b = a * std::sqrt(T(1) - ec * ec);
        return spherical<T>(ec, a, ea, b);
    }
    
    template <class T>
    math::vec3<T> cartesian(const math::vec3<T>& v)
    {
        const T x = v[0] * std::cos(v[1]);
        
        return math::vec3<T>
        {
            x * std::cos(v[2]),
            x * std::sin(v[2]),
            v[0] * std::sin(v[1]),
        };
    }
    
    template <class T>
    math::vec3<T> cartesian(T ec, T a, T ea, T b)
    {
        return cartesian<T>(spherical<T>(ec, a, ea, b));
    }
    
    template <class T>
    math::vec3<T> cartesian(T ec, T a, T ea)
    {
        return cartesian<T>(spherical<T>(ec, a, ea));
    }
 
    template <typename T>
    math::se3<T> to_bci(T om, T in, T w)
    {
        const math::quaternion<T> r = math::normalize
        (
            math::quaternion<T>::rotate_z(om) *
                math::quaternion<T>::rotate_x(in) *
                math::quaternion<T>::rotate_z(w)
        );
        
        return math::se3<T>{{T(0), T(0), T(0)}, r};
    }
 
} // namespace pqw
 
namespace bci {
    
    template <class T>
    math::vec3<T> cartesian(const math::vec3<T>& v)
    {
        const T x = v[0] * std::cos(v[1]);
        
        return math::vec3<T>
        {
            x * std::cos(v[2]),
            x * std::sin(v[2]),
            v[0] * std::sin(v[1]),
        };
    }
    
    template <class T>
    math::vec3<T> spherical(const math::vec3<T>& v)
    {
        const T xx_yy = v.x() * v.x() + v.y() * v.y();
        
        return math::vec3<T>
        {
            std::sqrt(xx_yy + v.z() * v.z()),
            std::atan2(v.z(), std::sqrt(xx_yy)),
            std::atan2(v.y(), v.x())
        };
    }
    
    template <typename T>
    math::se3<T> to_bcbf(T ra, T dec, T w)
    {
        const math::quaternion<T> r = math::normalize
        (
            math::quaternion<T>::rotate_z(-math::half_pi<T> - ra) *
                math::quaternion<T>::rotate_x(dec - math::half_pi<T>) *
                math::quaternion<T>::rotate_z(-w)
        );
        
        return math::se3<T>{{T(0), T(0), T(0)}, r};
    }
    
    template <typename T>
    math::se3<T> to_pqw(T om, T in, T w)
    {
        const math::quaternion<T> r = math::normalize
        (
            math::quaternion<T>::rotate_z(-w) *
                math::quaternion<T>::rotate_x(-in) *
                math::quaternion<T>::rotate_z(-om)
        );
        
        return math::se3<T>{{T(0), T(0), T(0)}, r};
    }
    
} // namespace bci
 
namespace bcbf {
    
    template <class T>
    math::vec3<T> cartesian(const math::vec3<T>& v)
    {
        const T x = v[0] * std::cos(v[1]);
        
        return math::vec3<T>
        {
            x * std::cos(v[2]),
            x * std::sin(v[2]),
            v[0] * std::sin(v[1]),
        };
    }
    
    template <class T>
    math::vec3<T> spherical(const math::vec3<T>& v)
    {
        const T xx_yy = v.x() * v.x() + v.y() * v.y();
        
        return math::vec3<T>
        {
            std::sqrt(xx_yy + v.z() * v.z()),
            std::atan2(v.z(), std::sqrt(xx_yy)),
            std::atan2(v.y(), v.x())
        };
    }
    
    template <typename T>
    math::se3<T> to_bci(T ra, T dec, T w)
    {
        const math::quaternion<T> r = math::normalize
        (
            math::quaternion<T>::rotate_z(w) *
                math::quaternion<T>::rotate_x(math::half_pi<T> - dec) *
                math::quaternion<T>::rotate_z(ra + math::half_pi<T>)
 
        );
        
        return math::se3<T>{{T(0), T(0), T(0)}, r};
    }
    
    template <typename T>
    math::se3<T> to_enu(T distance, T latitude, T longitude)
    {
        const math::vec3<T> t = {T(0), T(0), -distance};
        const math::quaternion<T> r = math::normalize
        (
            math::quaternion<T>::rotate_x(-math::half_pi<T> + latitude) *
                math::quaternion<T>::rotate_z(-longitude - math::half_pi<T>)
        );
        
        return math::se3<T>{t, r};
    }
 
} // namespace bcbf
 
namespace enu {
    
    template <class T>
    math::vec3<T> cartesian(const math::vec3<T>& v)
    {
        const T x = v[0] * std::cos(v[1]);
        const T y = math::half_pi<T> - v[2];
        
        return math::vec3<T>
        {
            x * std::cos(y),
            x * std::sin(y),
            v[0] * std::sin(v[1]),
        };
    }
    
    template <class T>
    math::vec3<T> spherical(const math::vec3<T>& v)
    {
        const T xx_yy = v.x() * v.x() + v.y() * v.y();
        
        return math::vec3<T>
        {
            std::sqrt(xx_yy + v.z() * v.z()),
            std::atan2(v.z(), std::sqrt(xx_yy)),
            math::half_pi<T> - std::atan2(v.y(), v.x())
        };
    }
    
    template <typename T>
    math::se3<T> to_bcbf(T distance, T latitude, T longitude)
    {
        const math::vec3<T> t = {T(0), T(0), distance};
        const math::quaternion<T> r = math::normalize
        (
            math::quaternion<T>::rotate_z(longitude + math::half_pi<T>) *
                math::quaternion<T>::rotate_x(math::half_pi<T> - latitude)
        );
        
        return math::se3<T>{r * t, r};
    }
    
} // namespace enu
 
} // namespace frame
} // namespace orbit
} // namespace physics
 
#endif // ANTKEEPER_PHYSICS_ORBIT_FRAME_HPP