RotationBase.h

// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
// SPDX-License-Identifier: MPL-2.0
 
#ifndef EIGEN_ROTATIONBASE_H
#define EIGEN_ROTATIONBASE_H
 
// IWYU pragma: private
#include "./InternalHeaderCheck.h"
 
namespace Eigen {
 
// forward declaration
namespace internal {
template <typename RotationDerived, typename MatrixType, bool IsVector = MatrixType::IsVectorAtCompileTime>
struct rotation_base_generic_product_selector;
}

template <typename Derived, int Dim_>
class RotationBase {
 public:
  enum { Dim = Dim_ };
  using Scalar = typename internal::traits<Derived>::Scalar;

  using RotationMatrixType = Matrix<Scalar, Dim, Dim>;
  using VectorType = Matrix<Scalar, Dim, 1>;
 
 public:
  EIGEN_DEVICE_FUNC inline const Derived& derived() const { return *static_cast<const Derived*>(this); }
  EIGEN_DEVICE_FUNC inline Derived& derived() { return *static_cast<Derived*>(this); }

  EIGEN_DEVICE_FUNC inline RotationMatrixType toRotationMatrix() const { return derived().toRotationMatrix(); }

  EIGEN_DEVICE_FUNC inline RotationMatrixType matrix() const { return derived().toRotationMatrix(); }

  EIGEN_DEVICE_FUNC inline Derived inverse() const { return derived().inverse(); }

  EIGEN_DEVICE_FUNC inline Transform<Scalar, Dim, Isometry> operator*(const Translation<Scalar, Dim>& t) const {
    return Transform<Scalar, Dim, Isometry>(*this) * t;
  }

  EIGEN_DEVICE_FUNC inline RotationMatrixType operator*(const UniformScaling<Scalar>& s) const {
    return toRotationMatrix() * s.factor();
  }

  template <typename OtherDerived>
  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
      typename internal::rotation_base_generic_product_selector<Derived, OtherDerived,
                                                                OtherDerived::IsVectorAtCompileTime>::ReturnType
      operator*(const EigenBase<OtherDerived>& e) const {
    return internal::rotation_base_generic_product_selector<Derived, OtherDerived>::run(derived(), e.derived());
  }

  template <typename OtherDerived>
  friend EIGEN_DEVICE_FUNC inline RotationMatrixType operator*(const EigenBase<OtherDerived>& l, const Derived& r) {
    return l.derived() * r.toRotationMatrix();
  }

  EIGEN_DEVICE_FUNC friend inline Transform<Scalar, Dim, Affine> operator*(const DiagonalMatrix<Scalar, Dim>& l,
                                                                           const Derived& r) {
    Transform<Scalar, Dim, Affine> res(r);
    res.linear().applyOnTheLeft(l);
    return res;
  }

  template <int Mode, int Options>
  EIGEN_DEVICE_FUNC inline Transform<Scalar, Dim, Mode> operator*(
      const Transform<Scalar, Dim, Mode, Options>& t) const {
    return toRotationMatrix() * t;
  }
 
  template <typename OtherVectorType>
  EIGEN_DEVICE_FUNC inline VectorType _transformVector(const OtherVectorType& v) const {
    return toRotationMatrix() * v;
  }
};
 
namespace internal {
 
// implementation of the generic product rotation * matrix
template <typename RotationDerived, typename MatrixType>
struct rotation_base_generic_product_selector<RotationDerived, MatrixType, false> {
  enum { Dim = RotationDerived::Dim };
  using ReturnType = Matrix<typename RotationDerived::Scalar, Dim, Dim>;
  EIGEN_DEVICE_FUNC static inline ReturnType run(const RotationDerived& r, const MatrixType& m) {
    return r.toRotationMatrix() * m;
  }
};
 
template <typename RotationDerived, typename Scalar, int Dim, int MaxDim>
struct rotation_base_generic_product_selector<RotationDerived, DiagonalMatrix<Scalar, Dim, MaxDim>, false> {
  using ReturnType = Transform<Scalar, Dim, Affine>;
  EIGEN_DEVICE_FUNC static inline ReturnType run(const RotationDerived& r,
                                                 const DiagonalMatrix<Scalar, Dim, MaxDim>& m) {
    ReturnType res(r);
    res.linear() *= m;
    return res;
  }
};
 
template <typename RotationDerived, typename OtherVectorType>
struct rotation_base_generic_product_selector<RotationDerived, OtherVectorType, true> {
  enum { Dim = RotationDerived::Dim };
  using ReturnType = Matrix<typename RotationDerived::Scalar, Dim, 1>;
  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE ReturnType run(const RotationDerived& r, const OtherVectorType& v) {
    return r._transformVector(v);
  }
};
 
}  // end namespace internal

template <typename Scalar_, int Rows_, int Cols_, int Storage_, int MaxRows_, int MaxCols_>
template <typename OtherDerived>
EIGEN_DEVICE_FUNC Matrix<Scalar_, Rows_, Cols_, Storage_, MaxRows_, MaxCols_>::Matrix(
    const RotationBase<OtherDerived, ColsAtCompileTime>& r) {
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix, int(OtherDerived::Dim), int(OtherDerived::Dim))
  *this = r.toRotationMatrix();
}

