diff --git a/libs/geom/include/psemek/geom/quaternion.hpp b/libs/geom/include/psemek/geom/quaternion.hpp index 67d601d8..a7a378fa 100644 --- a/libs/geom/include/psemek/geom/quaternion.hpp +++ b/libs/geom/include/psemek/geom/quaternion.hpp @@ -199,6 +199,17 @@ namespace psemek::geom } } + template + quaternion exp(quaternion const & q) + { + auto v = vector{q[0], q[1], q[2]}; + auto l = length(v); + + auto s = sin_over_x(l); + + return std::exp(q[3]) * quaternion({v[0] * s, v[1] * s, v[2] * s, std::cos(l)}); + } + template std::ostream & operator << (std::ostream & os, quaternion const & q) {