BUG: Fix SLERP interp failing in some cases when t is 0 or 1

autoscoper/src/ui/AutoscoperMainWindow.cpp

@@ -1021,7 +1021,6 @@ bool AutoscoperMainWindow::load_tracking_results(
       if (save_as_matrix) {
         CoordFrame::from_matrix(m).to_xyzypr(m);
       }
-
       tracker->trial()->getXCurve(k)->insert(i, m[0]);
       tracker->trial()->getYCurve(k)->insert(i, m[1]);
       tracker->trial()->getZCurve(k)->insert(i, m[2]);
@@ -1030,6 +1029,8 @@ bool AutoscoperMainWindow::load_tracking_results(
   }
   file.close();
 
+
+
   is_tracking_saved = true;
 
   frame_changed();

libautoscoper/src/KeyCurve.hpp

@@ -57,6 +57,7 @@ class IKeyCurve { // Interface to simplify the use of KeyCurve in the frontend
 protected:
   class IKey {
   public:
+    virtual ~IKey() {}
     Tangent_type in_tangent_type;
     Tangent_type out_tangent_type;
     bool bind_tangents;
@@ -66,7 +67,7 @@ class IKeyCurve { // Interface to simplify the use of KeyCurve in the frontend
     float out_tangent;
     float a, b, c, d;
   };
-  typedef std::map<int, IKey*> key_map;
+  typedef std::map<int, std::shared_ptr<IKey>> key_map;
 public:
   enum Curve_type { X_CURVE, Y_CURVE, Z_CURVE, QUAT_CURVE };
   Curve_type type;
@@ -143,12 +144,12 @@ class KeyCurve : public IKeyCurve
 
     T value(const_iterator position) const
     {
-        return static_cast<const Key*>(position->second)->value;
+        return std::dynamic_pointer_cast<const KeyCurve<T>::Key>(position->second)->value;
     }
 
     void set_value(iterator position, T value)
     {
-        static_cast<Key*>(position->second)->value = value;
+        std::dynamic_pointer_cast<KeyCurve<T>::Key>(position->second)->value = value;
         key_changed(position);
     }
 
@@ -274,11 +275,12 @@ void KeyCurve<T>::insert(int time)
 {
   insert(time, (*this)(time));
 }
+
 template <typename T>
 void KeyCurve<T>::insert(int time, T value)
 {
-  Key *key = new Key;
-  key->value = value;
+  std::shared_ptr<Key> key(new Key);
+  key->value = T(value); // Copy the value
   key->in_tangent_type = SMOOTH;
   key->out_tangent_type = SMOOTH;
   key->bind_tangents = true;
@@ -354,17 +356,17 @@ T KeyCurve<T>::operator()(float time) const
 
   const_iterator p1 = keys.upper_bound(time);
   if (p1 == keys.begin()) {
-    return static_cast<KeyCurve<T>::Key*>(p1->second)->value;
+    return std::dynamic_pointer_cast<KeyCurve<T>::Key>(p1->second)->value;
   }
 
   const_iterator p0 = p1; p0--;
   if (p1 == keys.end()) {
-    return static_cast<KeyCurve<T>::Key*>(p0->second)->value;
+    return std::dynamic_pointer_cast<KeyCurve<T>::Key>(p0->second)->value;
   }
 
   float t = (time - p0->first) / (float)(p1->first - p0->first); // 0 <= t <= 1
   if constexpr (std::is_same<T, Quatf>::value) {
-    return slerp(static_cast<KeyCurve<T>::Key*>(p0->second)->value, static_cast<KeyCurve<T>::Key*>(p1->second)->value, t);
+    return slerp(std::dynamic_pointer_cast<KeyCurve<T>::Key>(p0->second)->value, std::dynamic_pointer_cast<KeyCurve<T>::Key>(p1->second)->value, t);
   }
   else {
     return p0->second->a + t * p0->second->b + t * t * p0->second->c + t * t * t * p0->second->d;
@@ -413,7 +415,7 @@ void KeyCurve<T>::update_curve(iterator position)
   if constexpr (!std::is_same<T, Quatf>::value) {
 
     float time = (float)position->first;
-    Key* key = static_cast<KeyCurve<T>::Key*>(position->second);
+    std::shared_ptr<Key> key = std::dynamic_pointer_cast<KeyCurve<T>::Key>(position->second);
 
     // First update the tangents
 
@@ -425,8 +427,8 @@ void KeyCurve<T>::update_curve(iterator position)
     if (next != --keys.end() && next != keys.end()) {
       ++next;
     }
-    float next_val = static_cast<KeyCurve<T>::Key*>(next->second)->value;
-    float prev_val = static_cast<KeyCurve<T>::Key*>(prev->second)->value;
+    float next_val = std::dynamic_pointer_cast<KeyCurve<T>::Key>(next->second)->value;
+    float prev_val = std::dynamic_pointer_cast<KeyCurve<T>::Key>(prev->second)->value;
 
     if (!key->in_tangent_lock) {
       switch (key->in_tangent_type) {

math/Quaternion.hpp

@@ -51,6 +51,16 @@ struct Quat
         z = qz;
     }
 
+    //! Copy constructor
+
+    Quat(const Quat& q)
+    {
+        w = q.w;
+        x = q.x;
+        y = q.y;
+        z = q.z;
+    }
+
     //! Constructs a quaternion from the specified array
 
     explicit Quat(const T* q)
@@ -424,6 +434,8 @@ struct Quat
 
     friend Quat slerp(const Quat& p, const Quat& q, const T& t)
     {
+      if (t <= T(0)) return p;
+      if (t >= T(1)) return q;
         T cosRad = dot(unit(p),unit(q));
         T rad = acos(cosRad);
         if (rad < std::numeric_limits<T>::epsilon()) {