From 4ff17ae0788b4a048a2aa0bb9928bd97a58152f1 Mon Sep 17 00:00:00 2001
From: Emma Broman <emma95.broman@hotmail.com>
Date: Wed, 29 Apr 2020 13:50:14 +0200
Subject: [PATCH] Improved look at rotation interpolation

---
 modules/autonavigation/pathsegment.cpp        |  2 +-
 .../autonavigation/rotationinterpolator.cpp   | 54 +++++++++++++++++++
 modules/autonavigation/rotationinterpolator.h | 14 +++++
 3 files changed, 69 insertions(+), 1 deletion(-)

diff --git a/modules/autonavigation/pathsegment.cpp b/modules/autonavigation/pathsegment.cpp
index 3db2c85ac6..213507f82e 100644
--- a/modules/autonavigation/pathsegment.cpp
+++ b/modules/autonavigation/pathsegment.cpp
@@ -129,7 +129,7 @@ void PathSegment::initCurve() {
     {
     case CurveType::Bezier3:
         _curve = std::make_unique<Bezier3Curve>(_start, _end);
-        _rotationInterpolator = std::make_unique<LookAtRotator>(
+        _rotationInterpolator = std::make_unique<LookAtInterpolator>(
             _start.rotation(),
             _end.rotation(),
             _start.node()->worldPosition(),
diff --git a/modules/autonavigation/rotationinterpolator.cpp b/modules/autonavigation/rotationinterpolator.cpp
index a6812c20e9..3ecdb5afc3 100644
--- a/modules/autonavigation/rotationinterpolator.cpp
+++ b/modules/autonavigation/rotationinterpolator.cpp
@@ -27,6 +27,7 @@
 #include <modules/autonavigation/helperfunctions.h>
 #include <modules/autonavigation/waypoint.h>
 #include <ghoul/logging/logmanager.h>
+#include <ghoul/misc/interpolator.h>
 
 namespace {
     constexpr const char* _loggerCat = "RotationInterpolator";
@@ -70,4 +71,57 @@ glm::dquat LookAtRotator::interpolate(double u) {
     return helpers::getLookAtQuaternion(_path->positionAt(u), lookAtPos, startUpVec);
 }
 
+LookAtInterpolator::LookAtInterpolator(glm::dquat start, glm::dquat end,
+                                       glm::dvec3 startLookAtPos,
+                                       glm::dvec3 endLookAtPos,
+                                       PathCurve* path)
+    : RotationInterpolator(start, end),
+    _startLookAtPos(startLookAtPos),
+    _endLookAtPos(endLookAtPos),
+    _path(path)
+{}
+
+// Turn towards start node, turn towards end node, then turn to end view
+glm::dquat LookAtInterpolator::interpolate(double u) {
+    //TODO: base on curve position?
+    double u1 = 0.2;
+    double u2 = 0.8;
+
+    glm::dvec3 startPosition = _path->positionAt(0.0);
+    glm::dvec3 endPosition = _path->positionAt(1.0);
+
+    glm::dvec3 lookAtPos;
+    if (u < u1) {
+        // Create aim positions not too close to camera based on view direction
+        glm::dvec3 startViewDir = glm::normalize(_start * glm::dvec3(0.0, 0.0, -1.0));
+        double startViewDist = glm::length(startPosition - _startLookAtPos);
+        glm::dvec3 startViewPos = startPosition + startViewDist * startViewDir;
+        double uNew = u / u1;
+        uNew = ghoul::cubicEaseInOut(uNew);
+        lookAtPos = interpolation::linear(uNew, startViewPos, _startLookAtPos);
+    }
+    else if (u <= u2) {
+        double uNew = (u - u1) / (u2 - u1);
+        uNew = ghoul::cubicEaseInOut(uNew);
+        lookAtPos = interpolation::linear(uNew, _startLookAtPos, _endLookAtPos);
+    }
+    else if (u2 < u) {
+        glm::dvec3 endViewDir = glm::normalize(_end * glm::dvec3(0.0, 0.0, -1.0));
+        double endViewDist = glm::length(endPosition - _endLookAtPos);
+        glm::dvec3 endViewPos = endPosition + endViewDist * endViewDir;
+        double uNew = (u - u2) / (1.0 - u2);
+        uNew = ghoul::cubicEaseInOut(uNew);
+        lookAtPos = interpolation::linear(uNew, _endLookAtPos, endViewPos);
+    }
+
+    // handle up vector
+    glm::dvec3 startUp = _start * glm::dvec3(0.0, 1.0, 0.0); 
+    glm::dvec3 endUp = _end * glm::dvec3(0.0, 1.0, 0.0);
+
+    double uUp = ghoul::cubicEaseInOut(u);
+    glm::dvec3 up = ghoul::interpolateLinear(uUp, startUp, endUp);
+
+    return helpers::getLookAtQuaternion(_path->positionAt(u), lookAtPos, up);
+}
+
 } // namespace openspace::autonavigation
diff --git a/modules/autonavigation/rotationinterpolator.h b/modules/autonavigation/rotationinterpolator.h
index bf5e1af13f..87abf76783 100644
--- a/modules/autonavigation/rotationinterpolator.h
+++ b/modules/autonavigation/rotationinterpolator.h
@@ -63,6 +63,20 @@ private:
     PathCurve* _path = nullptr;
 };
 
+// Interpolates a look at position for the camera, and takes the start and end rotation
+// into account
+class LookAtInterpolator : public RotationInterpolator {
+public:
+    LookAtInterpolator(glm::dquat start, glm::dquat end, glm::dvec3 startLookAtPos,
+        glm::dvec3 endLookAtPos, PathCurve* path);
+    glm::dquat interpolate(double u);
+
+private:
+    glm::dvec3 _startLookAtPos;
+    glm::dvec3 _endLookAtPos;
+    PathCurve* _path = nullptr;
+};
+
 } // namespace openspace::autonavigation
 
 #endif // __OPENSPACE_MODULE_AUTONAVIGATION___ROTATIONINTERPOLATOR___H__