Make slerp more robust

Make sure that slerp goes through the shortest path

Author: Rombur

source/Quaternion.cc

@@ -8,6 +8,7 @@
 #include <deal.II/base/signaling_nan.h>
 
 #include <cmath>
+#include <numeric>
 
 namespace adamantine
 {
@@ -81,6 +82,18 @@ Quaternion &Quaternion::operator*=(Quaternion const &other)
   return *this;
 }
 
+Quaternion &Quaternion::operator*=(double const &scalar)
+{
+  for (int i = 0; i < 4; ++i)
+  {
+    _quaternion[i] *= scalar;
+  }
+
+  build_rotation_matrices();
+
+  return *this;
+}
+
 Quaternion &Quaternion::operator/=(Quaternion const &other)
 {
   ASSERT(_is_valid, "");
@@ -162,4 +175,10 @@ void Quaternion::build_rotation_matrices()
     }
   }
 }
+
+double dot_product(Quaternion const &p, Quaternion const &q)
+{
+  return std::inner_product(p._quaternion.begin(), p._quaternion.end(),
+                            q._quaternion.begin(), 0.);
+}
 } // namespace adamantine

source/Quaternion.hh

@@ -1,4 +1,4 @@
-/* SPDX-FileCopyrightText: Copyright (c) 2025, the adamantine authors.
+/* SPDX-FileCopyrightText: Copyright (c) 2025 - 2026, the adamantine authors.
  * SPDX-License-Identifier: Apache-2.0 WITH LLVM-exception
  */
 
@@ -77,6 +77,11 @@ public:
    */
   Quaternion &operator*=(Quaternion const &other);
 
+  /**
+   * Multiply the quaternion with a scalar.
+   */
+  Quaternion &operator*=(double const &scalar);
+
   /**
    * Multiply the quaternions using the inverse of @p other.
    */
@@ -88,6 +93,11 @@ public:
    */
   void pow(double const exp);
 
+  /**
+   * Compute the dot product of two quaternions.
+   */
+  friend double dot_product(Quaternion const &p, Quaternion const &q);
+
 private:
   /**
    * Build the different rotation matrices.

source/ScanPath.cc

@@ -325,6 +325,15 @@ Quaternion ScanPath::get_quaternion(double const time) const
 
   Quaternion interpolated_rotation =
       _segment_list[current_segment].end_rotation;
+
+  // If the dot product of the initial quaternion and the final rotation is
+  // negative, slerp will take the long way instead of the short path. In that
+  // case, we need to negate one of the quaternion.
+  if (dot_product(segment_start_rotation, interpolated_rotation) < 0.)
+  {
+    interpolated_rotation *= -1.;
+  }
+
   interpolated_rotation /= segment_start_rotation;
   interpolated_rotation.pow((time - segment_start_time) / duration);
   interpolated_rotation *= segment_start_rotation;