diff --git a/libs/geom/include/psemek/geom/quaternion.hpp b/libs/geom/include/psemek/geom/quaternion.hpp index f91d50e8..7941372f 100644 --- a/libs/geom/include/psemek/geom/quaternion.hpp +++ b/libs/geom/include/psemek/geom/quaternion.hpp @@ -240,4 +240,24 @@ namespace psemek::geom return os << q.coords; } + // Returns a matrix M such that the angular gradient of function f(q) is M * grad(f) + template + geom::matrix angular_gradient(quaternion const & q) + { + geom::matrix result; + result[0][0] = q[3]; + result[0][1] = - q[2]; + result[0][2] = q[1]; + result[0][3] = - q[0]; + result[1][0] = q[2]; + result[1][1] = q[3]; + result[1][2] = - q[0]; + result[1][3] = - q[1]; + result[2][0] = - q[1]; + result[2][1] = q[0]; + result[2][2] = q[3]; + result[2][3] = - q[2]; + return result; + } + }