From e87151790976313f7b99f865ebb372228c0b98e0 Mon Sep 17 00:00:00 2001
From: lisyarus <lisyarus@gmail.com>
Date: Mon, 25 Jan 2021 00:59:13 +0300
Subject: [PATCH] Add quaternions & quaternion-based rotations

---
 libs/geom/include/psemek/geom/quaternion.hpp | 140 +++++++++++++++++++
 libs/geom/include/psemek/geom/rotation.hpp   | 110 +++++++++++++++
 2 files changed, 250 insertions(+)
 create mode 100644 libs/geom/include/psemek/geom/quaternion.hpp

diff --git a/libs/geom/include/psemek/geom/quaternion.hpp b/libs/geom/include/psemek/geom/quaternion.hpp
new file mode 100644
index 00000000..1fc0f5c5
--- /dev/null
+++ b/libs/geom/include/psemek/geom/quaternion.hpp
@@ -0,0 +1,140 @@
+#pragma once
+
+#include <psemek/geom/vector.hpp>
+
+namespace psemek::geom
+{
+
+	template <typename T>
+	struct quaternion
+	{
+		geom::vector<T, 4> coords;
+
+		quaternion() = default;
+
+		static quaternion<T> zero();
+		static quaternion<T> identity();
+		static quaternion<T> scalar(T value);
+		static quaternion<T> vector(geom::vector<T, 3> const & v);
+		static quaternion<T> rotation(T angle, geom::vector<T, 3> const & axis);
+
+		template <typename ... Args>
+		quaternion(Args && ... args)
+			: coords{ static_cast<T>(std::forward<Args>(args))... }
+		{
+			static_assert(sizeof...(Args) == 4);
+		}
+
+		T & operator[] (std::size_t i) { return coords[i]; }
+		T const & operator[] (std::size_t i) const { return coords[i]; }
+	};
+
+	template <typename ... Args>
+	quaternion(Args && ...) -> quaternion<std::common_type_t<Args...>>;
+
+	template <typename T>
+	quaternion<T> quaternion<T>::zero()
+	{
+		return quaternion<T>{T(0), T(0), T(0), T(0)};
+	}
+
+	template <typename T>
+	quaternion<T> quaternion<T>::identity()
+	{
+		return quaternion<T>{T(1), T(0), T(0), T(0)};
+	}
+
+	template <typename T>
+	quaternion<T> quaternion<T>::scalar(T value)
+	{
+		return {value, T(0), T(0), T(0)};
+	}
+
+	template <typename T>
+	quaternion<T> quaternion<T>::vector(geom::vector<T, 3> const & v)
+	{
+		return {T(0), v[0], v[1], v[2]};
+	}
+
+	template <typename T>
+	quaternion<T> quaternion<T>::rotation(T angle, geom::vector<T, 3> const & axis)
+	{
+		auto const c = std::cos(angle / 2);
+		auto const s = std::sin(angle / 2);
+
+		return quaternion<T>{c, s * axis[0], s * axis[1], s * axis[2]};
+	}
+
+	template <typename T>
+	quaternion<T> operator - (quaternion<T> const & q)
+	{
+		return {-q.coords};
+	}
+
+	template <typename T>
+	quaternion<T> operator + (quaternion<T> const & q1, quaternion<T> const & q2)
+	{
+		return {q1.coords + q2.coords};
+	}
+
+	template <typename T>
+	quaternion<T> operator - (quaternion<T> const & q1, quaternion<T> const & q2)
+	{
+		return {q1.coords - q2.coords};
+	}
+
+	template <typename T>
+	quaternion<T> operator * (quaternion<T> const & q, T s)
+	{
+		return {q.coords * s};
+	}
+
+	template <typename T>
+	quaternion<T> operator * (T s, quaternion<T> const & q)
+	{
+		return {s * q.coords};
+	}
+
+	template <typename T>
+	quaternion<T> operator * (quaternion<T> const & q1, quaternion<T> const & q2)
+	{
+		quaternion<T> r;
+		r[0] = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3];
+		r[1] = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2];
+		r[2] = q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1];
+		r[3] = q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0];
+		return r;
+	}
+
+	template <typename T>
+	T norm_sqr(quaternion<T> const & q)
+	{
+		return length_sqr(q.coords);
+	}
+
+	template <typename T>
+	T norm(quaternion<T> const & q)
+	{
+		return length(q.coords);
+	}
+
+	template <typename T>
+	quaternion<T> conjugate(quaternion<T> const & q)
+	{
+		return {q[0], -q[1], -q[2], -q[3]};
+	}
+
+	template <typename T>
+	quaternion<T> inverse(quaternion<T> const & q)
+	{
+		return conjugate(q) / norm(q);
+	}
+
+	template <typename T>
+	vector<T, 3> rotate(quaternion<T> const & q, vector<T, 3> const & v)
+	{
+		auto res = q * quaternion<T>::vector(v) * inverse(q);
+		return {res[1], res[2], res[3]};
+	}
+
+}
diff --git a/libs/geom/include/psemek/geom/rotation.hpp b/libs/geom/include/psemek/geom/rotation.hpp
index 01f24dd1..bb3eb3f3 100644
--- a/libs/geom/include/psemek/geom/rotation.hpp
+++ b/libs/geom/include/psemek/geom/rotation.hpp
@@ -1,6 +1,7 @@
 #pragma once
 
 #include <psemek/geom/affine_transform.hpp>
