Coordinate.h

Go to the documentation of this file.

 
#include "ReferenceFrame.h"
 
#ifndef _GAMS_POSE_COORDINATE_H_
#define _GAMS_POSE_COORDINATE_H_
 
#include "gams/GamsExport.h"
#include <string>
#include <cfloat>
#include <utility>
#include "gams/CPP11_compat.h"
#include "madara/knowledge/containers/DoubleVector.h"
#include "madara/knowledge/containers/NativeDoubleVector.h"
#include "ReferenceFrameFwd.h"
 
 
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
  #include "Eigen/Geometry"
#pragma GCC diagnostic pop
#elif defined _WIN32
  #include "Eigen/Geometry"
#endif
 
#define INVAL_COORD DBL_MAX
 
namespace gams { namespace pose {
 
namespace units {
  template<typename T>
  struct absolute {};
 
  struct length {};
  struct velocity {};
  struct acceleration {};
  struct plane_angle {};
  struct angular_velocity {};
  struct angular_acceleration {};
 
  template<typename T>
  struct base_of
  {
    using type = T;
  };
 
  template<typename T>
  struct base_of<absolute<T>>
  {
    using type = T;
  };
 
  template<typename L, typename R>
  struct same_base :
    std::is_same<typename base_of<L>::type,
                 typename base_of<R>::type> {};
}
 
template<typename Units>
struct unit_traits
{
  static_assert(sizeof(Units) < 0, "Unit type not supported by BasicVector");
};
 
struct default_unit_traits
{
  class storage_mixin
  {
  private:
    Eigen::Vector3d vec_;
 
  public:
    storage_mixin()
      : vec_(0, 0, 0) {}
 
    storage_mixin(double x, double y)
      : vec_(x, y, 0) {}
 
    storage_mixin(double x, double y, double z)
      : vec_(x, y, z) {}
 
    template<typename T, size_t N,
      typename std::enable_if<std::is_floating_point<T>::value, int>::type = 0>
    storage_mixin(T (&a)[N])
      : vec_(N>0 ? a[0] : 0, N>1 ? a[1] : 0, N>2 ? a[2] : 0) {}
 
    explicit storage_mixin(const Eigen::Vector3d &vec) : vec_(vec) {}
 
    explicit storage_mixin(const madara::knowledge::containers::DoubleVector &v)
      : vec_(v.size() > 0 ? v[0] : 0,
             v.size() > 1 ? v[1] : 0,
             v.size() > 2 ? v[2] : 0) {}
 
    explicit storage_mixin(const madara::knowledge::containers::NativeDoubleVector &v)
      : vec_(v.size() > 0 ? v[0] : 0,
             v.size() > 1 ? v[1] : 0,
             v.size() > 2 ? v[2] : 0) {}
 
    Eigen::Vector3d &vec() { return vec_; }
    const Eigen::Vector3d &vec() const { return vec_; }
  };
};
 
struct default_positional_unit_traits : default_unit_traits
{
  static const bool positional = true;
};
 
class Quaternion;
 
inline Eigen::Vector3d quat_to_axis_angle(const Eigen::Quaterniond &quat)
{
  double norm = sqrt(quat.x() * quat.x() +
                     quat.y() * quat.y() +
                     quat.z() * quat.z());
  double angle = 2 * atan2(norm, quat.w());
  double sin_half_angle = sin(angle / 2);
  if(sin_half_angle < 1e-10)
  {
    return {0, 0, 0};
  }
  return {(quat.x() / sin_half_angle) * angle,
          (quat.y() / sin_half_angle) * angle,
          (quat.z() / sin_half_angle) * angle};
}
 
struct default_rotational_unit_traits : default_unit_traits
{
  static const bool positional = false;
 
  class storage_mixin : public default_unit_traits::storage_mixin
  {
  public:
    using Base = default_unit_traits::storage_mixin;
    using Base::Base;
 
    storage_mixin() = default;
 
    storage_mixin(const Quaternion &quat);
 
