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);
}
}
 
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;
  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)
{
  Type x = q.x();
  Type y = q.y();
  Type z = q.z();
  Type w = q.w();
 
  return std::atan2(2.0 * (w * x + y * z), w * w - x * x - y * y + z * z);
}
 
template<typename Type>
Type quaternionToPitch(const Eigen::Quaternion<Type> & q)
{
  Type x = q.x();
  Type y = q.y();
  Type z = q.z();
  Type w = q.w();
 
  return std::atan2(2.0 * (w * y - z * x), 1.0 - 2.0 * (x * x + y * y));
}
 
template<typename Type>
Type quaternionToYaw(const Eigen::Quaternion<Type> & q)
{
  Type x = q.x();
  Type y = q.y();
  Type z = q.z();
  Type w = q.w();
 
  return std::atan2(2.0 * (x * y + w * z), w * w + x * x - y * y - z * z);
}
 
}  // namespace px4_ros2