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

SLERP for antiparallel Quaternions #165

Merged
meggert merged 1 commits from slerp-fix-antiparallel-quaternions into develop 2023-12-06 10:41:03 +01:00
Conversation 0 Commits 1 Files Changed 1 +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

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,