Inclination.hpp

Go to the documentation of this file.

/* This file is part of OpenKalman, a header-only C++ library for
 * Kalman filters and other recursive filters.
 *
 * Copyright (c) 2018-2021 Christopher Lee Ogden <ogden@gatech.edu>
 *
 * 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/.
 */

#ifndef OPENKALMAN_COEFFICIENTS_INCLINATION_HPP
#define OPENKALMAN_COEFFICIENTS_INCLINATION_HPP

#include <type_traits>
#ifdef __cpp_concepts
#include <concepts>
#endif
#include <cmath>
#include "../../../basics/compatibility/language-features.hpp"
#include "values/concepts/floating.hpp"
#include "values/classes/fixed-constants.hpp"
#include "values/functions/internal/update_real_part.hpp"
#include "values/math/abs.hpp"
#include "values/math/sin.hpp"
#include "values/math/cos.hpp"
#include "values/math/atan2.hpp"
#include "values/functions/cast_to.hpp"
#include "linear-algebra/coordinates/interfaces/coordinate_descriptor_traits.hpp"


namespace OpenKalman::coordinates
{
#ifdef __cpp_concepts
  template<values::fixed Down = values::fixed_minus_half_pi<long double>, values::fixed Up = values::fixed_half_pi<long double>>
  requires (values::fixed_number_of_v<Up> - values::fixed_number_of_v<Down> > 0) and
    (values::fixed_number_of_v<Down> <= 0) and (values::fixed_number_of_v<Up> >= 0)
#else
  template<typename Down = values::fixed_minus_half_pi<long double>, typename Up = values::fixed_half_pi<long double>>
#endif
  struct Inclination
  {
#ifndef __cpp_concepts
    static_assert(values::fixed<Down>);
    static_assert(values::fixed<Up>);
    static_assert(values::fixed_number_of_v<Up> - values::fixed_number_of_v<Down> > 0);
    static_assert(values::fixed_number_of_v<Down> <= 0);
    static_assert(values::fixed_number_of_v<Up> >= 0);
#endif
  };


  namespace inclination
  {
    using Radians = Inclination<>;


    using Degrees = Inclination<values::Fixed<long double, -90>, values::Fixed<long double, 90>>;


    namespace detail
    {
      template<typename T>
      struct is_inclination : std::false_type {};

      template<typename Down, typename Up>
      struct is_inclination<Inclination<Down, Up>> : std::true_type {};
    }


    template<typename T>
#ifdef __cpp_concepts
    concept inclination =
#else
    static constexpr bool inclination =
#endif
      detail::is_inclination<T>::value;

  } // namespace inclination

} // namespace OpenKalman::coordinates


namespace OpenKalman::interface
{
  template<typename Down, typename Up>
  struct coordinate_descriptor_traits<coordinates::Inclination<Down, Up>>
  {
  private:

    using T = coordinates::Inclination<Down, Up>;
    static constexpr auto down = values::fixed_number_of_v<Down>;
    static constexpr auto up = values::fixed_number_of_v<Up>;

  public:

    static constexpr bool is_specialized = true;


    static constexpr auto
    dimension(const T&) { return std::integral_constant<std::size_t, 1>{}; };


    static constexpr auto
    stat_dimension(const T&) { return std::integral_constant<std::size_t, 2>{}; };


    static constexpr auto
    is_euclidean(const T&) { return std::false_type{}; }


