updating code from Flying Laptop

This is the framework of Flying Laptop OBSW version A.13.0.

globalfunctions/math/QuaternionOperations.h

@@ -9,8 +9,9 @@ public:
 
 	static void multiply(const double *q1, const double *q2, double *q);
 
-	static void fromDcm(const double dcm[][3],double *quaternion, uint8_t *index = 0);
-	
+	static void fromDcm(const double dcm[][3], double *quaternion,
+			uint8_t *index = 0);
+
 	static void toDcm(const double *quaternion, double dcm[][3]);
 
 	static void toDcm(const double *quaternion, float dcm[][3]);
@@ -28,6 +29,51 @@ public:
 	 */
 	static double getAngle(const double *quaternion, bool abs = false);
 
+	//multiplies 3d vector with dcm derived from quaternion
+	template<typename T>
+	static void multiplyVector(const double *quaternion, const T *vector,
+			T * result) {
+		result[0] =
+				(2.
+						* (quaternion[0] * quaternion[0]
+								+ quaternion[3] * quaternion[3]) - 1.)
+						* vector[0]
+						+ 2.
+								* (quaternion[0] * quaternion[1]
+										+ quaternion[2] * quaternion[3])
+								* vector[1]
+						+ 2.
+								* (quaternion[0] * quaternion[2]
+										- quaternion[1] * quaternion[3])
+								* vector[2];
+
+		result[1] =
+				2.
+						* (quaternion[0] * quaternion[1]
+								- quaternion[2] * quaternion[3]) * vector[0]
+						+ (2.
+								* (quaternion[1] * quaternion[1]
+										+ quaternion[3] * quaternion[3]) - 1.)
+								* vector[1]
+						+ 2.
+								* (quaternion[1] * quaternion[2]
+										+ quaternion[0] * quaternion[3])
+								* vector[2];
+
+		result[2] =
+				2.
+						* (quaternion[0] * quaternion[2]
+								+ quaternion[1] * quaternion[3]) * vector[0]
+						+ 2.
+								* (quaternion[1] * quaternion[2]
+										- quaternion[0] * quaternion[3])
+								* vector[1]
+						+ (2.
+								* (quaternion[2] * quaternion[2]
+										+ quaternion[3] * quaternion[3]) - 1.)
+								* vector[2];
+	}
+
 private:
 	QuaternionOperations();
 };