+#include <psemek/geom/quaternion.hpp>
 
 #include <psemek/util/assert.hpp>
 
@@ -59,6 +60,30 @@ namespace psemek::geom
 		void fill_matrix(Matrix & m) const;
 	};
 
+	template <typename T>
+	struct quaternion_rotation
+	{
+		quaternion<T> quat;
+
+		quaternion_rotation();
+		quaternion_rotation(quaternion<T> const & quat);
+		quaternion_rotation(T angle, vector<T, 3> const & axis);
+
+		matrix<T, 3, 4> affine_matrix() const;
+		matrix<T, 3, 3> linear_matrix() const;
+		vector<T, 3> translation_vector() const;
+		matrix<T, 4, 4> homogeneous_matrix() const;
+
+		affine_transform<T, 3, 3> transform() const;
+
+		vector<T, 3> operator()(vector<T, 3> const & v) const;
+		point<T, 3> operator()(point<T, 3> const & p) const;
+
+	private:
+		template <typename Matrix>
+		void fill_matrix(Matrix & m) const;
+	};
+
 	template <typename T>
 	axis_rotation(vector<T, 3>, T) -> axis_rotation<T>;
 
@@ -239,4 +264,89 @@ namespace psemek::geom
 		return {r.axis, -r.angle};
 	}
 
+	template <typename T>
+	quaternion_rotation<T>::quaternion_rotation()
+		: quat{quaternion<T>::identity()}
+	{}
+
+	template <typename T>
+	quaternion_rotation<T>::quaternion_rotation(quaternion<T> const & quat)
+		: quat{quat}
+	{}
+
+	template <typename T>
+	quaternion_rotation<T>::quaternion_rotation(T angle, vector<T, 3> const & axis)
+		: quat{quaternion<T>::rotation(angle, axis)}
+	{}
+
+	template <typename T>
+	matrix<T, 3, 4> quaternion_rotation<T>::affine_matrix() const
+	{
+		auto result = matrix<T, 3, 4>::identity();
+		fill_matrix(result);
+		return result;
+	}
+
+	template <typename T>
+	matrix<T, 3, 3> quaternion_rotation<T>::linear_matrix() const
+	{
+		auto result = matrix<T, 3, 3>::identity();
+		fill_matrix(result);
+		return result;
+	}
+
+	template <typename T>
+	vector<T, 3> quaternion_rotation<T>::translation_vector() const
+	{
+		return vector<T, 3>::zero();
+	}
+
+	template <typename T>
+	matrix<T, 4, 4> quaternion_rotation<T>::homogeneous_matrix() const
+	{
+		auto result = matrix<T, 4, 4>::identity();
+		fill_matrix(result);
+		return result;
+	}
+
+	template <typename T>
+	affine_transform<T, 3, 3> quaternion_rotation<T>::transform() const
+	{
+		return {affine_matrix()};
+	}
+
+	template <typename T>
+	vector<T, 3> quaternion_rotation<T>::operator()(vector<T, 3> const & v) const
+	{
+		return linear_matrix() * v;
+	}
+
+	template <typename T>
+	point<T, 3> quaternion_rotation<T>::operator()(point<T, 3> const & p) const
+	{
+		static constexpr auto o = point<T, 3>::zero();
+		return o + linear_matrix() * (p - o);
+	}
+
+	template <typename T>
+	template <typename Matrix>
+	void quaternion_rotation<T>::fill_matrix(Matrix & m) const
+	{
+		m[0][0] = 1 - 2 * (quat[2] * quat[2] + quat[3] * quat[3]);
+		m[0][1] = 2 * (quat[1] * quat[2] - quat[0] * quat[3]);
+		m[0][2] = 2 * (quat[1] * quat[3] + quat[0] * quat[2]);
+		m[1][0] = 2 * (quat[1] * quat[2] + quat[0] * quat[3]);
+		m[1][1] = 1 - 2 * (quat[1] * quat[1] + quat[3] * quat[3]);
+		m[1][2] = 2 * (quat[2] * quat[3] - quat[0] * quat[1]);
+		m[2][0] = 2 * (quat[1] * quat[3] - quat[0] * quat[2]);
+		m[2][1] = 2 * (quat[2] * quat[3] + quat[0] * quat[1]);
+		m[2][2] = 1 - 2 * (quat[1] * quat[1] + quat[2] * quat[2]);
+	}
+
+	template <typename T>
+	quaternion_rotation<T> inverse(quaternion_rotation<T> const & r)
+	{
+		return quaternion_rotation<T>{quaternion<T>{r.quat[0], -r.quat[1], -r.quat[2], -r.quat[3]}};
+	}
+
 }