translation2.hpp

// Copyright (c) TrajoptLib contributors
 
#pragma once
 
#include <cmath>
#include <cstddef>
#include <tuple>
#include <type_traits>
#include <utility>
 
#include <sleipnir/autodiff/variable.hpp>
#include <sleipnir/autodiff/variable_matrix.hpp>
#include <sleipnir/util/concepts.hpp>
 
#include "trajopt/geometry/rotation2.hpp"
 
namespace trajopt {

template <typename T>
class Translation2 {
 public:
  constexpr Translation2() = default;

  template <typename X, typename Y>
  constexpr Translation2(X x, Y y) : m_x{std::move(x)}, m_y{std::move(y)} {}

  template <typename U>
  constexpr Translation2(T distance, const Rotation2<U>& angle)
      : m_x{distance * angle.cos()}, m_y{distance * angle.sin()} {}

  template <typename U>
  constexpr explicit Translation2(const Translation2<U>& other)
      : m_x{other.m_x}, m_y{other.m_y} {}

  constexpr const T& x() const { return m_x; }

  constexpr const T& y() const { return m_y; }

  template <typename U>
  constexpr auto operator+(const Translation2<U>& other) const {
    using R = decltype(std::declval<T>() + std::declval<U>());
    return Translation2<R>{x() + other.x(), y() + other.y()};
  }

  template <typename U>
  constexpr auto operator-(const Translation2<U>& other) const {
    return *this + -other;
  }

  constexpr Translation2<T> operator-() const { return {-m_x, -m_y}; }

  template <typename U>
  constexpr auto operator*(const U& scalar) const {
    using R = decltype(std::declval<T>() + std::declval<U>());
    return Translation2<R>{m_x * scalar, m_y * scalar};
  }

  template <typename U>
  constexpr auto operator/(const U& scalar) const {
    using R = decltype(std::declval<T>() + std::declval<U>());
    return Translation2<R>{m_x / scalar, m_y / scalar};
  }

  template <typename U>
  constexpr auto rotate_by(const Rotation2<U>& rotation) const {
    using R = decltype(std::declval<T>() + std::declval<U>());
    return Translation2<R>{m_x * rotation.cos() - m_y * rotation.sin(),
                           m_x * rotation.sin() + m_y * rotation.cos()};
  }

  constexpr Rotation2<T> angle() const { return Rotation2<T>{m_x, m_y}; }

  template <typename U>
  constexpr auto dot(const Translation2<U>& other) const {
    using R = decltype(std::declval<T>() + std::declval<U>());
    return R{m_x * other.x() + m_y * other.y()};
  }

  template <typename U>
  constexpr auto cross(const Translation2<U>& other) const {
    using R = decltype(std::declval<T>() + std::declval<U>());
    return R{m_x * other.y() - m_y * other.x()};
  }

  constexpr auto norm() const { return hypot(m_x, m_y); }

  constexpr auto squared_norm() const { return m_x * m_x + m_y * m_y; }

  template <typename U>
  constexpr auto distance(const Translation2<U>& other) const {
    return hypot(other.x() - m_x, other.y() - m_y);
  }
 
 private:
  T m_x{0};
  T m_y{0};
};
 
template <size_t I, typename T>
constexpr decltype(auto) get(const trajopt::Translation2<T>& translation) {
  if constexpr (I == 0) {
    return translation.x();
  } else {
    return translation.y();
  }
}
 
using Translation2d = Translation2<double>;
 
template <typename Scalar>
using Translation2v = Translation2<slp::Variable<Scalar>>;
 
template <typename LHS, typename RHS>
  requires slp::SleipnirType<LHS> && (!slp::SleipnirType<RHS>)
auto operator==(const Translation2<LHS>& lhs, const Translation2<RHS>& rhs) {
  using Scalar = typename std::decay_t<LHS>::Scalar;
 
  return slp::VariableMatrix<Scalar>{{lhs.x()}, {lhs.y()}} ==
         slp::VariableMatrix<Scalar>{{rhs.x()}, {rhs.y()}};
}
 
template <typename LHS, typename RHS>
  requires(!slp::SleipnirType<LHS>) || slp::SleipnirType<RHS>
auto operator==(const Translation2<LHS>& lhs, const Translation2<RHS>& rhs) {
  using Scalar = typename std::decay_t<RHS>::Scalar;
 
  return slp::VariableMatrix<Scalar>{{lhs.x()}, {lhs.y()}} ==
         slp::VariableMatrix<Scalar>{{rhs.x()}, {rhs.y()}};
}
 
template <typename LHS, typename RHS>
  requires slp::SleipnirType<LHS> && slp::SleipnirType<RHS>
auto operator==(const Translation2<LHS>& lhs, const Translation2<RHS>& rhs) {
  using Scalar = typename std::decay_t<LHS>::Scalar;
 
  return slp::VariableMatrix<Scalar>{{lhs.x()}, {lhs.y()}} ==
         slp::VariableMatrix<Scalar>{{rhs.x()}, {rhs.y()}};
}
 
inline bool operator==(const Translation2d& lhs, const Translation2d& rhs) {
  return lhs.x() == rhs.x() && lhs.y() == rhs.y();
}
 
}  // namespace trajopt
 
namespace std {

template <typename T>
struct tuple_size<trajopt::Translation2<T>>
    : std::integral_constant<size_t, 2> {};

template <typename T>
struct tuple_element<0, trajopt::Translation2<T>> {
  using type = T;
};

template <typename T>
struct tuple_element<1, trajopt::Translation2<T>> {
  using type = T;
};
 
}  // namespace std