geometry.hpp

/****************************************************************************
 * Copyright (c) 2024 PX4 Development Team.
 * SPDX-License-Identifier: BSD-3-Clause
 ****************************************************************************/
 
#pragma once
 
#include <Eigen/Eigen>
 
namespace px4_ros2 {
template <typename Type>
Type radToDeg(Type rad)
{
  return rad * static_cast<Type>(180.0 / M_PI);
}
 
template <typename Type>
Type degToRad(Type deg)
{
  return deg * static_cast<Type>(M_PI / 180.0);
}
 
namespace literals {
static inline constexpr float operator"" _deg(long double degrees)
{
  return static_cast<float>(degrees * M_PI / 180.0);
}
static inline constexpr float operator"" _rad(long double radians)
{
  return static_cast<float>(radians);
}
}  // namespace literals
 
template <typename Type>
Type wrapPi(Type angle)
{
  while (angle >= M_PI) {
    angle -= 2.0 * M_PI;
  }
  while (angle < -M_PI) {
    angle += 2.0 * M_PI;
  }
  return angle;
}
 
template <typename Type>
Eigen::Matrix<Type, 3, 1> quaternionToEulerRpy(const Eigen::Quaternion<Type>& q)
{
  Eigen::Matrix<Type, 3, 1> angles;
  const Eigen::Matrix<Type, 3, 3> dcm = q.toRotationMatrix();
 
  angles.y() = asin(-dcm.coeff(2, 0));
 
  if ((std::fabs(angles.y() - static_cast<Type>(M_PI / 2))) < static_cast<Type>(1.0e-3)) {
    angles.x() = 0;
    angles.z() = atan2(dcm.coeff(1, 2), dcm.coeff(0, 2));
  } else if ((std::fabs(angles.y() + static_cast<Type>(M_PI / 2))) < static_cast<Type>(1.0e-3)) {
    angles.x() = 0;
    angles.z() = atan2(-dcm.coeff(1, 2), -dcm.coeff(0, 2));
  } else {
    angles.x() = atan2(dcm.coeff(2, 1), dcm.coeff(2, 2));
    angles.z() = atan2(dcm.coeff(1, 0), dcm.coeff(0, 0));
  }
 
  return angles;
}
 
template <typename Type>
Eigen::Quaternion<Type> eulerRpyToQuaternion(const Eigen::Matrix<Type, 3, 1>& euler)
{
  return Eigen::Quaternion<Type>(
      Eigen::AngleAxis<Type>(euler[2], Eigen::Matrix<Type, 3, 1>::UnitZ()) *
      Eigen::AngleAxis<Type>(euler[1], Eigen::Matrix<Type, 3, 1>::UnitY()) *
      Eigen::AngleAxis<Type>(euler[0], Eigen::Matrix<Type, 3, 1>::UnitX()));
}
 
template <typename Type>
Eigen::Quaternion<Type> eulerRpyToQuaternion(const Type roll, const Type pitch, const Type yaw)
{
  return eulerRpyToQuaternion(Eigen::Matrix<Type, 3, 1>{roll, pitch, yaw});
}
 
template <typename Type>
Type quaternionToRoll(const Eigen::Quaternion<Type>& q)
{
  return quaternionToEulerRpy(q)[0];
}
 
template <typename Type>
Type quaternionToPitch(const Eigen::Quaternion<Type>& q)
{
  return quaternionToEulerRpy(q)[1];
}
 
template <typename Type>
Type quaternionToYaw(const Eigen::Quaternion<Type>& q)
{
  return quaternionToEulerRpy(q)[2];
}
 
}  // namespace px4_ros2