Add quaternion derivative to angular momentum transform matrix

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 <typename T>
+	geom::matrix<T, 3, 4> angular_gradient(quaternion<T> const & q)
+	{
+		geom::matrix<T, 3, 4> 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;
+	}
+
 }