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 vector rotate(quaternion const & q, vector const & v) { - auto res = q * quaternion::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