mr::Quat implementation

CMakeLists.txt

@@ -32,6 +32,7 @@ add_library(${MR_MATH_LIB_NAME} INTERFACE
   include/mr-math/row.hpp
   include/mr-math/units.hpp
   include/mr-math/vec.hpp
+  include/mr-math/quat.hpp
   include/mr-math/math.hpp
   include/mr-math/bound_box.hpp
 )

include/mr-math/math.hpp

@@ -6,6 +6,7 @@
 #include "rot.hpp"
 #include "norm.hpp"
 #include "matr.hpp"
+#include "quat.hpp"
 #include "units.hpp"
 #include "camera.hpp"
 #include "bound_box.hpp"

include/mr-math/quat.hpp

@@ -0,0 +1,75 @@
+#ifndef __quat_hpp_
+#define __quat_hpp_
+
+#include "def.hpp"
+#include "mr-math/operators.hpp"
+#include "vec.hpp"
+#include "matr.hpp"
+#include "rot.hpp"
+
+namespace mr {
+  template <ArithmeticT T>
+    struct Quat {
+    private:
+      Radians<T> _angle {};
+      Vec3<T> _vec {};
+
+    public:
+      constexpr Quat() noexcept = default;
+      constexpr Quat(Vec4<T> v) noexcept : _angle(v.x()), _vec(v.y(), v.z(), v.w()) {}
+      constexpr Quat(Radians<T> a, Vec3<T> v) noexcept : _angle(a), _vec(v) {}
+      constexpr Quat(Radians<T> a, T x, T y, T z) noexcept : _angle(a), _vec(x, y, z) {}
+
+      constexpr operator Vec4<T>() const noexcept {
+        return _vec;
+      }
+
+      friend constexpr Quat
+      operator+(const Quat &lhs, const Quat &rhs) noexcept {
+        return Quat{lhs._vec + rhs._vec};
+      }
+
+      friend constexpr Quat
+      operator-(const Quat &lhs, const Quat &rhs) noexcept {
+        return Quat{lhs._vec - rhs._vec};
+      }
+
+      friend constexpr Quat &
+      operator+=(Quat &lhs, const Quat &rhs) noexcept {
+        lhs._vec += rhs._vec;
+        return lhs;
+      }
+
+      friend constexpr Quat &
+      operator-=(Quat &lhs, const Quat &rhs) noexcept {
+        lhs._vec -= rhs._vec;
+        return lhs;
+      }
+
+      friend constexpr Quat operator*(const Quat &lhs, const Quat &rhs) noexcept {
+        return {
+          2 * lhs[0] * rhs[0] - lhs & rhs,
+          lhs._angle * rhs + rhs._angle * lhs + lhs % rhs
+        };
+      }
+      friend constexpr Quat & operator*=(Quat &lhs, const Quat &rhs) noexcept {
+        lhs = lhs * rhs;
+        return lhs;
+      }
+
+        friend constexpr Vec<T, 3> operator*(const Vec<T, 3> &lhs, const Quat &rhs) noexcept {
+          auto vq = rhs._vec * std::cos(rhs._angle._data / 2);
+          auto t = vq % lhs;
+          auto u = std::sin(rhs._angle._data / 2) * t + vq % t;
+
+          return {lhs + u + u};
+        }
+      template <std::size_t N>
+        friend constexpr Vec<T, N> & operator*=(Vec<T, N> &lhs, const Quat &rhs) noexcept {
+          lhs = lhs * rhs;
+          return lhs;
+        }
+    };
+}
+
+#endif // __quat_hpp_

include/mr-math/row.hpp

@@ -4,8 +4,13 @@
 #include "operators.hpp"
 
 namespace mr {
+  template <ArithmeticT T>
+    struct Quat;
+
   template <ArithmeticT T, std::size_t N>
     struct Row : RowOperators<Row<T, N>> {
+      friend struct Quat<T>;
+
     public:
       using ValueT = T;
       using SimdT = SimdImpl<T, N>;

include/mr-math/vec.hpp

@@ -12,6 +12,8 @@ namespace mr {
     struct Norm;
   template <ArithmeticT T, std::size_t N>
     struct Matr;
+  template <ArithmeticT T>
+    struct Quat;
 
   // common aliases
   template <ArithmeticT T>
@@ -39,8 +41,9 @@ namespace mr {
 
   // base vector (use aliases for full functional)
   template <ArithmeticT T, std::size_t N> requires (N >= 2)
-    struct [[nodiscard]] Vec : public RowOperators<Vec<T, N>>
-    {
+    struct [[nodiscard]] Vec : public RowOperators<Vec<T, N>> {
+      friend struct Quat<T>;
+
     public:
       using ValueT = T;
       using RowT = Row<T, N>;

tests/main.cpp

@@ -213,3 +213,15 @@ TEST_F(MatrixTest, RotateVector) {
   mr::Vec3f expected{38.340427, 81.678845, 36.980571};
   EXPECT_TRUE(mr::equal(v * mr::Matr4f::rotate({1, 1, 1}, 102_deg), expected, 0.0001));
 }
+
+class QuaternionTest : public ::testing::Test {
+protected:
+  mr::Quat<float> q1 {mr::Degreesf(90), 1, 0, 0};
+};
+
+TEST_F(QuaternionTest, RotateMatrix) {
+  mr::Vec3f v {0, 1, 0};
+  mr::Vec3f expected {0, 0, 1};
+  EXPECT_TRUE(mr::equal(v * q1, expected));
+}
+