    template<typename Units>
    storage_mixin(double rx, double ry, double rz, Units units)
      : Base(units.to_radians(rx),
             units.to_radians(ry),
             units.to_radians(rz)) {}
 
    template<typename T, typename Units, size_t N,
      typename std::enable_if<std::is_floating_point<T>::value, int>::type = 0>
    storage_mixin(T (&a)[N], Units units)
      : Base(
          N>0 ? units.to_radians(a[0]) : 0,
          N>1 ? units.to_radians(a[1]) : 0,
          N>2 ? units.to_radians(a[2]) : 0) {}
 
  public:
    storage_mixin(const Eigen::Quaterniond &quat)
      : Base(quat_to_axis_angle(quat)) {}
  };
};
 
template<typename Derived>
struct basic_positional_mixin
{
  using derived_type = Derived;
  Derived &self() { return static_cast<Derived&>(*this); }
  const Derived &self() const { return static_cast<const Derived&>(*this); }
 
  double x() const { return self().vec()[0]; }
  double y() const { return self().vec()[1]; }
  double z() const { return self().vec()[2]; }
 
  double x(double v) { return (self().vec()[0] = v); }
  double y(double v) { return (self().vec()[1] = v); }
  double z(double v) { return (self().vec()[2] = v); }
};
 
template<>
struct unit_traits<units::absolute<units::length>>
  : default_positional_unit_traits
{
  static const bool free = false;
 
  template<typename Derived>
  struct mixin : basic_positional_mixin<Derived> {
    using Base = basic_positional_mixin<Derived>;
    using Base::x;
    using Base::y;
    using Base::z;
    using Base::self;
 
    double lng () const { return x(); }
    double lon () const { return x(); }
    double longitude () const { return x(); }
 
    double lat () const { return y(); }
    double latitude () const { return y(); }
 
    double alt () const { return -z(); }
    double altitude () const { return -z(); }
 
    double lng (double v) { return x(v); }
    double lon (double v) { return x(v); }
    double longitude (double v) { return x(v); }
 
    double lat (double v) { return y(v); }
    double latitude (double v) { return y(v); }
 
    double alt (double v) { return z(-v); }
    double altitude (double v) { return z(-v); }
 
    Eigen::Vector3d &pos_vec() { return self().vec(); }
    const Eigen::Vector3d &pos_vec() const { return self().vec(); }
  };
};
 
template<>
struct unit_traits<units::length>
  : default_positional_unit_traits
{
  static const bool free = true;
 
  template<typename Derived>
  struct mixin : basic_positional_mixin<Derived> {
    using Base = basic_positional_mixin<Derived>;
    using Base::self;
 
    Eigen::Vector3d &dis_vec() { return self().vec(); }
    const Eigen::Vector3d &dis_vec() const { return self().vec(); }
  };
};
 
template<>
struct unit_traits<units::velocity>
  : default_positional_unit_traits
{
  static const bool free = true;
 
  template<typename Derived>
  struct mixin {
    Derived &self() { return static_cast<Derived&>(*this); }
    const Derived &self() const { return static_cast<const Derived&>(*this); }
 
    double dx() const { return self().vec()[0]; }
    double dy() const { return self().vec()[1]; }
    double dz() const { return self().vec()[2]; }
 
    double dx(double v) { return (self().vec()[0] = v); }
    double dy(double v) { return (self().vec()[1] = v); }
    double dz(double v) { return (self().vec()[2] = v); }
 
    Eigen::Vector3d &vel_vec() { return self().vec(); }
    const Eigen::Vector3d &vel_vec() const { return self().vec(); }
  };
};
 
template<>
struct unit_traits<units::acceleration>
  : default_positional_unit_traits
{
  static const bool positional = true;
  static const bool free = true;
 
  template<typename Derived>
  struct mixin {
    Derived &self() { return static_cast<Derived&>(*this); }
    const Derived &self() const { return static_cast<const Derived&>(*this); }
 
    double ddx() const { return self().vec()[0]; }
    double ddy() const { return self().vec()[1]; }
    double ddz() const { return self().vec()[2]; }
 
    double ddx(double v) { return (self().vec()[0] = v); }
    double ddy(double v) { return (self().vec()[1] = v); }
    double ddz(double v) { return (self().vec()[2] = v); }
 
    Eigen::Vector3d &acc_vec() { return self().vec(); }
    const Eigen::Vector3d &acc_vec() const { return self().vec(); }
  };
};
 
template<typename Derived>
struct common_rotational_mixin
{
  Derived &self() { return static_cast<Derived&>(*this); }
  const Derived &self() const { return static_cast<const Derived&>(*this); }
 
