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

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

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
parent c648229c99 94de483836
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);
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) {
// nothing to calculate here
std::memcpy(q, q1s, 4 * sizeof(double));
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,