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

SLERP #163

Merged
muellerr merged 4 commits from slerp into develop 2023-12-05 09:25:50 +01:00
Conversation 0 Commits 4 Files Changed 2 +17
2 changed files with 17 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 74775bb59d - Show all commits

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); 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() {} QuaternionOperations::QuaternionOperations() {}
void QuaternionOperations::normalize(const double* quaternion, double* unitQuaternion) { 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 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 * returns angle in ]-Pi;Pi] or [0;Pi] if abs == true
*/ */