  double angle_to(const Derived &target) const
  {
    return self().distance_to(target);
  }
 
  template<typename U>
  double angle_to (const Derived &target, U u) const
  {
    return u.from_radians (self().angle_to (target));
  }
};
 
template<typename Derived>
struct basic_rotational_mixin : common_rotational_mixin<Derived>
{
  using Base = common_rotational_mixin<Derived>;
  using Base::self;
 
  double rx() const { return self().vec()[0]; }
  double ry() const { return self().vec()[1]; }
  double rz() const { return self().vec()[2]; }
 
  double rx(double v) { return (self().vec()[0] = v); }
  double ry(double v) { return (self().vec()[1] = v); }
  double rz(double v) { return (self().vec()[2] = v); }
 
  Eigen::Quaterniond into_quat() const
  {
    double magnitude = sqrt(rx() * rx() + ry() * ry() + rz() * rz());
    if(magnitude == 0)
    {
      return {1, 0, 0, 0};
    }
    double half_mag = magnitude / 2;
    double cos_half_mag = cos(half_mag);
    double sin_half_mag = sin(half_mag);
    return {cos_half_mag,
            (rx() / magnitude) * sin_half_mag,
            (ry() / magnitude) * sin_half_mag,
            (rz() / magnitude) * sin_half_mag};
  }
 
  void from_quat(const Eigen::Quaterniond &quat)
  {
    this->vec() = quat_to_axis_angle(quat);
  }
 
  template<typename Other>
  auto slerp(double scale, const Other &other) ->
    typename std::decay<decltype(other.into_quat(),
        std::declval<Derived>())>::type
  {
    return Derived(into_quat().slerp(scale, other.into_quat()));
  }
 
  template<typename Other>
  auto slerp(const Other &other, double scale) ->
    typename std::decay<decltype(other.into_quat(),
        std::declval<Derived>())>::type
  {
    return slerp(scale, other);
  }
 
  template<typename Other>
  auto slerp_this(double scale, const Other &other) ->
    decltype(other.into_quat(), void())
  {
    self() = Derived(into_quat().slerp(scale, other.into_quat()));
  }
 
