187 lines
6.9 KiB
C++
187 lines
6.9 KiB
C++
#include "fsfw/globalfunctions/math/QuaternionOperations.h"
|
|
|
|
#include <stdint.h>
|
|
|
|
#include <cmath>
|
|
#include <cstring>
|
|
|
|
#include "fsfw/globalfunctions/math/VectorOperations.h"
|
|
|
|
QuaternionOperations::~QuaternionOperations() {}
|
|
|
|
void QuaternionOperations::multiply(const double* q1, const double* q2, double* q) {
|
|
double out[4];
|
|
|
|
out[0] = q1[3] * q2[0] + q1[2] * q2[1] - q1[1] * q2[2] + q1[0] * q2[3];
|
|
out[1] = -q1[2] * q2[0] + q1[3] * q2[1] + q1[0] * q2[2] + q1[1] * q2[3];
|
|
out[2] = q1[1] * q2[0] - q1[0] * q2[1] + q1[3] * q2[2] + q1[2] * q2[3];
|
|
out[3] = -q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] + q1[3] * q2[3];
|
|
|
|
memcpy(q, out, 4 * sizeof(*q));
|
|
}
|
|
|
|
void QuaternionOperations::toDcm(const double* quaternion, double dcm[][3]) {
|
|
dcm[0][0] = 2 * (quaternion[0] * quaternion[0] + quaternion[3] * quaternion[3]) - 1;
|
|
dcm[0][1] = 2 * (quaternion[0] * quaternion[1] + quaternion[2] * quaternion[3]);
|
|
dcm[0][2] = 2 * (quaternion[0] * quaternion[2] - quaternion[1] * quaternion[3]);
|
|
|
|
dcm[1][0] = 2 * (quaternion[0] * quaternion[1] - quaternion[2] * quaternion[3]);
|
|
dcm[1][1] = 2 * (quaternion[1] * quaternion[1] + quaternion[3] * quaternion[3]) - 1;
|
|
dcm[1][2] = 2 * (quaternion[1] * quaternion[2] + quaternion[0] * quaternion[3]);
|
|
|
|
dcm[2][0] = 2 * (quaternion[0] * quaternion[2] + quaternion[1] * quaternion[3]);
|
|
dcm[2][1] = 2 * (quaternion[1] * quaternion[2] - quaternion[0] * quaternion[3]);
|
|
dcm[2][2] = 2 * (quaternion[2] * quaternion[2] + quaternion[3] * quaternion[3]) - 1;
|
|
}
|
|
|
|
void QuaternionOperations::inverse(const double* quaternion, double* inverseQuaternion) {
|
|
memcpy(inverseQuaternion, quaternion, 4 * sizeof(*quaternion));
|
|
VectorOperations<double>::mulScalar(inverseQuaternion, -1, inverseQuaternion, 3);
|
|
}
|
|
|
|
void QuaternionOperations::slerp(const double q1[4], const double q2[4], const double weight,
|
|
double q[4]) {
|
|
double q1s[4] = {0, 0, 0, 0}, q2I[4] = {0, 0, 0, 0}, qD[4] = {0, 0, 0, 0}, left[4] = {0, 0, 0, 0},
|
|
right[4] = {0, 0, 0, 0}, angle = 0;
|
|
|
|
// we need to be able to invert this quaternion
|
|
std::memcpy(q1s, q1, 4 * sizeof(double));
|
|
// calculate angle between orientations
|
|
inverse(q2, q2I);
|
|
multiply(q1s, q2I, qD);
|
|
angle = std::acos(qD[3]);
|
|
|
|
if (std::cos(angle) < 0.0) {
|
|
// we need to invert one quaternion
|
|
VectorOperations<double>::mulScalar(q1s, -1, q1s, 4);
|
|
multiply(q1s, q2I, qD);
|
|
angle = std::acos(qD[3]);
|
|
}
|
|
|
|
if (std::sin(angle) == 0.0) {
|
|
// nothing to calculate here
|
|
std::memcpy(q, q1s, 4 * sizeof(double));
|
|
return;
|
|
}
|
|
|
|
VectorOperations<double>::mulScalar(q1s, std::sin((1 - weight) * angle) / std::sin(angle), left,
|
|
4);
|
|
VectorOperations<double>::mulScalar(q2, std::sin(weight * angle) / std::sin(angle), right, 4);
|
|
VectorOperations<double>::add(left, right, q, 4);
|
|
|
|
normalize(q);
|
|
}
|
|
|
|
void QuaternionOperations::preventSignJump(double qNew[4], const double qOld[4]) {
|
|
double qDiff[4] = {0, 0, 0, 0}, qSum[4] = {0, 0, 0, 0};
|
|
VectorOperations<double>::subtract(qOld, qNew, qDiff, 4);
|
|
VectorOperations<double>::add(qOld, qNew, qSum, 4);
|
|
if (VectorOperations<double>::norm(qDiff, 4) > VectorOperations<double>::norm(qSum, 4)) {
|
|
VectorOperations<double>::mulScalar(qNew, -1, qNew, 4);
|
|
}
|
|
}
|
|
|
|
QuaternionOperations::QuaternionOperations() {}
|
|
|
|
void QuaternionOperations::normalize(const double* quaternion, double* unitQuaternion) {
|
|
VectorOperations<double>::normalize(quaternion, unitQuaternion, 4);
|
|
}
|
|
|
|
float QuaternionOperations::norm(const double* quaternion) {
|
|
return VectorOperations<double>::norm(quaternion, 4);
|
|
}
|
|
|
|
void QuaternionOperations::fromDcm(const double dcm[][3], double* quaternion, uint8_t* index) {
|
|
double a[4];
|
|
|
|
a[0] = 1 + dcm[0][0] - dcm[1][1] - dcm[2][2];
|
|
a[1] = 1 - dcm[0][0] + dcm[1][1] - dcm[2][2];
|
|
a[2] = 1 - dcm[0][0] - dcm[1][1] + dcm[2][2];
|
|
a[3] = 1 + dcm[0][0] + dcm[1][1] + dcm[2][2];
|
|
|
|
uint8_t maxAIndex = 0;
|
|
|
|
VectorOperations<double>::maxValue(a, 4, &maxAIndex);
|
|
|
|
if (index != 0) {
|
|
*index = maxAIndex;
|
|
}
|
|
|
|
switch (maxAIndex) {
|
|
case 0:
|
|
quaternion[0] = 0.5 * sqrt(a[0]);
|
|
quaternion[1] = (dcm[0][1] + dcm[1][0]) / (2 * sqrt(a[0]));
|
|
quaternion[2] = (dcm[0][2] + dcm[2][0]) / (2 * sqrt(a[0]));
|
|
quaternion[3] = (dcm[1][2] - dcm[2][1]) / (2 * sqrt(a[0]));
|
|
break;
|
|
case 1:
|
|
quaternion[0] = (dcm[0][1] + dcm[1][0]) / (2 * sqrt(a[1]));
|
|
quaternion[1] = 0.5 * sqrt(a[1]);
|
|
quaternion[2] = (dcm[1][2] + dcm[2][1]) / (2 * sqrt(a[1]));
|
|
quaternion[3] = (dcm[2][0] - dcm[0][2]) / (2 * sqrt(a[1]));
|
|
break;
|
|
case 2:
|
|
quaternion[0] = (dcm[0][2] + dcm[2][0]) / (2 * sqrt(a[2]));
|
|
quaternion[1] = (dcm[1][2] + dcm[2][1]) / (2 * sqrt(a[2]));
|
|
quaternion[2] = 0.5 * sqrt(a[2]);
|
|
quaternion[3] = (dcm[0][1] - dcm[1][0]) / (2 * sqrt(a[2]));
|
|
break;
|
|
case 3:
|
|
quaternion[0] = (dcm[1][2] - dcm[2][1]) / (2 * sqrt(a[3]));
|
|
quaternion[1] = (dcm[2][0] - dcm[0][2]) / (2 * sqrt(a[3]));
|
|
quaternion[2] = (dcm[0][1] - dcm[1][0]) / (2 * sqrt(a[3]));
|
|
quaternion[3] = 0.5 * sqrt(a[3]);
|
|
break;
|
|
}
|
|
}
|
|
|
|
void QuaternionOperations::toDcm(const double* quaternion, float dcm[][3]) {
|
|
dcm[0][0] = 2 * (quaternion[0] * quaternion[0] + quaternion[3] * quaternion[3]) - 1;
|
|
dcm[0][1] = 2 * (quaternion[0] * quaternion[1] + quaternion[2] * quaternion[3]);
|
|
dcm[0][2] = 2 * (quaternion[0] * quaternion[2] - quaternion[1] * quaternion[3]);
|
|
|
|
dcm[1][0] = 2 * (quaternion[0] * quaternion[1] - quaternion[2] * quaternion[3]);
|
|
dcm[1][1] = 2 * (quaternion[1] * quaternion[1] + quaternion[3] * quaternion[3]) - 1;
|
|
dcm[1][2] = 2 * (quaternion[1] * quaternion[2] + quaternion[0] * quaternion[3]);
|
|
|
|
dcm[2][0] = 2 * (quaternion[0] * quaternion[2] + quaternion[1] * quaternion[3]);
|
|
dcm[2][1] = 2 * (quaternion[1] * quaternion[2] - quaternion[0] * quaternion[3]);
|
|
dcm[2][2] = 2 * (quaternion[2] * quaternion[2] + quaternion[3] * quaternion[3]) - 1;
|
|
}
|
|
|
|
void QuaternionOperations::normalize(double* quaternion) { normalize(quaternion, quaternion); }
|
|
|
|
double QuaternionOperations::getAngle(const double* quaternion, bool abs) {
|
|
if (quaternion[3] >= 0) {
|
|
return 2 * acos(quaternion[3]);
|
|
} else {
|
|
if (abs) {
|
|
return 2 * acos(-quaternion[3]);
|
|
} else {
|
|
return -2 * acos(-quaternion[3]);
|
|
}
|
|
}
|
|
}
|
|
|
|
void QuaternionOperations::rotationFromQuaternions(const double qNew[4], const double qOld[4],
|
|
const double timeDelta, double rotRate[3]) {
|
|
double qOldInv[4] = {0, 0, 0, 0};
|
|
double qDelta[4] = {0, 0, 0, 0};
|
|
|
|
inverse(qOld, qOldInv);
|
|
multiply(qNew, qOldInv, qDelta);
|
|
if (VectorOperations<double>::norm(qDelta, 4) != 0.0) {
|
|
normalize(qDelta);
|
|
}
|
|
if (VectorOperations<double>::norm(qDelta, 3) == 0.0) {
|
|
rotRate[0] = 0.0;
|
|
rotRate[1] = 0.0;
|
|
rotRate[2] = 0.0;
|
|
return;
|
|
}
|
|
double rotVec[3] = {0, 0, 0};
|
|
double angle = getAngle(qDelta);
|
|
VectorOperations<double>::normalize(qDelta, rotVec, 3);
|
|
VectorOperations<double>::mulScalar(rotVec, angle / timeDelta, rotRate, 3);
|
|
}
|