diff --git a/libs/geom/include/psemek/geom/quaternion.hpp b/libs/geom/include/psemek/geom/quaternion.hpp index c7abc87e..ade080aa 100644 --- a/libs/geom/include/psemek/geom/quaternion.hpp +++ b/libs/geom/include/psemek/geom/quaternion.hpp @@ -246,6 +246,21 @@ namespace psemek::geom return std::exp(q[3]) * quaternion({v[0] * s, v[1] * s, v[2] * s, std::cos(l)}); } + template + quaternion log(quaternion const & q) + { + auto v = vector{q[0], q[1], q[2]}; + auto l = length(v); + auto n = length(q); + + if (l == T{}) + v = v.zero(); + else + v = (v / l) * std::acos(q[3] / n); + + return quaternion{{v[0], v[1], v[2], std::log(n)}}; + } + template std::ostream & operator << (std::ostream & os, quaternion const & q) {