  template<typename Other>
  auto slerp_this(const Other &other, double scale) ->
    decltype(other.into_quat(), void())
  {
    return slerp_this(scale, other);
  }
};
 
template<>
struct unit_traits<units::absolute<units::plane_angle>>
  : default_rotational_unit_traits
{
  static const bool free = false;
 
  template<typename Derived>
  struct mixin : basic_rotational_mixin<Derived> {
    using Base = basic_rotational_mixin<Derived>;
    using Base::self;
 
    Eigen::Vector3d &ori_vec() { return self().vec(); }
    const Eigen::Vector3d &ori_vec() const { return self().vec(); }
  };
};
 
template<>
struct unit_traits<units::plane_angle>
  : default_rotational_unit_traits
{
  static const bool free = true;
 
  template<typename Derived>
  struct mixin : basic_rotational_mixin<Derived> {
    using Base = basic_rotational_mixin<Derived>;
    using Base::self;
 
    Eigen::Vector3d &rot_vec() { return self().vec(); }
    const Eigen::Vector3d &rot_vec() const { return self().vec(); }
  };
};
 
template<>
struct unit_traits<units::angular_velocity>
  : default_rotational_unit_traits
{
  static const bool free = true;
 
  template<typename Derived>
  struct mixin {
    Derived &self() { return static_cast<Derived&>(*this); }
    const Derived &self() const { return static_cast<const Derived&>(*this); }
 
    double drx() const { return self().vec()[0]; }
    double dry() const { return self().vec()[1]; }
    double drz() const { return self().vec()[2]; }
 
    double drx(double v) { return (self().vec()[0] = v); }
    double dry(double v) { return (self().vec()[1] = v); }
    double drz(double v) { return (self().vec()[2] = v); }
 
    Eigen::Vector3d &ang_vel_vec() { return self().vec(); }
    const Eigen::Vector3d &ang_vel_vec() const { return self().vec(); }
  };
};
 
template<>
struct unit_traits<units::angular_acceleration>
  : default_rotational_unit_traits
{
  static const bool positional = true;
  static const bool free = true;
 
  template<typename Derived>
  struct mixin {
    Derived &self() { return static_cast<Derived&>(*this); }
    const Derived &self() const { return static_cast<const Derived&>(*this); }
 
    double ddrx() const { return self().vec()[0]; }
    double ddry() const { return self().vec()[1]; }
    double ddrz() const { return self().vec()[2]; }
 
    double ddrx(double v) { return (self().vec()[0] = v); }
    double ddry(double v) { return (self().vec()[1] = v); }
    double ddrz(double v) { return (self().vec()[2] = v); }
 
    Eigen::Vector3d &ang_acc_vec() { return self().vec(); }
    const Eigen::Vector3d &ang_acc_vec() const { return self().vec(); }
  };
};
 
template<typename T>
struct is_positional : std::integral_constant<bool, T::positional()> {};
 
template<typename T>
struct is_rotational : std::integral_constant<bool, T::rotational()> {};
 
template<typename Derived, typename Units>
class BasicVector
  : public unit_traits<Units>::storage_mixin,
    public unit_traits<Units>::template mixin<Derived>
{
private:
  Eigen::Vector3d vec_;
 
  using traits = unit_traits<Units>;
  using storage_mixin = typename unit_traits<Units>::storage_mixin;
  using mixin = typename unit_traits<Units>::template mixin<Derived>;
 
public:
  using storage_mixin::storage_mixin;
  using storage_mixin::vec;
 
  using mixin::self;
 
  using derived_type = Derived;
  using units_type = Units;
 
  BasicVector() = default;
 
  template<typename Derived2>
  BasicVector(const BasicVector<Derived2, Units> &v) : storage_mixin(v.vec()) {}
 
  static constexpr bool positional() { return traits::positional; }
 
  static constexpr bool rotational() { return !positional(); }
 
  static constexpr bool free() { return traits::free; }
 
  static constexpr bool fixed() { return !free(); }
 
  size_t size() const { return (size_t)vec().size(); }
 
  double get(size_t i) const { return vec()[i]; }
 
  double set(size_t i, double v) { return (vec()[i] = v); }
 
  bool is_set() const
  {
    for (int i = 0; i < vec().size(); ++i) {
      if (vec()[i] != INVAL_COORD) {
        return true;
      }
    }
    return false;
  }
 
  bool is_zero() const
  {
    for (int i = 0; i < vec().size(); ++i) {
      if (vec()[i] != 0) {
        return false;
      }
    }
    return true;
  }
 
  template<typename ContainType>
  void to_array(ContainType &out) const
  {
    for(size_t i = 0; i < size(); i++)
    {
      out[i] = get(i);
    }
  }
 
  template<typename ContainType>
  void from_array(const ContainType &in)
  {
    for(size_t i = 0; i < size(); i++)
    {
      set(i, in[i]);
    }
  }
 
  std::string to_string (
      const std::string & delimiter = ",",
      const std::string & unset_identifier = "<unset>") const
  {
    std::stringstream buffer;
    bool first = true;
 
    for (int i = 0; i < vec().size(); ++i)
    {
      auto x = vec()[i];
 
      if (!first) {
        buffer << delimiter;
      } else {
        first = false;
      }
 
      if (x != INVAL_COORD)
        buffer << x;
      else
        buffer << unset_identifier;
    }
 
    return buffer.str ();
  }
 
  void to_container (
    madara::knowledge::containers::NativeDoubleVector &container) const
  {
    container.set (0, this->get (0));
    container.set (1, this->get (1));
    container.set (2, this->get (2));
  }
 
  void from_container (
    const madara::knowledge::containers::NativeDoubleVector &container)
  {
    this->set (0, container[0]);
    this->set (1, container[1]);
    this->set (2, container[2]);
  }
 
  template<typename Other>
  double dot(const BasicVector<Other, Units> &other) const
  {
    return this->vec().dot(other.vec());
  }
 
  template<typename Other>
  Derived cross(const BasicVector<Other, Units> &other) const
  {
    Derived ret = this->self();
    ret.vec() = this->vec().cross(other.vec());
    return ret;
  }
 
  double norm() const
  {
    return this->vec().norm();
  }
 
  double squaredNorm() const
  {
    return this->vec().squaredNorm();
  }
 
  Derived normalized() const
  {
    Derived ret = this->self();
    ret.vec().normalize();
    return ret;
  }
 
  /*template<typename Derived2>
  bool approximately_equal(const BasicVector<Derived2, Units> &other,
      double epsilon) const
  {
    return std::fabs(self().distance_to(other.self())) < epsilon;
  }*/
 
  /*template<typename Derived2>
  double distance_to(const BasicVector<Derived2, Units> &target) const {
    return (target.vec() - vec()).norm();
  }*/
};
 
} }
 