template <typename Scalar_, int Rows_, int Cols_, int Storage_, int MaxRows_, int MaxCols_>
template <typename OtherDerived>
EIGEN_DEVICE_FUNC Matrix<Scalar_, Rows_, Cols_, Storage_, MaxRows_, MaxCols_>&
Matrix<Scalar_, Rows_, Cols_, Storage_, MaxRows_, MaxCols_>::operator=(
    const RotationBase<OtherDerived, ColsAtCompileTime>& r) {
  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix, int(OtherDerived::Dim), int(OtherDerived::Dim))
  return *this = r.toRotationMatrix();
}
 
namespace internal {

template <typename Scalar, int Dim, typename OtherScalar,
          std::enable_if_t<std::is_scalar<OtherScalar>::value, int> = 0>
EIGEN_DEVICE_FUNC static inline Matrix<Scalar, 2, 2> toRotationMatrix(const OtherScalar& s) {
  EIGEN_STATIC_ASSERT(Dim == 2, YOU_MADE_A_PROGRAMMING_MISTAKE)
  return Rotation2D<Scalar>(static_cast<Scalar>(s)).toRotationMatrix();
}
 
template <typename Scalar, int Dim, typename OtherDerived,
          std::enable_if_t<std::is_same<Scalar, typename OtherDerived::Scalar>::value, int> = 0>
EIGEN_DEVICE_FUNC static inline typename OtherDerived::RotationMatrixType toRotationMatrix(
    const RotationBase<OtherDerived, Dim>& r) {
  return r.toRotationMatrix();
}
 
template <typename Scalar, int Dim, typename OtherDerived,
          std::enable_if_t<!std::is_same<Scalar, typename OtherDerived::Scalar>::value, int> = 0>
EIGEN_DEVICE_FUNC static inline Matrix<Scalar, Dim, Dim> toRotationMatrix(const RotationBase<OtherDerived, Dim>& r) {
  return r.toRotationMatrix().template cast<Scalar>();
}
 
template <typename Scalar, int Dim, typename OtherDerived,
          std::enable_if_t<std::is_same<Scalar, typename OtherDerived::Scalar>::value, int> = 0>
EIGEN_DEVICE_FUNC static inline const OtherDerived& toRotationMatrix(const MatrixBase<OtherDerived>& mat) {
  EIGEN_STATIC_ASSERT(OtherDerived::RowsAtCompileTime == Dim && OtherDerived::ColsAtCompileTime == Dim,
                      YOU_MADE_A_PROGRAMMING_MISTAKE)
  return mat.derived();
}
 
template <typename Scalar, int Dim, typename OtherDerived,
          std::enable_if_t<!std::is_same<Scalar, typename OtherDerived::Scalar>::value, int> = 0>
EIGEN_DEVICE_FUNC static inline auto toRotationMatrix(const MatrixBase<OtherDerived>& mat) {
  EIGEN_STATIC_ASSERT(OtherDerived::RowsAtCompileTime == Dim && OtherDerived::ColsAtCompileTime == Dim,
                      YOU_MADE_A_PROGRAMMING_MISTAKE)
  return mat.template cast<Scalar>();
}
 
}  // end namespace internal
 
}  // end namespace Eigen
 
#endif  // EIGEN_ROTATIONBASE_H