misc.h

Go to the documentation of this file.

// clang: MatousFormat
#ifndef GEOMETRY_MISC_H
#define GEOMETRY_MISC_H
 
#include <cmath>
#include <Eigen/Dense>
#include <geometry_msgs/msg/point.h>
#include <geometry_msgs/msg/quaternion.h>
#include <mrs_lib/geometry/cyclic.h>
 
namespace mrs_lib
{
  namespace geometry
  {
    template <int dims>
    using vec_t = Eigen::Matrix<double, dims, 1>;
 
    using pt2_t = vec_t<2>;
    using vec2_t = vec_t<2>;
    using pt3_t = vec_t<3>;
    using vec3_t = vec_t<3>;
 
    using mat3_t = Eigen::Matrix3d;
 
    using quat_t = Eigen::Quaterniond;
    using anax_t = Eigen::AngleAxisd;
 
    template <int dims>
    vec_t<dims + 1> toHomogenous(const vec_t<dims>& vec)
    {
      const Eigen::Matrix<double, dims + 1, 1> ret((Eigen::Matrix<double, dims + 1, 1>() << vec, 1).finished());
      return ret;
    }
 
    // | ----------------- Angle-related functions ---------------- |
 
    /* angle-related functions //{ */
 
    /* headingNEDtoENU() //{ */
 
    template <typename OutRangeT = sradians>
    double headingNEDtoENU(const double& heading_ned)
    {
      const OutRangeT heading_enu(M_PI_2 - heading_ned);
      return heading_enu.value();
    }
 
    //}
 
    /* headingENUtoNED() //{ */
 
    template <typename OutRangeT = radians>
    double headingENUtoNED(const double& heading_enu)
    {
      const OutRangeT heading_ned(M_PI_2 - heading_enu);
      return heading_ned.value();
    }
 
    //}
 
    /* headingFromRot() //{ */
 
    template <typename T>
    double headingFromRot(const T& rot)
    {
      const vec3_t rot_vec = rot * vec3_t::UnitX();
      return std::atan2(rot_vec.y(), rot_vec.x());
    }
 
    //}
 
    /* angleBetween() //{ */
 
    double angleBetween(const vec2_t& a, const vec2_t& b);
 
    double angleBetween(const vec3_t& a, const vec3_t& b);
 
    //}
 
    /* angleaxisBetween() //{ */
 
    anax_t angleaxisBetween(const vec3_t& a, const vec3_t& b, const double tolerance = 1e-9);
 
    //}
 
    /* quaternionBetween() //{ */
 
    quat_t quaternionBetween(const vec3_t& a, const vec3_t& b, const double tolerance = 1e-9);
 
    //}
 
    /* rotationBetween() //{ */
 
    Eigen::Matrix3d rotationBetween(const vec3_t& a, const vec3_t& b, const double tolerance = 1e-9);
 
    //}
 
    /* haversin() //{ */
 
    double haversin(const double angle);
 
    //}
 
    /* invHaversin() //{ */
 
    double invHaversin(const double value);
 
    //}
 
    /* solidAngle //{ */
 
    double solidAngle(double a, double b, double c);
 
    //}
 
    /* quaternionFromEuler() //{ */
 
    quat_t quaternionFromEuler(double x, double y, double z);
 
    quat_t quaternionFromEuler(const Eigen::Vector3d& euler);
 
    //}
 
    quat_t quaternionFromHeading(const double heading);
 
    //}
 
    // | ----------------- Miscellaneous functions ---------------- |
 
    /* 2D cross() //{ */
 
    double cross(const vec2_t& a, const vec2_t b);
 
    //}
 
    /* vector distance //{ */
 
    double dist(const vec2_t& a, const vec2_t& b);
 
    double dist(const vec3_t& a, const vec3_t& b);
 
    //}
 
    /* triangleArea() //{ */
 
    double triangleArea(const double a, const double b, const double c);
 
    //}
 
    /* sphericalTriangleArea //{ */
 
    double sphericalTriangleArea(Eigen::Vector3d a, Eigen::Vector3d b, Eigen::Vector3d c);
 
    //}
 
    /* rotateCovariance() //{ */
 
    template <typename T>
    mat3_t rotateCovariance(const mat3_t& cov, const T& rot)
    {
      const mat3_t matrot(rot);
      return matrot * cov * matrot.transpose();
    }
 
    //}
 
  } // namespace geometry
} // namespace mrs_lib
 
#endif