From 4bb8336b2012f5f5325a03d1aab04d29c44864a4 Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Mon, 31 Oct 2022 09:28:54 +0200
Subject: [PATCH] Add a Quaternion class

---
 source/geometry/affinetransform.h |  13 ++
 source/geometry/quaternion.h      | 194 ++++++++++++++++++++++++++++++
 2 files changed, 207 insertions(+)
 create mode 100644 source/geometry/quaternion.h

diff --git a/source/geometry/affinetransform.h b/source/geometry/affinetransform.h
index 1d48a5d..f9660c6 100644
--- a/source/geometry/affinetransform.h
+++ b/source/geometry/affinetransform.h
@@ -4,6 +4,7 @@
 #include <msp/linal/matrix.h>
 #include "angle.h"
 #include "boundingbox.h"
+#include "quaternion.h"
 #include "ray.h"
 
 namespace Msp {
@@ -41,6 +42,7 @@ protected:
 
 public:
 	static AffineTransform<T, 3> rotation(Angle<T>, const LinAl::Vector<T, 3> &);
+	static AffineTransform<T, 3> rotation(const Quaternion<T> &);
 };
 
 
@@ -144,6 +146,17 @@ AffineTransform<T, 3> AffineTransformOps<T, 3>::rotation(Angle<T> angle, const L
 	return r;
 }
 
