Line data    Source code

       1             : // SPDX-License-Identifier: BSD-3-Clause
       2             : // Copyright (c) 2023 Scipp contributors (https://github.com/scipp)
       3             : /// @file
       4             : /// @author Simon Heybrock
       5             : #pragma once
       6             : #include <Eigen/Core>
       7             : #include <Eigen/Geometry>
       8             : 
       9             : #include "scipp/core/dtype.h"
      10             : 
      11             : #include <optional>
      12             : #include <type_traits>
      13             : #include <utility>
      14             : 
      15             : namespace scipp::core {
      16             : 
      17             : class Quaternion {
      18             : private:
      19             :   // Store as quaterniond as this is more space efficient than storing as matrix
      20             :   // (4 doubles for quat vs 9 doubles for 3x3 matrix).
      21             :   Eigen::Quaterniond m_quat;
      22             : 
      23             : public:
      24             :   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
      25     1199368 :   Quaternion() : m_quat(Eigen::Quaterniond::Identity()){};
      26           7 :   explicit Quaternion(const Eigen::Quaterniond &x) : m_quat(x){};
      27             : 
      28          19 :   [[nodiscard]] const Eigen::Quaterniond &quat() const { return m_quat; }
      29             : 
      30           0 :   bool operator==(const Quaternion &other) const {
      31           0 :     return m_quat.w() == other.m_quat.w() && m_quat.x() == other.m_quat.x() &&
      32           0 :            m_quat.y() == other.m_quat.y() && m_quat.z() == other.m_quat.z();
      33             :   }
      34             : 
      35           0 :   bool operator!=(const Quaternion &other) const { return !operator==(other); }
      36             : 
      37           9 :   double &operator()(const int i) {
      38           9 :     if (i == 0) {
      39           9 :       return m_quat.x();
      40           0 :     } else if (i == 1) {
      41           0 :       return m_quat.y();
      42           0 :     } else if (i == 2) {
      43           0 :       return m_quat.z();
      44           0 :     } else if (i == 3) {
      45           0 :       return m_quat.w();
      46             :     } else {
      47           0 :       throw std::out_of_range("invalid index for Quaternion");
      48             :     }
      49             :   }
      50             : 
      51           3 :   [[nodiscard]] Quaternion inverse() const {
      52             :     // We do not guarantee that quaternions are normalized.
      53             :     // So use inverse() here instead of conjugate().
      54           6 :     return Quaternion(m_quat.inverse());
      55             :   }
      56             : };
      57             : 
      58             : class Translation {
      59             : private:
      60             :   Eigen::Vector3d m_vec;
      61             : 
      62             : public:
      63             :   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
      64     1077696 :   Translation() : m_vec(Eigen::Vector3d(0, 0, 0)){};
      65             :   // https://eigen.tuxfamily.org/dox/group__TopicPassingByValue.html
      66             :   // NOLINTNEXTLINE
      67           8 :   explicit Translation(const Eigen::Vector3d &x) : m_vec(x){};
      68             : 
      69          16 :   [[nodiscard]] const Eigen::Vector3d &vector() const { return m_vec; }
      70             : 
      71           0 :   bool operator==(const Translation &other) const {
      72           0 :     return m_vec == other.m_vec;
      73             :   }
      74             : 
      75           0 :   bool operator!=(const Translation &other) const {
      76           0 :     return m_vec != other.m_vec;
      77             :   }
      78             : 
      79          10 :   double &operator()(const int i) { return m_vec(i); }
      80             : 
      81           3 :   [[nodiscard]] Translation inverse() const { return Translation(-m_vec); }
      82             : };
      83             : 
      84           0 : template <typename T> inline const T &asEigenType(const T &obj) { return obj; }
      85          11 : inline const auto &asEigenType(const Quaternion &obj) { return obj.quat(); }
      86           7 : inline auto asEigenType(const Translation &obj) {
      87           7 :   return Eigen::Translation<double, 3>(obj.vector());
      88             : }
      89             : 
      90             : template <class T_LHS, class T_RHS>
      91             : struct combines_to_linear : std::false_type {};
      92             : template <>
      93             : struct combines_to_linear<Quaternion, Eigen::Matrix3d> : std::true_type {};
      94             : template <>
      95             : struct combines_to_linear<Eigen::Matrix3d, Quaternion> : std::true_type {};
      96             : 
      97             : template <typename T_LHS, typename T_RHS>
      98             : [[nodiscard]] inline std::enable_if_t<combines_to_linear<T_LHS, T_RHS>::value,
      99             :                                       Eigen::Matrix3d>
     100           0 : operator*(const T_LHS &lhs, const T_RHS &rhs) {
     101           0 :   return asEigenType(lhs) * asEigenType(rhs);
     102             : }
     103             : 
     104             : template <class T_LHS, class T_RHS>
     105             : struct combines_to_affine : std::false_type {};
     106             : template <>
     107             : struct combines_to_affine<Quaternion, Translation> : std::true_type {};
     108             : template <>
     109             : struct combines_to_affine<Quaternion, Eigen::Affine3d> : std::true_type {};
     110             : template <>
     111             : struct combines_to_affine<Eigen::Matrix3d, Translation> : std::true_type {};
     112             : template <>
     113             : struct combines_to_affine<Eigen::Affine3d, Quaternion> : std::true_type {};
     114             : template <>
     115             : struct combines_to_affine<Eigen::Affine3d, Translation> : std::true_type {};
     116             : template <>
     117             : struct combines_to_affine<Translation, Quaternion> : std::true_type {};
     118             : template <>
     119             : struct combines_to_affine<Translation, Eigen::Matrix3d> : std::true_type {};
     120             : template <>
     121             : struct combines_to_affine<Translation, Eigen::Affine3d> : std::true_type {};
     122             : 
     123             : template <typename T_LHS, typename T_RHS>
     124             : [[nodiscard]] inline std::enable_if_t<combines_to_affine<T_LHS, T_RHS>::value,
     125             :                                       Eigen::Affine3d>
     126           1 : operator*(const T_LHS &lhs, const T_RHS &rhs) {
     127           1 :   return Eigen::Affine3d(asEigenType(lhs) * asEigenType(rhs));
     128             : }
     129             : 
     130          10 : [[nodiscard]] inline Eigen::Vector3d operator*(const Quaternion &lhs,
     131             :                                                const Eigen::Vector3d &rhs) {
     132          10 :   return asEigenType(lhs) * rhs;
     133             : }
     134             : 
     135           6 : [[nodiscard]] inline Eigen::Vector3d operator*(const Translation &lhs,
     136             :                                                const Eigen::Vector3d &rhs) {
     137          12 :   return asEigenType(lhs) * rhs;
     138             : }
     139             : 
     140           4 : [[nodiscard]] inline Quaternion operator*(const Quaternion &lhs,
     141             :                                           const Quaternion &rhs) {
     142           8 :   return Quaternion(lhs.quat() * rhs.quat());
     143             : }
     144             : 
     145           4 : [[nodiscard]] inline Translation operator*(const Translation &lhs,
     146             :                                            const Translation &rhs) {
     147           4 :   return Translation(lhs.vector() + rhs.vector());
     148             : }
     149             : 
     150             : template <> inline constexpr DType dtype<Eigen::Matrix3d>{4001};
     151             : template <> inline constexpr DType dtype<Eigen::Affine3d>{4002};
     152             : template <> inline constexpr DType dtype<Translation>{4003};
     153             : template <> inline constexpr DType dtype<Quaternion>{4004};
     154             : 
     155             : } // namespace scipp::core