#include "Coordinate.inl"
 
#include "Framed.h"
#include "Stamped.h"
 
namespace gams { namespace pose {
 
#define GAMS_POSE_MAKE_COMPOSITE_VEC_SCALAR_OP(op) \
template<typename Derived, typename Units, typename Scalar> \
inline auto operator op##=(BasicVector<Derived, Units> &vec, Scalar scalar) -> \
  typename std::enable_if<std::is_arithmetic<Scalar>::value && \
                          BasicVector<Derived, Units>::free(), \
                          typename BasicVector<Derived, Units>::derived_type&>::type \
{ \
  vec.vec() op##= scalar; \
  return vec.self(); \
}
 
#define GAMS_POSE_MAKE_BINARY_VEC_SCALAR_OP(op) \
template<typename Derived, typename Units, typename Scalar> \
inline auto operator op(const BasicVector<Derived, Units> &vec, Scalar scalar) -> \
  typename std::enable_if<std::is_arithmetic<Scalar>::value && \
                          BasicVector<Derived, Units>::free(), \
                          typename BasicVector<Derived, Units>::derived_type>::type \
{ \
  typename BasicVector<Derived, Units>::derived_type ret = vec.self(); \
  ret op##= scalar; \
  return ret; \
} \
\
template<typename Derived, typename Units, typename Scalar> \
inline auto operator op(Scalar scalar, const BasicVector<Derived, Units> &vec) -> \
  decltype(vec op scalar) \
{ \
  return vec op scalar; \
}
 
#define GAMS_POSE_MAKE_VEC_SCALAR_OPS(op) \
GAMS_POSE_MAKE_COMPOSITE_VEC_SCALAR_OP(op) \
GAMS_POSE_MAKE_BINARY_VEC_SCALAR_OP(op)
 
GAMS_POSE_MAKE_VEC_SCALAR_OPS(*)
GAMS_POSE_MAKE_VEC_SCALAR_OPS(/)
 
#define GAMS_POSE_MAKE_COMPOSITE_VECS_OP(op) \
template<typename LDerived, \
         typename RDerived, typename Units> \
inline auto operator op##=(BasicVector<LDerived, Units> &lhs, \
              const Eigen::MatrixBase<RDerived> &rhs) -> \
  typename std::enable_if<unit_traits<Units>::positional, \
                          LDerived &>::type \
{ \
  lhs.vec() op##= rhs; \
  return lhs.self(); \
} \
\
template<typename LDerived, \
         typename RDerived, typename Units> \
inline auto operator op##=(BasicVector<LDerived, Units> &lhs, \
                 const BasicVector<RDerived, Units> &rhs) -> \
  typename std::enable_if<unit_traits<Units>::positional && \
                          unit_traits<Units>::free, LDerived &>::type \
{ \
  lhs.vec() op##= rhs.vec(); \
  return lhs.self(); \
} \
\
template<typename LDerived, \
         typename RDerived, typename Units> \