+template<typename T>
+AffineTransform<T, 3> AffineTransformOps<T, 3>::rotation(const Quaternion<T> &quat)
+{
+	AffineTransform<T, 3> r;
+	LinAl::Matrix<T, 3, 3> m = quat.to_matrix();
+	for(unsigned i=0; i<3; ++i)
+		for(unsigned j=0; j<3; ++j)
+			r.matrix(i, j) = m(i, j);
+	return r;
+}
+
 template<typename T, unsigned D>
 inline AffineTransform<T, D> &AffineTransform<T, D>::operator*=(const AffineTransform<T, D> &other)
 {
diff --git a/source/geometry/quaternion.h b/source/geometry/quaternion.h
new file mode 100644
index 0000000..7c03612
--- /dev/null
+++ b/source/geometry/quaternion.h
@@ -0,0 +1,194 @@
+#ifndef MSP_MATH_QUATERNION_H_
+#define MSP_MATH_QUATERNION_H_
+
+#include <cmath>
+#include <msp/linal/matrix.h>
+#include <msp/linal/vector.h>
+#include "angle.h"
+
+namespace Msp {
+namespace Geometry {
+
+template<typename T>
+struct Quaternion
+{
+	T a, b, c, d;
+
+	Quaternion(): a(0), b(0), c(0), d(0) { }
+	Quaternion(T a_, T b_, T c_, T d_): a(a_), b(b_), c(c_), d(d_) { }
+
+	static Quaternion one() { return Quaternion(1, 0, 0, 0); }
+	static Quaternion rotation(Angle<T>, const LinAl::Vector<T, 3> &);
+
+	T scalar() const { return a; }
+	LinAl::Vector<T, 3> vector() const { return LinAl::Vector<T, 3>(b, c, d); }
+
+	Quaternion &operator+=(const Quaternion &);
+	Quaternion &operator-=(const Quaternion &);
+	Quaternion &operator*=(T);
+	Quaternion &operator*=(const Quaternion &);
+	Quaternion &operator/=(T);
+	Quaternion &operator/=(const Quaternion &);
+
+	T norm() const;
+	Quaternion &normalize();
+
+	Quaternion conjugate() const { return Quaternion(a, -b, -c, -d); }
+	Quaternion inverse() const;
+
+	LinAl::Vector<T, 3> rotate(const LinAl::Vector<T, 3> &) const;
+	LinAl::Matrix<T, 3, 3> to_matrix() const;
+};
+
+template<typename T>
+Quaternion<T> Quaternion<T>::rotation(Angle<T> angle, const LinAl::Vector<T, 3> &axis)
+{
+	float c = cos(angle/T(2));
+	float s = sin(angle/T(2));
+	return Quaternion<T>(c, s*axis.x, s*axis.y, s*axis.z);
+}
+
+template<typename T>
+Quaternion<T> &Quaternion<T>::operator+=(const Quaternion<T> &q)
+{
+	a += q.a;
+	b += q.b;
+	c += q.c;
+	d += q.d;
+	return *this;
+}
+
+template<typename T>
+Quaternion<T> operator+(const Quaternion<T> &q1, const Quaternion<T> &q2)
+{
+	Quaternion<T> r(q1);
+	return r += q2;
+}
+
+template<typename T>
+Quaternion<T> &Quaternion<T>::operator-=(const Quaternion<T> &q)
+{
+	a -= q.a;
+	b -= q.b;
+	c -= q.c;
+	d -= q.d;
+	return *this;
+}
+
+template<typename T>
+Quaternion<T> operator-(const Quaternion<T> &q1, const Quaternion<T> &q2)
+{
+	Quaternion<T> r(q1);
+	return r -= q2;
+}
+
+template<typename T>
+Quaternion<T> &Quaternion<T>::operator*=(T s)
+{
+	a *= s;
+	b *= s;
+	c *= s;
+	d *= s;
+	return *this;
+}
+
+template<typename T>
+Quaternion<T> operator*(const Quaternion<T> &q, T s)
+{
+	Quaternion<T> r(q);
+	return r *= s;
+}
+
+template<typename T>
+Quaternion<T> operator*(T s, const Quaternion<T> &q)
+{
+	return q*s;
+}
+
+template<typename T>
+Quaternion<T> operator*(const Quaternion<T> &q1, const Quaternion<T> &q2)
+{
+	return Quaternion<T>(q1.a*q2.a-q1.b*q2.b-q1.c*q2.c-q1.d*q2.d,
+		q1.a*q2.b+q1.b*q2.a+q1.c*q2.d-q1.d*q2.c,
+		q1.a*q2.c+q1.c*q2.a-q1.b*q2.d+q1.d*q2.b,
+		q1.a*q2.d+q1.d*q2.a+q1.b*q2.c-q1.c*q2.b);
+}
+
+template<typename T>
+Quaternion<T> &Quaternion<T>::operator*=(const Quaternion<T> &q)
+{
+	return *this = *this*q;
+}
+
+template<typename T>
+Quaternion<T> &Quaternion<T>::operator/=(T s)
+{
+	a /= s;
+	b /= s;
+	c /= s;
+	d /= s;
+	return *this;
+}
+
+template<typename T>
+Quaternion<T> operator/(const Quaternion<T> &q, T s)
+{
+	Quaternion<T> r(q);
+	return r /= s;
+}
+
+template<typename T>
+Quaternion<T> operator/(T s, const Quaternion<T> &q)
+{
+	return s*q.inverse();
+}
+
+template<typename T>
+Quaternion<T> &Quaternion<T>::operator/=(const Quaternion<T> &q)
+{
+	return *this *= q.inverse();
+}
+
+template<typename T>
+Quaternion<T> operator/(const Quaternion<T> &q1, const Quaternion<T> &q2)
+{
+	Quaternion<T> r(q1);
+	return r /= q2;
+}
+
+template<typename T>
+T Quaternion<T>::norm() const
+{
+	using std::sqrt;
+	return sqrt(a*a+b*b+c*c+d*d);
+}
+
+template<typename T>
+Quaternion<T> Quaternion<T>::inverse() const
+{
+	return conjugate()/(a*a+b*b+c*c+d*d);
+}
+
+template<typename T>
+LinAl::Vector<T, 3> Quaternion<T>::rotate(const LinAl::Vector<T, 3> &v) const
+{
+	return (*this*Quaternion(0, v.x, v.y, v.z)*inverse()).vector();
+}
+
+template<typename T>
+LinAl::Matrix<T, 3, 3> Quaternion<T>::to_matrix() const
+{
+	T s = 2*(a*a+b*b+c*c+d*d);
+	LinAl::Vector<T, 3> columns[3] =
+	{
+		{ 1-s*(c*c+d*d), s*(b*c+a*d), s*(b*d-a*c) },
+		{ s*(b*c-a*d), 1-s*(b*b+d*d), s*(c*d+a*b) },
+		{ s*(b*d+a*c), s*(c*d-a*b), 1-s*(b*b+c*c) }
+	};
+	return LinAl::Matrix<T, 3, 3>::from_columns(columns);
+}
+
+} // namespace Geometry
+} // namespace Msp
+
+#endif
-- 
2.45.2