Implements rotgen::quaternion

Closes #19

Co-authored-by: Jules Pénuchot <jules@penuchot.com>

include/rotgen/algebra/qr.hpp

@@ -7,6 +7,8 @@
 //==================================================================================================
 #pragma once
 
+#include <rotgen/container/ref.hpp>
+
 namespace rotgen::solver
 {
   template<typename X, typename M, typename RHS>

include/rotgen/algebra/svd/fixed.hpp

@@ -7,6 +7,11 @@
 //==================================================================================================
 #pragma once
 
+#include <rotgen/detail/helpers.hpp>
+
+#include <rotgen/config.hpp>
+#include <rotgen/container/matrix.hpp>
+
 #include <Eigen/Dense>
 #include <Eigen/SVD>
 

include/rotgen/alias.hpp

@@ -5,6 +5,11 @@
   SPDX-License-Identifier: BSL-1.0
 */
 //==================================================================================================
+
+#pragma once
+
+#include <rotgen/container/matrix.hpp>
+
 namespace rotgen
 {
   // matrix aliases (float and double, fixed and dynamic)

include/rotgen/container.hpp

@@ -7,7 +7,8 @@
 //==================================================================================================
 #pragma once
 
-#include <rotgen/container/matrix.hpp>
 #include <rotgen/container/block.hpp>
 #include <rotgen/container/map.hpp>
+#include <rotgen/container/matrix.hpp>
+#include <rotgen/container/quaternion.hpp>
 #include <rotgen/container/ref.hpp>

include/rotgen/container/block/fixed.hpp

@@ -7,10 +7,12 @@
 //==================================================================================================
 #pragma once
 
+#include <rotgen/detail/helpers.hpp>
+
+#include <rotgen/container/matrix.hpp>
 #include <rotgen/container/strides.hpp>
 
 #include <Eigen/Dense>
-#include <iostream>
 
 namespace rotgen
 {

include/rotgen/container/map/dynamic.hpp

@@ -11,6 +11,7 @@
 
 #include <rotgen/concepts.hpp>
 #include <rotgen/container/map/dynamic/impl.hpp>
+#include <rotgen/container/matrix.hpp>
 
 #include <cassert>
 

include/rotgen/container/matrix/dynamic.hpp

@@ -8,7 +8,6 @@
 #pragma once
 
 #include <rotgen/detail/helpers.hpp>
-
 #include <rotgen/concepts.hpp>
 #include <rotgen/container/matrix/dynamic/impl.hpp>
 
@@ -445,11 +444,7 @@ namespace rotgen
 
     parent& base() { return static_cast<parent&>(*this); }
 
-    parent const& base() const
-    {
-      return static_cast<parent const&>(*this);
-      ;
-    }
+    parent const& base() const { return static_cast<parent const&>(*this); }
   };
 
   template<typename S, int R, int C, int O, int MR, int MC>

include/rotgen/container/matrix/fixed.hpp

@@ -9,8 +9,9 @@
 
 #include <rotgen/detail/helpers.hpp>
 
+#include <rotgen/concepts.hpp>
+
 #include <Eigen/Dense>
-#include <iostream>
 
 namespace rotgen
 {

include/rotgen/container/quaternion.hpp

@@ -0,0 +1,19 @@
+//==============================================================================
+/*
+  ROTGEN - Runtime Overlay for Eigen
+  Copyright : CODE RECKONS
+  SPDX-License-Identifier: BSL-1.0
+*/
+//==============================================================================
+
+#pragma once
+
+// https://libeigen.gitlab.io/eigen/docs-3.4/classEigen_1_1Quaternion.html
+
+#include <rotgen/config.hpp>
+
+#if defined(ROTGEN_FORCE_DYNAMIC)
+#include <rotgen/container/quaternion/dynamic.hpp>
+#else
+#include <rotgen/container/quaternion/fixed.hpp>
+#endif

include/rotgen/container/quaternion/dynamic.hpp

@@ -0,0 +1,84 @@
+//==============================================================================
+/*
+  ROTGEN - Runtime Overlay for Eigen
+  Copyright : CODE RECKONS
+  SPDX-License-Identifier: BSL-1.0
+*/
+//==============================================================================
+
+#pragma once
+
+#include <rotgen/detail/helpers.hpp>
+
+#include <rotgen/config.hpp>
+#include <rotgen/container/matrix.hpp>
+#include <rotgen/container/quaternion/dynamic/impl.hpp>
+#include <rotgen/container/ref.hpp>
+
+namespace rotgen
+{
+  template<typename Scalar, int Options>
+  class quaternion : public find_quaternion<Scalar>
+  {
+  public:
+    using parent = find_quaternion<Scalar>;
+
+    using CoeffReturnType = Scalar;
+    using NonConstCoeffReturnType = Scalar&;
+
+    // Constructors
+
+    quaternion() : parent() {}
+
+    quaternion(parent const& other) : parent(other) {}
+
+    quaternion(parent&& other) : parent(other) {}
+
+    quaternion(Scalar w, Scalar x, Scalar y, Scalar z) : parent(w, x, y, z) {}
+
+    // Coefficient accessors
+
+    CoeffReturnType w() const { return parent::w(); }
+
+    NonConstCoeffReturnType w() { return parent::w(); }
+
+    CoeffReturnType x() const { return parent::x(); }
+
+    NonConstCoeffReturnType x() { return parent::x(); }
+
+    CoeffReturnType y() const { return parent::y(); }
+
+    NonConstCoeffReturnType y() { return parent::y(); }
+
+    CoeffReturnType z() const { return parent::z(); }
+
+    NonConstCoeffReturnType z() { return parent::z(); }
+
+    parent& base() { return static_cast<parent&>(*this); }
+
+    parent const& base() const { return static_cast<parent const&>(*this); }
+
+    /// Multiplication method, implemented not as operator* to bypass common
+    /// operator* implementation and avoid Eigen's quirky behavior when
+    /// multiplying quaternions and matrices.
+    template<typename T> auto mul(ref<matrix<T, 3, 1> const> mat) const
+    {
+      return parent::mul(mat.base());
+    }
+  };
+
+  /// Wraps `Eigen::AngleAxis`
+  template<typename Scalar>
+  quaternion<Scalar> angleAxis(Scalar theta,
+                               rotgen::matrix<Scalar, 3, 1> const& axis)
+  {
+    return quaternion<Scalar>::angleAxis(theta, axis);
+  }
+
+  template<typename Scalar>
+  auto operator*(quaternion<Scalar> const& q,
+                 ref<matrix<Scalar, 3, 1> const> const& mat)
+  {
+    return q.mul(mat);
+  }
+}

include/rotgen/container/quaternion/dynamic/impl.hpp

@@ -0,0 +1,163 @@
+//==============================================================================
+/*
+  ROTGEN - Runtime Overlay for Eigen
+  Copyright : CODE RECKONS
+  SPDX-License-Identifier: BSL-1.0
+*/
+//==============================================================================
+
+#pragma once
+
+#include <rotgen/detail/generators.hpp>
+
+#include <rotgen/config.hpp>
+#include <rotgen/container/map.hpp>
+#include <rotgen/container/matrix.hpp>
+
+namespace rotgen
+{
+  // Only example to support:
+
+  // ConstColVectorRef<3>& T_;
+  // Eigen::Quaternion<Real> Q;
+  // Q = Eigen::AngleAxis<Real>(theta_, N_);
+  // T_ = Q * T_;
+
+  /// Hard-wired implementation of quaternion for 32-bit floats
+  class ROTGEN_EXPORT quaternion_impl_32
+  {
+  private:
+    struct payload;
+    std::unique_ptr<payload> storage_;
+
+  public:
+    // Construction, destruction, assignment
+
+    quaternion_impl_32();
+    quaternion_impl_32(quaternion_impl_32 const&);
+    quaternion_impl_32(quaternion_impl_32&&) noexcept;
+    quaternion_impl_32& operator=(quaternion_impl_32 const&);
+    quaternion_impl_32& operator=(quaternion_impl_32&&) noexcept;
+    ~quaternion_impl_32();
+
+    quaternion_impl_32(float w, float x, float y, float z);
+
+    // Comparison
+
+    friend ROTGEN_EXPORT bool operator==(quaternion_impl_32 const&,
+                                         quaternion_impl_32 const&);
+    friend ROTGEN_EXPORT bool operator!=(quaternion_impl_32 const&,
+                                         quaternion_impl_32 const&);
+
+    // Data and payload access
+
+    std::unique_ptr<payload>& storage() { return storage_; }
+
+    std::unique_ptr<payload> const& storage() const { return storage_; }
+
+    float const* data() const;
+    float* data();
+
+    // Coefficient accessors
+
+    float w() const;
+    float& w();
+    float x() const;
+    float& x();
+    float y() const;
+    float& y();
+    float z() const;
+    float& z();
+
+    // Arithmetic operators
+
+    /// Multiplication method, implemented not as operator* to bypass common
+    /// operator* implementation and avoid Eigen's quirky behavior when
+    /// multiplying quaternions and matrices.
+    matrix<float, 3, 1> mul(map<matrix<float, 3, 1> const>) const;
+
+    // Generators
+
+    static quaternion_impl_32 angleAxis(float theta,
+                                        matrix<float, 3, 1> const& axis);
+  };
+
+  /// Hard-wired implementation of quaternion for 64-bit floats
+  class ROTGEN_EXPORT quaternion_impl_64
+  {
+  private:
+    struct payload;
+    std::unique_ptr<payload> storage_;
+
+  public:
+    // Construction, destruction, assignment
+
+    quaternion_impl_64();
+    quaternion_impl_64(quaternion_impl_64 const&);
+    quaternion_impl_64(quaternion_impl_64&&) noexcept;
+    quaternion_impl_64& operator=(quaternion_impl_64 const&);
+    quaternion_impl_64& operator=(quaternion_impl_64&&) noexcept;
+    ~quaternion_impl_64();
+
+    quaternion_impl_64(double w, double x, double y, double z);
+
+    // Comparison
+
+    friend ROTGEN_EXPORT bool operator==(quaternion_impl_64 const&,
+                                         quaternion_impl_64 const&);
+    friend ROTGEN_EXPORT bool operator!=(quaternion_impl_64 const&,
+                                         quaternion_impl_64 const&);
+
+    // Data and payload access
+
+    std::unique_ptr<payload>& storage() { return storage_; }
+
+    std::unique_ptr<payload> const& storage() const { return storage_; }
+
+    double const* data() const;
+    double* data();
+
+    // Coefficient accessors
+
+    double w() const;
+    double& w();
+    double x() const;
+    double& x();
+    double y() const;
+    double& y();
+    double z() const;
+    double& z();
+
+    // Arithmetic operators
+
+    /// Multiplication method, implemented not as operator* to bypass common
+    /// operator* implementation and avoid Eigen's quirky behavior when
+    /// multiplying quaternions and matrices.
+    matrix<double, 3, 1> mul(map<matrix<double, 3, 1> const>) const;
+
+    // Generators
+
+    static quaternion_impl_64 angleAxis(double theta,
+                                        matrix<double, 3, 1> const& axis);
+  };
+
+  // find_quaternion
+
+  template<typename Scalar> struct _find_quaternion_impl;
+
+  template<> struct _find_quaternion_impl<float>
+  {
+    using type = quaternion_impl_32;
+  };
+
+  template<> struct _find_quaternion_impl<double>
+  {
+    using type = quaternion_impl_64;
+  };
+
+  /// Resolves to the right type of quaternion depending on the input type
+  /// (float or double).
+  template<typename Scalar>
+  using find_quaternion = typename _find_quaternion_impl<Scalar>::type;
+
+}

include/rotgen/container/quaternion/fixed.hpp

@@ -0,0 +1,98 @@
+//==============================================================================
+/*
+  ROTGEN - Runtime Overlay for Eigen
+  Copyright : CODE RECKONS
+  SPDX-License-Identifier: BSL-1.0
+*/
+//==============================================================================
+
+#pragma once
+
+#include <rotgen/detail/helpers.hpp>
+
+#include <rotgen/concepts.hpp>
+#include <rotgen/config.hpp>
+#include <rotgen/container/matrix.hpp>
+#include <rotgen/container/ref.hpp>
+
+#include <Eigen/Dense>
+
+namespace rotgen
+{
+  template<typename Scalar, int Options>
+  class quaternion : private Eigen::Quaternion<Scalar, Options>
+  {
+  public:
+    using rotgen_tag = void;
+    using parent = Eigen::Quaternion<Scalar, Options>;
+    using value_type = Scalar;
+
+    quaternion() : parent() {}
+
+    quaternion(Scalar w_, Scalar x_, Scalar y_, Scalar z_)
+      : parent(w_, x_, y_, z_)
+    {
+    }
+
+    quaternion(concepts::entity auto const& other) : parent(other.base()) {}
+
+    template<typename OtherDerived>
+    quaternion(Eigen::QuaternionBase<OtherDerived> const& other) : parent(other)
+    {
+    }
+
+    template<typename OtherDerived>
+    quaternion(Eigen::EigenBase<OtherDerived> const& other) : parent(other)
+    {
+    }
+
+    template<typename OtherDerived>
+    quaternion& operator=(Eigen::QuaternionBase<OtherDerived> const& other)
+    {
+      parent::operator=(other);
+      return *this;
+    }
+
+    template<typename OtherDerived>
+    quaternion& operator=(Eigen::EigenBase<OtherDerived> const& other)
+    {
+      parent::operator=(other);
+      return *this;
+    }
+
+    quaternion& operator=(concepts::entity auto const& other)
+    {
+      parent::operator=(other.base());
+      return *this;
+    }
+
+    quaternion(quaternion const& other) = default;
+    quaternion(quaternion&& other) = default;
+    quaternion& operator=(quaternion const&) = default;
+    quaternion& operator=(quaternion&&) = default;
+
+    parent& base() { return static_cast<parent&>(*this); }
+
+    parent const& base() const { return static_cast<parent const&>(*this); }
+
+    using parent::w;
+    using parent::x;
+    using parent::y;
+    using parent::z;
+  };
+
+  template<typename Scalar>
+  auto operator*(quaternion<Scalar> const& q,
+                 ref<matrix<Scalar, 3, 1> const> const& mat)
+  {
+    return matrix<Scalar, 3, 1>{q.base() * mat.base()};
+  }
+
+  template<typename Scalar>
+  quaternion<Scalar> angleAxis(Scalar theta, rotgen::concepts::entity auto mat)
+  {
+    Eigen::Quaternion<Scalar> result;
+    result = Eigen::AngleAxis<Scalar>(theta, mat.base());
+    return {result};
+  }
+}

include/rotgen/container/ref.hpp

@@ -8,6 +8,13 @@
 #pragma once
 
 #include <rotgen/config.hpp>
+#include <rotgen/container/block.hpp>
+#include <rotgen/container/map.hpp>
+#include <rotgen/format.hpp>
+
+#include <cassert>
+#include <iostream>
+#include <type_traits>
 
 #if defined(ROTGEN_FORCE_DYNAMIC)
 #include <rotgen/container/ref/dynamic.hpp>

include/rotgen/container/ref/generalize.hpp

@@ -7,10 +7,14 @@
 //==================================================================================================
 #pragma once
 
+#include <rotgen/config.hpp>
+
 #include <type_traits>
 
 namespace rotgen
 {
+  template<typename Scalar, int Options = AutoAlign> class quaternion;
+
   //-------------------------------------------------------------------------------------------
   // Convert entity/eigen types to a proper ref so we can write less function
   // overloads
@@ -46,6 +50,16 @@ namespace rotgen
     using type = ref<T, O, S>;
   };
 
+  template<typename T> struct generalize<quaternion<T>>
+  {
+    using type = quaternion<T>;
+  };
+
+  template<typename T> struct generalize<quaternion<T> const>
+  {
+    using type = quaternion<T>;
+  };
+
   template<concepts::entity T> typename T::parent& base_of(T& a)
   {
     return a.base();

include/rotgen/container/strides.hpp

@@ -7,6 +7,7 @@
 //==================================================================================================
 #pragma once
 
+#include <rotgen/concepts.hpp>
 #include <rotgen/config.hpp>
 
 #if defined(ROTGEN_FORCE_DYNAMIC)

include/rotgen/detail/payload.hpp

@@ -1,19 +1,49 @@
-//==================================================================================================
+//==============================================================================
 /*
   ROTGEN - Runtime Overlay for Eigen
   Copyright : CODE RECKONS
   SPDX-License-Identifier: BSL-1.0
 */
-//==================================================================================================
+//==============================================================================
 #pragma once
 
 #include <rotgen/detail/payload/block.hpp>
 #include <rotgen/detail/payload/map.hpp>
 #include <rotgen/detail/payload/matrix.hpp>
 
+#include <rotgen/container/quaternion/dynamic/impl.hpp>
+
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include <cstddef>
+
 namespace rotgen
 {
-  // IO Formatting paylod
+  struct quaternion_impl_32::payload
+  {
+    using data_type = Eigen::Quaternion<float>;
+    data_type data;
+
+    payload() : data() {}
+
+    payload(data_type const& o) : data(o) {}
+
+    payload(float* ptr) : data(ptr) {}
+  };
+
+  struct quaternion_impl_64::payload
+  {
+    using data_type = Eigen::Quaternion<double>;
+    data_type data;
+
+    payload() : data() {}
+
+    payload(data_type const& o) : data(o) {}
+
+    payload(double* ptr) : data(ptr) {}
+  };
+
+  // IO Payload
   struct ioformat::payload
   {
     Eigen::IOFormat instance;

include/rotgen/rotgen.hpp

@@ -7,11 +7,9 @@
 //==================================================================================================
 #pragma once
 
-//clang-format off
-#include <rotgen/config.hpp>
-#include <rotgen/format.hpp>
-#include <rotgen/container.hpp>
-#include <rotgen/alias.hpp>
 #include <rotgen/algebra.hpp>
+#include <rotgen/alias.hpp>
+#include <rotgen/config.hpp>
+#include <rotgen/container.hpp>
+#include <rotgen/format.hpp>
 #include <rotgen/functions.hpp>
-//clang-format on