Merge pull request #19 from 4J-company/feature/quat

Add 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/color.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,100 @@
+#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) {}
+
+      // getters
+      [[nodiscard]] constexpr Vec3<T> vec() const noexcept { return _vec; }
+      [[nodiscard]] constexpr T x() const noexcept { return _vec.x(); }
+      [[nodiscard]] constexpr T y() const noexcept { return _vec.y(); }
+      [[nodiscard]] constexpr T z() const noexcept { return _vec.z(); }
+      [[nodiscard]] constexpr T w() const noexcept { return _angle._data; }
+
+      explicit constexpr operator Vec4<T>() const noexcept {
+        return Vec4<T>{_angle._data, _vec.x(), _vec.y(), _vec.z()};
+      }
+
+      // normalize methods
+      constexpr Quat & normalize() noexcept {
+        auto len = _vec.length2() + w() * w();
+        if (len <= mr::Vec3<T>::_epsilon) [[unlikely]] return *this;
+        _vec /= std::sqrt(len);
+        _angle /= std::sqrt(len);
+        return *this;
+      };
+
+      constexpr std::optional<Quat> normalized() const noexcept {
+        auto len = _vec.length2() + w() * w();
+        if (len <= mr::Vec3<T>::_epsilon) [[unlikely]] return std::nullopt;
+        auto res = *this;
+        res._vec /= sqrt(len);
+        res._angle /= sqrt(len);
+        return res;
+      };
+
+      friend constexpr Quat
+      operator+(const Quat &lhs, const Quat &rhs) noexcept {
+        return Quat{mr::Radiansf(lhs.w() + rhs.w()), lhs.vec() + rhs.vec()};
+      }
+
+      friend constexpr Quat
+      operator-(const Quat &lhs, const Quat &rhs) noexcept {
+        return Quat{mr::Radiansf(lhs.w() - rhs.w()), lhs.vec() - rhs.vec()};
+      }
+
+      friend constexpr Quat &
+      operator+=(Quat &lhs, const Quat &rhs) noexcept {
+        lhs = lhs + rhs;
+        return lhs;
+      }
+
+      friend constexpr Quat &
+      operator-=(Quat &lhs, const Quat &rhs) noexcept {
+        lhs = lhs - rhs;
+        return lhs;
+      }
+
+      friend constexpr Quat operator*(const Quat &lhs, const Quat &rhs) noexcept {
+        return {
+          mr::Radiansf(lhs.w() * rhs.w() - lhs.vec().dot(rhs.vec())),
+          lhs.w() * rhs.vec() + rhs.w() * lhs.vec() + lhs.vec() % rhs.vec()
+        };
+      }
+      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.w() / 2);
+          auto t = vq % lhs;
+          auto u = std::sin(rhs.w() / 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/units.hpp

@@ -49,7 +49,7 @@ namespace mr {
     public:
       using ValueT = T;
 
-      T _data;
+      T _data {};
 
       constexpr Radians() noexcept {};
       explicit constexpr Radians(T x) noexcept : _data(x) {};
@@ -79,7 +79,7 @@ namespace mr {
     public:
       using ValueT = T;
 
-      T _data;
+      T _data {};
 
       explicit constexpr Degrees(T x) noexcept : _data(x) {};
 

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

@@ -2,6 +2,7 @@
 
 #include "gtest/gtest.h"
 #include "mr-math/math.hpp"
+#include "mr-math/units.hpp"
 
 using namespace mr::literals;
 
@@ -235,6 +236,63 @@ TEST_F(MatrixTest, RotateVector) {
   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, DefaultConstructor) {
+  mr::Quat<float> q;
+  EXPECT_EQ((mr::Vec4f)q, mr::Vec4f());
+}
+
+TEST_F(QuaternionTest, ParameterizedConstructor) {
+    float w = 1.0, x = 2.0, y = 3.0, z = 4.0;
+    mr::Quat<float> p(mr::Radiansf(w), x, y, z);
+    EXPECT_EQ(p.w(), w);
+    EXPECT_EQ(p.vec(), mr::Vec3f(x, y, z));
+}
+
+TEST_F(QuaternionTest, Multiplication) {
+    mr::Quat<float> a(mr::Radiansf(1), 2, 3, 4);
+    mr::Quat<float> b(mr::Radiansf(5), 6, 7, 8);
+    mr::Quat<float> res = a * b;
+    mr::Quat<float> expected(mr::Radiansf(-60), 12, 30, 24);
+    EXPECT_TRUE(mr::equal(res.vec(), expected.vec()));
+    EXPECT_TRUE(mr::equal(res.w(), expected.w()));
+}
+
+TEST_F(QuaternionTest, Addition) {
+    mr::Quat<float> c(mr::Radiansf(1), 2, 3, 4);
+    mr::Quat<float> d(mr::Radiansf(5), 6, 7, 8);
+    mr::Quat<float> res = c + d;
+    mr::Quat<float> sum(mr::Radiansf(6), 8, 10, 12);
+    EXPECT_TRUE(mr::equal(res.vec(), sum.vec()));
+    EXPECT_TRUE(mr::equal(res.w(), sum.w()));
+}
+
+TEST_F(QuaternionTest, Subtraction) {
+    mr::Quat<float> e(mr::Radiansf(1), 2, 3, 4);
+    mr::Quat<float> f(mr::Radiansf(5), 6, 7, 8);
+    mr::Quat<float> diff(mr::Radiansf(-4), -4, -4, -4);
+    mr::Quat<float> res = e - f;
+    EXPECT_TRUE(mr::equal(res.vec(), diff.vec()));
+    EXPECT_TRUE(mr::equal(res.w(), diff.w()));
+}
+
+TEST_F(QuaternionTest, Normalize) {
+    mr::Quat<float> g(mr::Radiansf(3), 4, 0, 0);
+    g.normalize();
+    EXPECT_TRUE(mr::equal(g.w(), 0.6));
+    EXPECT_TRUE(mr::equal(g.vec(), mr::Vec3f(0.8, 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));
+}
+
 // TODO: camera tests
 
 TEST(ColorTest, Constructors) {