KSat/fsfw
15
4
Fork 0
forked from fsfw/fsfw
Code Issues Pull Requests 1 Releases Wiki Activity

Bump FSFW #31

Merged
muellerr merged 139 commits from bump-fsfw into develop 2023-12-18 13:16:28 +01:00
Conversation 0 Commits 139 Files Changed 116 +7 -5
1 changed files with 7 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 7105e199c6 - Show all commits

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

@ -51,15 +51,17 @@ void QuaternionOperations::slerp(const double q1[4], const double q2[4], const d
multiply(q1s, q2I, qD); multiply(q1s, q2I, qD);
angle = std::acos(qD[3]); 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) { if (std::sin(angle) == 0.0) {
// nothing to calculate here // nothing to calculate here
std::memcpy(q, q1s, 4 * sizeof(double)); std::memcpy(q, q1s, 4 * sizeof(double));
return; return;
} else if (std::cos(angle) < 0.0) {
// we need to invert one quaternione
VectorOperations<double>::mulScalar(q1s, -1, q1s, 4);
multiply(q1s, q2I, qD);
angle = std::acos(qD[3]);
} }
VectorOperations<double>::mulScalar(q1s, std::sin((1 - weight) * angle) / std::sin(angle), left, VectorOperations<double>::mulScalar(q1s, std::sin((1 - weight) * angle) / std::sin(angle), left,