62 lines
2.2 KiB
C++
62 lines
2.2 KiB
C++
#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);
|
|
|
|
static void slerp(const double q1[4], const double q2[4], const double weight, double q[4]);
|
|
|
|
static void rotationFromQuaternions(const double qNew[4], const double qOld[4],
|
|
const double timeDelta, double rotRate[3]);
|
|
|
|
static void preventSignJump(double qNew[4], const double qOld[4]);
|
|
|
|
/**
|
|
* 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_ */
|