orientation.h

/*******************************************************************************
 * Copyright (c) 2021-2024, Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
 *
 * This program and the accompanying materials are made
 * available under the terms of the Eclipse Public License 2.0
 * which is available at https://www.eclipse.org/legal/epl-2.0/
 *
 * SPDX-License-Identifier: EPL-2.0
 *******************************************************************************/

//-----------------------------------------------------------------------------
/** @file  orientation.h */
//-----------------------------------------------------------------------------

#ifndef MANTLEAPI_COMMON_ORIENTATION_H
#define MANTLEAPI_COMMON_ORIENTATION_H

#include <MantleAPI/Common/floating_point_helper.h>
#include <MantleAPI/Common/unit_definitions.h>
#include <units.h>

namespace mantle_api
{
template <typename T,
          class = typename std::enable_if_t<units::traits::is_angle_unit<T>::value ||
                                            units::traits::is_angular_velocity_unit<T>::value ||
                                            units::traits::is_angular_acceleration_unit<T>::value>>

/// Definition of the orientation in terms of the Yaw/Pitch/Roll orientation angles in the Cartesian coordinate system
struct Orientation3
{
  Orientation3() = default;

  /// Constructor
  ///
  /// @param[in] yaw_in   yaw angle
  /// @param[in] pitch_in pitch angle
  /// @param[in] roll_in  roll angle
  Orientation3(T yaw_in, T pitch_in, T roll_in)
      : yaw{yaw_in}, pitch{pitch_in}, roll{roll_in}
  {
  }

  T yaw{};    ///< Yaw represents the rotation around the vertical axis (heading angle)
  T pitch{};  ///< Pitch represents the rotation around the lateral axis (elevation angle)
  T roll{};   ///< Roll represents the rotation around the longitudinal axis (bank angle)
};

/// @brief  almost-equality
/// @details  Compares the values of two orientations.
/// @tparam T the value type
/// @param[in]  lhs The left-hand side value for the comparison
/// @param[in]	rhs The right-hand side value for the comparison
/// @returns  true if the values of lhs almost equal to the values of rhs.
template <typename T>
constexpr bool operator==(const Orientation3<T>& lhs, const Orientation3<T>& rhs) noexcept
{
  return AlmostEqual(lhs.yaw, rhs.yaw) && AlmostEqual(lhs.pitch, rhs.pitch) && AlmostEqual(lhs.roll, rhs.roll);
}

/// @brief  inequality
/// @details  Compares the value of two orientations.
/// @tparam T the value type
/// @param[in]  lhs left-hand side value for the comparison
/// @param[in]	rhs right-hand side value for the comparison
/// @returns  true if the value of lhs is not almost equal to the value of rhs.
template <typename T>
constexpr bool operator!=(const Orientation3<T>& lhs, const Orientation3<T>& rhs) noexcept
{
  return !(lhs == rhs);
}

/// @brief addition
/// @details Returns the sum of two orientations
/// @tparam T the value type
/// @param[in]  lhs left-hand side value for the sum
/// @param[in]	rhs right-hand side value for the sum
/// @returns sum of lhs and rhs.
template <typename T>
constexpr Orientation3<T> operator+(const Orientation3<T>& lhs, const Orientation3<T>& rhs) noexcept
{
  return Orientation3<T>{lhs.yaw + rhs.yaw, lhs.pitch + rhs.pitch, lhs.roll + rhs.roll};
}

/// @brief subtraction
/// @details Returns the difference of two orientations
/// @tparam T the value type
/// @param[in]  lhs left-hand side value for the difference
/// @param[in]	rhs right-hand side value for the difference
/// @returns difference of lhs and rhs.
template <typename T>
constexpr Orientation3<T> operator-(const Orientation3<T>& lhs, const Orientation3<T>& rhs) noexcept
{
  return Orientation3<T>{lhs.yaw - rhs.yaw, lhs.pitch - rhs.pitch, lhs.roll - rhs.roll};
}

}  // namespace mantle_api

#endif  // MANTLEAPI_COMMON_ORIENTATION_H