From b51b0d3a94102af936df42917eabe85b826184a5 Mon Sep 17 00:00:00 2001
From: lisyarus <lisyarus@gmail.com>
Date: Tue, 13 Jul 2021 11:45:11 +0300
Subject: [PATCH] Change quaternion coordinate order: wxyz -> xyzw

---
 libs/geom/include/psemek/geom/quaternion.hpp | 28 ++++++++++----------
 libs/geom/include/psemek/geom/rotation.hpp   | 20 +++++++-------
 2 files changed, 24 insertions(+), 24 deletions(-)

diff --git a/libs/geom/include/psemek/geom/quaternion.hpp b/libs/geom/include/psemek/geom/quaternion.hpp
index 6b927c9b..da1de6d3 100644
--- a/libs/geom/include/psemek/geom/quaternion.hpp
+++ b/libs/geom/include/psemek/geom/quaternion.hpp
@@ -40,19 +40,19 @@ namespace psemek::geom
 	template <typename T>
 	quaternion<T> quaternion<T>::identity()
 	{
-		return quaternion<T>{{T(1), T(0), T(0), T(0)}};
+		return quaternion<T>{{T(0), T(0), T(0), T(1)}};
 	}
 
 	template <typename T>
 	quaternion<T> quaternion<T>::scalar(T value)
 	{
-		return {{value, T(0), T(0), T(0)}};
+		return {{T(0), T(0), T(0), value}};
 	}
 
 	template <typename T>
 	quaternion<T> quaternion<T>::vector(geom::vector<T, 3> const & v)
 	{
-		return {{T(0), v[0], v[1], v[2]}};
+		return {{v[0], v[1], v[2], T(0)}};
 	}
 
 	template <typename T>
@@ -61,7 +61,7 @@ namespace psemek::geom
 		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]}};
+		return quaternion<T>{{s * axis[0], s * axis[1], s * axis[2], c}};
 	}
 
 	template <typename T>
@@ -74,22 +74,22 @@ namespace psemek::geom
 		if (tr > 0)
 		{
 			auto S = std::sqrt(tr + 1) * 2;
-			return {{S / 4, (m[2][1] - m[1][2]) / S, (m[0][2] - m[2][0]) / S, (m[1][0] - m[0][1]) / S}};
+			return {{(m[2][1] - m[1][2]) / S, (m[0][2] - m[2][0]) / S, (m[1][0] - m[0][1]) / S, S / 4}};
 		}
 		else if (m[0][0] > m[1][1] && m[0][0] > m[2][2])
 		{
 			auto S = std::sqrt(1 + m[0][0] - m[1][1] - m[2][2]) * 2;
-			return {{(m[2][1] - m[1][2]) / S, S / 4, (m[0][1] + m[1][0]) / S, (m[0][2] + m[2][0]) / S}};
+			return {{S / 4, (m[0][1] + m[1][0]) / S, (m[0][2] + m[2][0]) / S, (m[2][1] - m[1][2]) / S}};
 		}
 		else if (m[1][1] > m[2][2])
 		{
 			auto S = std::sqrt(1 - m[0][0] + m[1][1] - m[2][2]) * 2;
-			return {{(m[0][2] - m[2][0]) / S, (m[0][1] + m[1][0]) / S, S / 4, (m[1][2] + m[2][1]) / S}};
+			return {{(m[0][1] + m[1][0]) / S, S / 4, (m[1][2] + m[2][1]) / S, (m[0][2] - m[2][0]) / S}};
 		}
 		else
 		{
 			auto S = std::sqrt(1 - m[0][0] - m[1][1] + m[2][2]) * 2;
-			return {{(m[1][0] - m[0][1]) / S, (m[0][2] + m[2][0]) / S, (m[1][2] + m[2][1]) / S, S / 4}};
+			return {{(m[0][2] + m[2][0]) / S, (m[1][2] + m[2][1]) / S, S / 4, (m[1][0] - m[0][1]) / S}};
 		}
 	}
 
@@ -127,10 +127,10 @@ namespace psemek::geom
 	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];
+		r[0] = q1[3] * q2[0] + q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1];
+		r[1] = q1[3] * q2[1] - q1[0] * q2[2] + q1[1] * q2[3] + q1[2] * q2[0];
+		r[2] = q1[3] * q2[2] + q1[0] * q2[1] - q1[1] * q2[0] + q1[2] * q2[3];
+		r[3] = q1[3] * q2[3] - q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2];
 		return r;
 	}
 
@@ -149,7 +149,7 @@ namespace psemek::geom
 	template <typename T>
 	quaternion<T> conjugate(quaternion<T> const & q)
 	{
-		return {{q[0], -q[1], -q[2], -q[3]}};
+		return {{-q[0], -q[1], -q[2], q[3]}};
 	}
 
 	template <typename T>
@@ -162,7 +162,7 @@ namespace psemek::geom
 	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]};
+		return {res[0], res[1], res[2]};
 	}
 
 	template <typename T>
diff --git a/libs/geom/include/psemek/geom/rotation.hpp b/libs/geom/include/psemek/geom/rotation.hpp
index bb3eb3f3..04576f10 100644
--- a/libs/geom/include/psemek/geom/rotation.hpp
+++ b/libs/geom/include/psemek/geom/rotation.hpp
@@ -332,21 +332,21 @@ namespace psemek::geom
 	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]);
+		m[0][0] = 1 - 2 * (quat[1] * quat[1] + quat[2] * quat[2]);
+		m[0][1] = 2 * (quat[0] * quat[1] - quat[3] * quat[2]);
+		m[0][2] = 2 * (quat[0] * quat[2] + quat[3] * quat[1]);
+		m[1][0] = 2 * (quat[0] * quat[1] + quat[3] * quat[2]);
+		m[1][1] = 1 - 2 * (quat[0] * quat[0] + quat[2] * quat[2]);
+		m[1][2] = 2 * (quat[1] * quat[2] - quat[3] * quat[0]);
+		m[2][0] = 2 * (quat[0] * quat[2] - quat[3] * quat[1]);
+		m[2][1] = 2 * (quat[1] * quat[2] + quat[3] * quat[0]);
+		m[2][2] = 1 - 2 * (quat[0] * quat[0] + quat[1] * quat[1]);
 	}
 
 	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]}};
+		return quaternion_rotation<T>{quaternion<T>{-r.quat[0], -r.quat[1], -r.quat[2], r.quat[3]}};
 	}
 
 }