Merge pull request 'SLERP' (#163) from slerp into develop

Reviewed-on: #163
Reviewed-by: Robin Müller <muellerr@irs.uni-stuttgart.de>

src/fsfw/globalfunctions/math/QuaternionOperations.cpp

@@ -39,6 +39,37 @@ void QuaternionOperations::inverse(const double* quaternion, double* inverseQuat
   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() {}
 
 void QuaternionOperations::normalize(const double* quaternion, double* unitQuaternion) {

src/fsfw/globalfunctions/math/QuaternionOperations.h

@@ -23,6 +23,8 @@ class QuaternionOperations {
 
   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
    */