1 files changed, 74 insertions, 5 deletions
diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp
index 64b980ad8..1b0cb3c41 100644
--- a/src/lib/mathlib/math/Quaternion.hpp
+++ b/src/lib/mathlib/math/Quaternion.hpp
@@ -44,15 +44,17 @@
 
 #include <math.h>
 #include "../CMSIS/Include/arm_math.h"
+#include "Vector.hpp"
+#include "Matrix.hpp"
 
 namespace math
 {
 
-class Quaternion {
-public:
-	float real;
-	Vector3<float> imag;
+template <unsigned int N, unsigned int M>
+class Matrix;
 
+class Quaternion : public Vector<4> {
+public:
 	/**
 	 * trivial ctor
 	 */
@@ -62,7 +64,74 @@ public:
 	/**
 	 * setting ctor
 	 */
-	Quaternion(const float a, const float b, const float c, const float d): real(a), imag(b, c, d) {
+	Quaternion(const float a0, const float b0, const float c0, const float d0): Vector(a0, b0, c0, d0) {
+	}
+
+	Quaternion(const Vector<4> &v) : Vector(v) {
+	}
+
+	Quaternion(const float *v) : Vector(v) {
+	}
+
+	/**
+	 * access to elements by index
+	 */
+	/*
+	inline float &operator ()(unsigned int i) {
+		return *(&a + i);
+	}
+	*/
+
+	/**
+	 * access to elements by index
+	 */
+	/*
+	inline const float &operator ()(unsigned int i) const {
+		return *(&a + i);
+	}
+	*/
+
+	/**
+	 * addition
+	 */
+	/*
+	const Quaternion operator +(const Quaternion &q) const {
+		return Quaternion(a + q.a, b + q.b, c + q.c, d + q.d);
+	}
+	*/
+
+	/**
+	 * subtraction
+	 */
+	/*
+	const Quaternion operator -(const Quaternion &q) const {
+		return Quaternion(a - q.a, b - q.b, c - q.c, d - q.d);
+	}
+	*/
+
+	Quaternion derivative(const Vector<3> &w) {
+	        float dataQ[] = {
+	                data[0], -data[1], -data[2], -data[3],
+	                data[1],  data[0], -data[3],  data[2],
+	                data[2],  data[3],  data[0], -data[1],
+	                data[3], -data[2],  data[1],  data[0]
+	        };
+	        Matrix<4,4> Q(dataQ);
+	        Vector<4> v(0.0f, w.data[0], w.data[1], w.data[2]);
+	        return Q * v * 0.5f;
+	}
+
+	void from_euler(float roll, float pitch, float yaw) {
+		double cosPhi_2 = cos(double(roll) / 2.0);
+		double sinPhi_2 = sin(double(roll) / 2.0);
+		double cosTheta_2 = cos(double(pitch) / 2.0);
+		double sinTheta_2 = sin(double(pitch) / 2.0);
+		double cosPsi_2 = cos(double(yaw) / 2.0);
+		double sinPsi_2 = sin(double(yaw) / 2.0);
+		data[0] = cosPhi_2 * cosTheta_2 * cosPsi_2 + sinPhi_2 * sinTheta_2 * sinPsi_2;
+		data[1] = sinPhi_2 * cosTheta_2 * cosPsi_2 - cosPhi_2 * sinTheta_2 * sinPsi_2;
+		data[2] = cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2;
+		data[3] = cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2;
 	}
 };
 }