Bump FSFW #31

Merged
muellerr merged 139 commits from bump-fsfw into develop 2023-12-18 13:16:28 +01:00
2 changed files with 33 additions and 0 deletions
Showing only changes of commit c648229c99 - Show all commits

View File

@ -39,6 +39,37 @@ 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 q1s[4] = {0, 0, 0, 0}, q2I[4] = {0, 0, 0, 0}, qD[4] = {0, 0, 0, 0}, left[4] = {0, 0, 0, 0},
right[4] = {0, 0, 0, 0}, angle = 0;
// we need to be able to invert this quaternion
std::memcpy(q1s, q1, 4 * sizeof(double));
// calculate angle between orientations
inverse(q2, q2I);
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,
4);
VectorOperations<double>::mulScalar(q2, 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) {

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
*/ */