eive/fsfw
eive
/
fsfw
forked from fsfw/fsfw
14
1
Fork 0
Code Issues Pull Requests 1 Releases 1 Wiki Activity

slerp

This commit is contained in:
Marius Eggert 2023-11-27 10:38:25 +01:00
parent 41d67bff63
commit 74775bb59d
2 changed files with 17 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/fsfw/globalfunctions/math/QuaternionOperations.cpp
View File

@ -39,6 +39,21 @@ void QuaternionOperations::inverse(const double* quaternion, double* inverseQuat
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 qD[4] = {0, 0, 0, 0}, left[4] = {0, 0, 0, 0}, right[4] = {0, 0, 0, 0};
multiply(q1, q2, qD);
double angle = getAngle(qD) / 2.;
VectorOperations<double>::mulScalar(q1, std::sin((1 - weight) * angle) / std::sin(angle), left,
4);
VectorOperations<double>::mulScalar(q1, std::sin(weight * angle) / std::sin(angle), right, 4);
VectorOperations<double>::add(left, right, q, 4);
normalize(q);
}
QuaternionOperations::QuaternionOperations() {}
void QuaternionOperations::normalize(const double* quaternion, double* unitQuaternion) {

2
src/fsfw/globalfunctions/math/QuaternionOperations.h
View File

@ -23,6 +23,8 @@ class QuaternionOperations {
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]);
/**
* returns angle in ]-Pi;Pi] or [0;Pi] if abs == true
*/