From aef495a39408e91464d7a18bdc3c8f507e23baf0 Mon Sep 17 00:00:00 2001
From: karl <karl.bittner@hexaquo.at>
Date: Fri, 2 Oct 2020 22:27:12 +0200
Subject: [PATCH] Add Quaternion interpolation

glm made this quite easy!
---
 ECS/Systems/PathMoveSystem.h | 30 ++++++++++++++++++++++++++++--
 main.cpp                     |  6 +++---
 2 files changed, 31 insertions(+), 5 deletions(-)

diff --git a/ECS/Systems/PathMoveSystem.h b/ECS/Systems/PathMoveSystem.h
index 6870a8e..3ef48c1 100644
--- a/ECS/Systems/PathMoveSystem.h
+++ b/ECS/Systems/PathMoveSystem.h
@@ -112,7 +112,6 @@ class PathMoveSystem : public EntitySystem, public EventSubscriber<InputEvent> {
                         p3 = p2 + interp_direction * 2.0f;
                     } else {
                         // We're fine - use the point after the target for p3
-                        p2 = path.points[pathmove->current_point_index + 1];
                         p3 = path.points[pathmove->current_point_index + 2];
                     }
 
@@ -140,7 +139,34 @@ class PathMoveSystem : public EntitySystem, public EventSubscriber<InputEvent> {
                     // Rotation
                     // https://www.3dgep.com/understanding-quaternions/#SQUAD
                     PathMove::Views views = pathmove->views;
-                    transform->set_rotation_from_quat(views.views[pathmove->current_point_index]);
+
+                    // Similar procedure as with position to get the relevant values
+                    glm::quat q0;
+                    glm::quat q1 = views.views[pathmove->current_point_index];
+                    glm::quat q2 = views.views[pathmove->current_point_index + 1];
+                    glm::quat q3;
+
+                    if (pathmove->current_point_index == num_points - 2) {
+                        // Interpolate what q3 would be if the change from q1 to q2 continues
+                        q3 = glm::fastMix(q1, q2, 2.0f);
+                    } else {
+                        // We're fine - use the point after the target for p3
+                        q3 = views.views[pathmove->current_point_index + 2];
+                    }
+
+                    if (pathmove->current_point_index == 0) {
+                        // Interpolate what q0 would be if the same change happened from q0 to q1 as from q1 to q2
+                        q0 = glm::fastMix(q1, q2, -1.0f);
+                    } else {
+                        // We're fine - use the point before the current point
+                        q0 = views.views[pathmove->current_point_index - 1];
+                    }
+
+                    // Interpolate
+                    glm::quat result = glm::squad(q1, q2, glm::intermediate(q0, q1, q2), glm::intermediate(q1, q2, q3), pathmove->time_passed);
+
+                    // Apply
+                    transform->set_rotation_from_quat(result);
                 });
     }
 
diff --git a/main.cpp b/main.cpp
index c430998..5b16558 100644
--- a/main.cpp
+++ b/main.cpp
@@ -101,12 +101,12 @@ int main() {
             glm::vec3(0.0, 2.0, -10.0)
         }),
         PathMove::Views(std::vector<glm::quat>{
-            glm::quat(0.0, 0.0, 0.0, 0.0),
-            glm::angleAxis(glm::radians(-10.f), glm::vec3(0.f, 1.f, 0.f)),
+            glm::angleAxis(glm::radians(0.f), glm::vec3(0.f, 1.f, 0.f)),
             glm::angleAxis(glm::radians(10.f), glm::vec3(0.f, 1.f, 0.f)),
+            glm::angleAxis(glm::radians(20.f), glm::vec3(0.f, 1.f, 0.f)),
             glm::angleAxis(glm::radians(40.f), glm::vec3(0.f, 1.f, 0.f)),
             glm::angleAxis(glm::radians(90.f), glm::vec3(0.f, 1.f, 0.f)),
-            glm::angleAxis(glm::radians(95.f), glm::vec3(0.f, 1.f, 0.f)),
+            glm::angleAxis(glm::radians(120.f), glm::vec3(0.f, 1.f, 0.f)),
             glm::angleAxis(glm::radians(180.f), glm::vec3(0.f, 1.f, 0.f))
         })
     );