Files
action
container
contrib
controller
coordinates
datalinklayer
datapool
datapoollocal
defaultcfg
devicehandlers
doc
events
fdir
globalfunctions
matching
math
CMakeLists.txt
MatrixOperations.h
QuaternionOperations.cpp
QuaternionOperations.h
VectorOperations.h
AsciiConverter.cpp
AsciiConverter.h
CMakeLists.txt
CRC.cpp
CRC.h
DleEncoder.cpp
DleEncoder.h
PeriodicOperationDivider.cpp
PeriodicOperationDivider.h
Type.cpp
Type.h
arrayprinter.cpp
arrayprinter.h
bitutility.cpp
bitutility.h
constants.h
sign.h
timevalOperations.cpp
timevalOperations.h
health
housekeeping
internalError
ipc
logo
memory
modes
monitoring
objectmanager
osal
parameters
power
pus
returnvalues
rmap
serialize
serviceinterface
storagemanager
subsystem
tasks
tcdistribution
thermal
timemanager
tmstorage
tmtcpacket
tmtcservices
unittest
.gitignore
.gitmodules
CHANGELOG
CMakeLists.txt
FSFWVersion.h
LICENSE
NOTICE
README.md
fsfw.mk
fsfw/globalfunctions/math/QuaternionOperations.cpp
2020-08-13 20:53:35 +02:00

157 lines
4.6 KiB
C++

#include "QuaternionOperations.h"
#include "VectorOperations.h"
#include <cmath>
#include <cstring>
#include <stdint.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);
}
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]);
}
}
}