inline auto operator op##=(BasicVector<LDerived, units::absolute<Units>> &lhs, \
                 const BasicVector<RDerived, Units> &rhs) -> \
  typename std::enable_if<unit_traits<Units>::positional && \
                          unit_traits<Units>::free, LDerived &>::type \
{ \
  lhs.vec() op##= rhs.vec(); \
  return lhs.self(); \
} \
 
GAMS_POSE_MAKE_COMPOSITE_VECS_OP(+)
GAMS_POSE_MAKE_COMPOSITE_VECS_OP(-)
 
template<typename LDerived, typename LUnits,
         typename RDerived, typename RUnits>
inline auto operator+(const BasicVector<LDerived, LUnits> &lhs,
               const BasicVector<RDerived, RUnits> &rhs) ->
  typename std::enable_if<RDerived::free(), LDerived>::type
{
  LDerived ret = lhs.self();
  ret += rhs;
  return ret;
}
 
template<typename LDerived, typename LUnits,
         typename RDerived, typename RUnits>
inline auto operator+(const BasicVector<LDerived, LUnits> &lhs,
               const BasicVector<RDerived, RUnits> &rhs) ->
  typename std::enable_if<LDerived::free() && RDerived::fixed(), RDerived>::type
{
  RDerived ret = rhs.self();
  ret += lhs;
  return ret;
}
 
template<typename LDerived, typename LUnits,
         typename RDerived, typename RUnits>
inline auto operator-(const BasicVector<LDerived, LUnits> &lhs,
               const BasicVector<RDerived, RUnits> &rhs) ->
  typename std::enable_if<LDerived::free() && RDerived::free(), LDerived>::type
{
  LDerived ret = lhs.self();
  ret -= rhs;
  return ret;
}
 
template<typename T>
struct fixed_into_free {};
 
template<typename LDerived, typename LUnits,
         typename RDerived, typename RUnits>
inline auto operator-(const BasicVector<LDerived, LUnits> &lhs,
               const BasicVector<RDerived, RUnits> &rhs) ->
  typename std::enable_if<LDerived::fixed() && RDerived::fixed(),
    typename fixed_into_free<LDerived>::type>::type
{
  typename fixed_into_free<LDerived>::type ret(lhs);
  ret.vec() -= rhs.vec();
  return ret;
}
 
template<typename LDerived, typename RDerived, typename Units>
inline bool operator == (
    const BasicVector<LDerived, Units> &lhs,
    const BasicVector<RDerived, Units> &rhs)
{
  for (int i = 0; i < lhs.vec().size(); ++i) {
    if (!(lhs.vec()[i] == rhs.vec()[i])) {
      return false;
    }
  }
  return true;
}
 
template<typename LDerived, typename RDerived, typename Units>
inline bool operator == (
    const Framed<BasicVector<LDerived, Units>> &lhs,
    const Framed<BasicVector<RDerived, Units>> &rhs)
{
  return lhs.frame() == rhs.frame() &&
    static_cast<const BasicVector<LDerived, Units> &>(lhs) ==
    static_cast<const BasicVector<RDerived, Units> &>(rhs);
}
 
template<typename LDerived, typename RDerived, typename Units>
inline bool operator == (
    const Stamped<Framed<BasicVector<LDerived, Units>>> &lhs,
    const Stamped<Framed<BasicVector<RDerived, Units>>> &rhs)
{
  return lhs.time() == rhs.time() &&
    static_cast<const Framed<BasicVector<LDerived, Units> &>>(lhs) ==
    static_cast<const Framed<BasicVector<RDerived, Units> &>>(rhs);
}
 
template<typename LDerived, typename RDerived, typename Units>
inline bool operator != (
    const BasicVector<LDerived, Units> &lhs,
    const BasicVector<RDerived, Units> &rhs)
{
  return !(lhs == rhs);
}
 
