From a2cf92dd6984657b4fe1c485198b2033543636d8 Mon Sep 17 00:00:00 2001
From: Milo Priegnitz <milo.priegnitz@rwth-aachen.de>
Date: Sat, 22 Jun 2024 15:30:31 +0200
Subject: [PATCH] Add Quaternion to rotation matrix

---
 include/sta/math/quaternion.hpp |  3 +++
 src/quaternion.cpp              | 17 +++++++++++++++++
 2 files changed, 20 insertions(+)

diff --git a/include/sta/math/quaternion.hpp b/include/sta/math/quaternion.hpp
index 70f4111..b6b48ec 100644
--- a/include/sta/math/quaternion.hpp
+++ b/include/sta/math/quaternion.hpp
@@ -8,6 +8,7 @@
 #ifndef STA_PEAK_QUATERNION_HPP
 #define STA_PEAK_QUATERNION_HPP
 
+#include <sta/math/linalg/matrix.hpp>
 namespace sta
 {
 	namespace math {
@@ -24,6 +25,8 @@ namespace sta
 
 			float norm();
 
+			matrix toRotationMatrix();
+
 			Quaternion normalized();
 
 			Quaternion operator*(const Quaternion& quat);
diff --git a/src/quaternion.cpp b/src/quaternion.cpp
index 54c1b16..25e6ea6 100644
--- a/src/quaternion.cpp
+++ b/src/quaternion.cpp
@@ -54,6 +54,23 @@ namespace sta
 			return Quaternion(qw, qx, qy, qz).normalized();
 	    }
 
+		matrix Quaternion::toRotationMatrix(){
+			
+			matrix R = matrix(3, 3);
+			R.set(0, 0, 2 *(w*w + x*x) - 1);
+			R.set(0, 1, 2 *(x*y - w*z));
+			R.set(0, 2, 2 *(x*z + w*y));
+
+			R.set(1, 0, 2 *(x*y + w*z));
+			R.set(1, 1, 2 *(w*w + y*y) - 1);
+			R.set(1, 2, 2 *(y*z - w*x));
+
+			R.set(2, 0, 2 *(x*z - w*y));
+			R.set(2, 1, 2 *(y*z + w*x));
+			R.set(2, 2, 2 *(w*w + z*z) - 1);
+			return R;
+		}
+
 	    Quaternion Quaternion::unit()
 	    {
 	    	return Quaternion();