From 84ef3faa68989eb73548ce6f63be7419d7c40e35 Mon Sep 17 00:00:00 2001
From: lisyarus <lisyarus@gmail.com>
Date: Wed, 28 Feb 2024 16:59:29 +0300
Subject: [PATCH] Use faster quaternion rotation formula

---
 libs/geom/include/psemek/geom/quaternion.hpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/libs/geom/include/psemek/geom/quaternion.hpp b/libs/geom/include/psemek/geom/quaternion.hpp
index 911f5a0d..3cbc2840 100644
--- a/libs/geom/include/psemek/geom/quaternion.hpp
+++ b/libs/geom/include/psemek/geom/quaternion.hpp
@@ -213,8 +213,10 @@ namespace psemek::geom
 	template <typename T>
 	vector<T, 3> rotate(quaternion<T> const & q, vector<T, 3> const & v)
 	{
-		auto res = q * quaternion<T>::vector(v) * conjugate(q);
-		return {res[0], res[1], res[2]};
+		// 2x faster than q*v*cong(q), see https://www.johndcook.com/blog/2021/06/16/faster-quaternion-rotations/
+		auto qv = vector{q[0], q[1], q[2]};
+		auto t = T(2) * cross(qv, v);
+		return v + q[3] * t + cross(qv, t);
 	}
 
 	template <typename T>