template<typename LDerived, typename RDerived, typename Units>
inline bool operator != (
    const Framed<BasicVector<LDerived, Units>> &lhs,
    const Framed<BasicVector<RDerived, Units>> &rhs)
{
  return !(lhs == rhs);
}
 
template<typename LDerived, typename RDerived, typename Units>
inline bool operator != (
    const Stamped<Framed<BasicVector<LDerived, Units>>> &lhs,
    const Stamped<Framed<BasicVector<RDerived, Units>>> &rhs)
{
  return !(lhs == rhs);
}
 
template<typename LDerived, typename RDerived, typename Units>
inline bool operator < (
    const BasicVector<LDerived, Units> &lhs,
    const BasicVector<RDerived, Units> &rhs)
{
  for (int i = 0; i < lhs.vec().size(); ++i) {
    const auto &l = lhs.vec()[i];
    const auto &r = rhs.vec()[i];
 
    if (l > r) {
      return false;
    }
 
    if (l < r) {
      return true;
    }
  }
  return false;
}
 
template<typename LDerived, typename RDerived, typename Units>
inline bool operator < (
    const Framed<BasicVector<LDerived, Units>> &lhs,
    const Framed<BasicVector<RDerived, Units>> &rhs)
{
  return &lhs.frame() < &rhs.frame() || (lhs.frame() == rhs.frame() &&
    static_cast<const BasicVector<LDerived, Units> &>(lhs) ==
    static_cast<const BasicVector<RDerived, Units> &>(rhs));
}
 
template<typename LDerived, typename RDerived, typename Units>
inline bool operator < (
    const Stamped<Framed<BasicVector<LDerived, Units>>> &lhs,
    const Stamped<Framed<BasicVector<RDerived, Units>>> &rhs)
{
  return lhs.time() < rhs.time() || (lhs.time() == rhs.time() &&
    static_cast<const Framed<BasicVector<LDerived, Units> &>>(lhs) ==
    static_cast<const Framed<BasicVector<RDerived, Units> &>>(rhs));
}
 
template<typename LDerived, typename RDerived, typename Units>
inline bool operator <= (
    const BasicVector<LDerived, Units> &lhs,
    const BasicVector<RDerived, Units> &rhs)
{
  return (lhs == rhs) || (lhs < rhs);
}
 
template<typename LDerived, typename RDerived, typename Units>
inline bool operator <= (
    const Framed<BasicVector<LDerived, Units>> &lhs,
    const Framed<BasicVector<RDerived, Units>> &rhs)
{
  return (lhs == rhs) || (lhs < rhs);
}
 
template<typename LDerived, typename RDerived, typename Units>
inline bool operator <= (
    const Stamped<Framed<BasicVector<LDerived, Units>>> &lhs,
    const Stamped<Framed<BasicVector<RDerived, Units>>> &rhs)
{
  return (lhs == rhs) || (lhs < rhs);
}
 
template<typename LDerived, typename RDerived, typename Units>
inline bool operator >= (
    const BasicVector<LDerived, Units> &lhs,
    const BasicVector<RDerived, Units> &rhs)
{
  return !(lhs < rhs);
}
 
template<typename LDerived, typename RDerived, typename Units>
inline bool operator >= (
    const Framed<BasicVector<LDerived, Units>> &lhs,
    const Framed<BasicVector<RDerived, Units>> &rhs)
{
  return !(lhs < rhs);
}
 
template<typename LDerived, typename RDerived, typename Units>
inline bool operator >= (
    const Stamped<Framed<BasicVector<LDerived, Units>>> &lhs,
    const Stamped<Framed<BasicVector<RDerived, Units>>> &rhs)
{
  return !(lhs < rhs);
}
 
template<typename LDerived, typename RDerived, typename Units>
inline bool operator > (
    const BasicVector<LDerived, Units> &lhs,
    const BasicVector<RDerived, Units> &rhs)
{
  return !(lhs <= rhs);
}
 
