From 1f0e4e545eeb7700ad7466e02b7b96a6636a9eb0 Mon Sep 17 00:00:00 2001
From: lisyarus <lisyarus@gmail.com>
Date: Fri, 7 Jan 2022 23:49:14 +0300
Subject: [PATCH] Add quaternion derivative to angular momentum transform
 matrix

---
 libs/geom/include/psemek/geom/quaternion.hpp | 20 ++++++++++++++++++++
 1 file changed, 20 insertions(+)

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 <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;
+	}
+
 }