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

Merge pull request 'SLERP for antiparallel Quaternions' (#165) from slerp-fix-antiparallel-quaternions into develop

Browse Source 
Reviewed-on: #165
Reviewed-by: Robin Müller <muellerr@irs.uni-stuttgart.de>
This commit is contained in:
Marius Eggert 2023-12-06 10:41:02 +01:00
commit 7105e199c6
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,