src/fsfw/globalfunctions/math/QuaternionOperations.cpp

@@ -41,12 +41,28 @@ void QuaternionOperations::inverse(const double* quaternion, double* inverseQuat
 
 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};
+  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;
 
-  multiply(q1, q2, qD);
+  // we need to be able to invert this quaternion
-  double angle = getAngle(qD) / 2.;
+  std::memcpy(q1s, q1, 4 * sizeof(double));
+  // calculate angle between orientations
+  inverse(q2, q2I);
+  multiply(q1s, q2I, qD);
+  angle = std::acos(qD[3]);
 
-  VectorOperations<double>::mulScalar(q1, std::sin((1 - weight) * angle) / std::sin(angle), left,
+  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);