    static constexpr auto
    hash_code(const T&)
    {
      constexpr auto down_float = static_cast<float>(down);
      constexpr auto up_float = static_cast<float>(up);
      constexpr float a = (down_float * 3.f + up_float * 2.f) / (up_float - down_float);
      constexpr auto bits = std::numeric_limits<std::size_t>::digits;
      if constexpr (bits < 32) return 0x8CE6_uz + static_cast<std::size_t>(a * a * 0x1.p2f);
      else if constexpr (bits < 64) return 0x8CE6267E_uz + static_cast<std::size_t>(a * a * 0x1.p4f);
      else return 0x8CE6267E341642F7_uz + static_cast<std::size_t>(a * a * 0x1.p8f);
    }


#ifdef __cpp_concepts
    static constexpr values::value auto
    to_euclidean_component(const T& t, const auto& g, const values::index auto& euclidean_local_index)
    requires requires(std::size_t i){ {g(i)} -> values::value; }
#else
    template<typename Getter, typename L, std::enable_if_t<values::index<L> and
      values::value<typename std::invoke_result<const Getter&, std::size_t>::type>, int> = 0>
    static constexpr auto
    to_euclidean_component(const T& t, const Getter& g, const L& euclidean_local_index)
#endif
    {
      using Scalar = std::decay_t<decltype(g(0_uz))>;
      using R = std::decay_t<decltype(values::real(std::declval<Scalar>()))>;
      const Scalar cf {numbers::pi_v<R> / (up - down)};
      const Scalar horiz {R{up + down} * R{0.5}};

      Scalar theta = cf * (g(0_uz) - horiz); // Convert to radians

      if (euclidean_local_index == 0) return values::cos(theta);
      else return values::sin(theta);
    }


#ifdef __cpp_concepts
    static constexpr values::value auto
    from_euclidean_component(const T& t, const auto& g, const values::index auto& local_index)
    requires requires(std::size_t i){ {g(i)} -> values::value; }
#else
    template<typename Getter, typename L, std::enable_if_t<values::index<L> and
      values::value<typename std::invoke_result<const Getter&, std::size_t>::type>, int> = 0>
    static constexpr auto
    from_euclidean_component(const T& t, const Getter& g, const L& local_index)
#endif
    {
      using Scalar = std::decay_t<decltype(g(0_uz))>;
      using R = std::decay_t<decltype(values::real(std::declval<Scalar>()))>;
      const Scalar cf {numbers::pi_v<R> / (up - down)};
      const Scalar horiz {R{up + down} * R{0.5}};

      Scalar x = g(0_uz);
      // In Euclidean space, (the real part of) x must be non-negative since the inclination is in range [-½pi,½pi].
      Scalar pos_x = values::internal::update_real_part(x, values::abs(values::real(x)));
      Scalar y = g(1_uz);

      if constexpr (values::complex<Scalar>) return values::atan2(y, pos_x) / cf + horiz;
      else { return values::atan2(y, pos_x) / cf + horiz; }
    }

  private:

    template<typename A>
    static constexpr std::decay_t<A> wrap_impl(A&& a)
    {
      auto ap = values::real(a);
      if (ap >= down and ap <= up)
      {
        return std::forward<decltype(a)>(a);
      }
      else
      {
        using R = std::decay_t<decltype(ap)>;
        constexpr R range {up - down};
        constexpr R period {range * 2};
        using std::fmod;
        auto ar = fmod(ap - R{down}, period);

        if (ar < 0) return values::internal::update_real_part(std::forward<decltype(a)>(a), R{down} + ar + period);
        else if (ar > range) return values::internal::update_real_part(std::forward<decltype(a)>(a), R{down} - ar + period);
        else return values::internal::update_real_part(std::forward<decltype(a)>(a), R{down} + ar);
      }
    }

  public:

#ifdef __cpp_concepts
    static constexpr values::value auto
    get_wrapped_component(const T& t, const auto& g, const values::index auto& local_index)
    requires requires(std::size_t i){ {g(i)} -> values::value; }
#else
    template<typename Getter, typename L, std::enable_if_t<values::index<L> and
      values::value<typename std::invoke_result<const Getter&, std::size_t>::type>, int> = 0>
    static constexpr auto
    get_wrapped_component(const T& t, const Getter& g, const L& local_index)
#endif
    {
      return wrap_impl(g(0_uz));
    };


#ifdef __cpp_concepts
    static constexpr void
    set_wrapped_component(const T& t, const auto& s, const auto& g, const values::value auto& x, const values::index auto& local_index)
    requires requires(std::size_t i){ s(x, i); s(g(i), i); }
#else
    template<typename Setter, typename Getter, typename X, typename L, std::enable_if_t<values::value<X> and values::index<L> and
      std::is_invocable<const Setter&, const X&, std::size_t>::value and
      std::is_invocable<const Setter&, typename std::invoke_result<const Getter&, std::size_t>::type, std::size_t>::value, int> = 0>
    static constexpr void
    set_wrapped_component(const T& t, const Setter& s, const Getter& g, const X& x, const L& local_index)
#endif
    {
      s(wrap_impl(x), 0_uz);
    }

  };


} // namespace OpenKalman::interface


#endif //OPENKALMAN_COEFFICIENTS_INCLINATION_HPP