pose2.hpp

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

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

  constexpr Pose2(Translation2<T> translation, Rotation2<T> rotation)
      : m_translation{std::move(translation)},
        m_rotation{std::move(rotation)} {}

  constexpr Pose2(T x, T y, Rotation2<T> rotation)
      : m_translation{Translation2<T>{std::move(x), std::move(y)}},
        m_rotation{std::move(rotation)} {}

  template <typename U>
  constexpr explicit Pose2(const Pose2<U>& other)
      : m_translation{other.translation()}, m_rotation{other.rotation()} {}

  constexpr const Translation2<T>& translation() const { return m_translation; }

  constexpr const T& x() const { return m_translation.x(); }

  constexpr const T& y() const { return m_translation.y(); }

  constexpr const Rotation2<T>& rotation() const { return m_rotation; }

  constexpr Pose2<T> rotate_by(const Rotation2<T>& rotation) const {
    return {m_translation.rotate_by(rotation), m_rotation.rotate_by(rotation)};
  }
 
 private:
  Translation2<T> m_translation;
  Rotation2<T> m_rotation;
};
 
using Pose2d = Pose2<double>;
 
template <typename Scalar>
using Pose2v = Pose2<slp::Variable<Scalar>>;
 
template <typename LHS, typename RHS>
  requires slp::SleipnirType<LHS> && (!slp::SleipnirType<RHS>)
auto operator==(const Pose2<LHS>& lhs, const Pose2<RHS>& rhs) {
  using Scalar = typename std::decay_t<LHS>::Scalar;
 
  return slp::EqualityConstraints<Scalar>{
      {lhs.translation() == rhs.translation(),
       lhs.rotation() == rhs.rotation()}};
}
 
template <typename LHS, typename RHS>
  requires(!slp::SleipnirType<LHS>) && slp::SleipnirType<RHS>
auto operator==(const Pose2<LHS>& lhs, const Pose2<RHS>& rhs) {
  using Scalar = typename std::decay_t<RHS>::Scalar;
 
  return slp::EqualityConstraints<Scalar>{
      {lhs.translation() == rhs.translation(),
       lhs.rotation() == rhs.rotation()}};
}
 
template <typename LHS, typename RHS>
  requires slp::SleipnirType<LHS> && slp::SleipnirType<RHS>
auto operator==(const Pose2<LHS>& lhs, const Pose2<RHS>& rhs) {
  using Scalar = typename std::decay_t<LHS>::Scalar;
 
  return slp::EqualityConstraints<Scalar>{
      {lhs.translation() == rhs.translation(),
       lhs.rotation() == rhs.rotation()}};
}
 
}  // namespace trajopt