82 lines
2.1 KiB
C
82 lines
2.1 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);
|
||
|
|
|
||
|
|
/**
|
||
|
|
* 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_ */
|