template<typename LDerived, typename RDerived, typename Units>
inline bool operator > (
    const Framed<BasicVector<LDerived, Units>> &lhs,
    const Framed<BasicVector<RDerived, Units>> &rhs)
{
  return !(lhs <= rhs);
}
 
template<typename LDerived, typename RDerived, typename Units>
inline bool operator > (
    const Stamped<Framed<BasicVector<LDerived, Units>>> &lhs,
    const Stamped<Framed<BasicVector<RDerived, Units>>> &rhs)
{
  return !(lhs <= rhs);
}
 
#define GAMS_POSE_MAKE_COORDINATE_TYPE(name, units) \
class name##Vector : \
  public BasicVector<name##Vector, units> \
{ \
public: \
  using Base = BasicVector<name##Vector, units>; \
  using Base::Base; \
  static constexpr const char * type_name = #name "Vector"; \
}; \
\
class name : \
  public Framed<BasicVector<name, units>> \
{ \
public: \
  using Base = Framed<BasicVector<name, units>>; \
  using Base::Base; \
  static constexpr const char * type_name = #name; \
  operator name##Vector () const { return name##Vector(this->vec()); } \
  name(name##Vector o) : Base(o.vec()) {} \
  name() = default; \
}; \
\
class Stamped##name : \
  public Stamped<Framed<BasicVector<Stamped##name, units>>> \
{ \
public: \
  using Base = Stamped<Framed<BasicVector<Stamped##name, units>>>; \
  using Base::Base; \
  static constexpr const char * type_name = "Stamped" #name; \
  operator name##Vector () const { return name##Vector(this->vec()); } \
  operator name () const { return name(this->frame(), this->vec()); } \
  Stamped##name(name##Vector o) : Base(o.vec()) {} \
  Stamped##name(name o) : Base(o.frame(), o.vec()) {} \
  Stamped##name() = default; \
}
 
class Position;
 
class Position;
 
class Velocity;
 
class Accelerations;
 
class Orientation;
 
class Rotation;
 
class AngularVelocity;
 
class AngularAccelerations;
 
GAMS_POSE_MAKE_COORDINATE_TYPE(Position, units::absolute<units::length>);
GAMS_POSE_MAKE_COORDINATE_TYPE(Displacement, units::length);
GAMS_POSE_MAKE_COORDINATE_TYPE(Velocity, units::velocity);
GAMS_POSE_MAKE_COORDINATE_TYPE(Acceleration, units::acceleration);
 
GAMS_POSE_MAKE_COORDINATE_TYPE(Orientation, units::absolute<units::plane_angle>);
GAMS_POSE_MAKE_COORDINATE_TYPE(Rotation, units::plane_angle);
GAMS_POSE_MAKE_COORDINATE_TYPE(AngularVelocity, units::angular_velocity);
GAMS_POSE_MAKE_COORDINATE_TYPE(AngularAcceleration, units::angular_acceleration);
 
template<>
struct fixed_into_free<PositionVector>
{
  using type = DisplacementVector;
};
 
template<>
struct fixed_into_free<Position>
{
  using type = Displacement;
};
 
template<>
struct fixed_into_free<StampedPosition>
{
  using type = StampedDisplacement;
};
 
template<>
struct fixed_into_free<OrientationVector>
{
  using type = RotationVector;
};
 
template<>
struct fixed_into_free<Orientation>
{
  using type = Rotation;
};
 
template<>
struct fixed_into_free<StampedOrientation>
{
  using type = StampedRotation;
};
 
inline std::ostream &operator<<(std::ostream &o, const Eigen::Vector3d &v)
{
  bool first = true;
  for (int i = 0; i < v.size(); ++i)
  {
    auto x = v[i];
 
    if (!first) {
      o << ",";
    } else {
      first = false;
    }
 
    if (x != INVAL_COORD) {
      o << x;
    } else {
      o << "<unset>";
    }
  }
  return o;
}
 
template<typename Derived, typename Units>
inline std::ostream &operator<<(std::ostream &o, const BasicVector<Derived, Units> &v)
{
  o << Derived::type_name << "(";
  o << v.vec();
  o << ")";
  return o;
}
 
} }
 
#endif