fsfw/src/fsfw/globalfunctions/math/QuaternionOperations.h

#ifndef QUATERNIONOPERATIONS_H_
#define QUATERNIONOPERATIONS_H_

#include <stdint.h>

class QuaternionOperations {
public:
	virtual ~QuaternionOperations();

	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 toDcm(const double *quaternion, double dcm[][3]);

	static void toDcm(const double *quaternion, float dcm[][3]);

	static float norm(const double *quaternion);

	static void normalize(double *quaternion);

	static void normalize(const double *quaternion, double *unitQuaternion);

	static void inverse(const double *quaternion, double *inverseQuaternion);

	/**
	 * returns angle in ]-Pi;Pi] or [0;Pi] if abs == true
	 */
	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();
};

#endif /* QUATERNIONOPERATIONS_H_ */