Merge from main, fixed collision in helperfunctions.cpp

modules/autonavigation/helperfunctions.cpp

@@ -33,8 +33,8 @@ namespace {
 namespace openspace::autonavigation::helpers {
 
     // Shift and scale to a subinterval [start,end]
-    double shiftAndScale(double t, double start, double end) { 
-        ghoul_assert(0.0 < start && start < end  && end < 1.0, 
+    double shiftAndScale(double t, double start, double end) {
+        ghoul_assert(0.0 < start && start < end&& end < 1.0,
             "Values must be 0.0 < start < end < 1.0!");
         double tScaled = t / (end - start) - start;
         return std::max(0.0, std::min(tScaled, 1.0));
@@ -91,8 +91,44 @@ namespace openspace::autonavigation::helpers {
 
 namespace openspace::autonavigation::interpolation {
 
-    glm::dvec3 cubicBezier(double t, const glm::dvec3 &cp1, const glm::dvec3 &cp2,
-                                     const glm::dvec3 &cp3, const glm::dvec3 &cp4 )
+    // Based on implementation by Mika Rantanen https://qroph.github.io/2018/07/30/smooth-paths-using-catmull-rom-splines.html
+    glm::dvec3 catmullRom(double t, const glm::dvec3& p0, const glm::dvec3& p1,
+        const glm::dvec3& p2, const glm::dvec3& p3, double alpha)
+    {
+        const double Epsilon = 1E-7;
+        glm::dvec3 m01, m02, m23, m13;
+
+        double t01 = pow(glm::distance(p0, p1), alpha);
+        double t12 = pow(glm::distance(p1, p2), alpha);
+        double t23 = pow(glm::distance(p2, p3), alpha);
+
+        // Prevent zero division
+        (t01 < Epsilon) ?
+            m01 = glm::dvec3{} : m01 = (p1 - p0) / t01;
+        (t23 < Epsilon) ?
+            m23 = glm::dvec3{} : m23 = (p3 - p2) / t23;
+        (t01 + t12 < Epsilon) ?
+            m02 = glm::dvec3{} : m02 = (p2 - p0) / (t01 + t12);
+        (t12 + t23 < Epsilon) ?
+            m13 = glm::dvec3{} : m13 = (p3 - p1) / (t12 + t23);
+
+        glm::dvec3 m1 = p2 - p1 + t12 * (m01 - m02);
+        glm::dvec3 m2 = p2 - p1 + t12 * (m23 - m13);
+
+        glm::dvec3 a = 2.0 * (p1 - p2) + m1 + m2;
+        glm::dvec3 b = -3.0 * (p1 - p2) - m1 - m1 - m2;
+        glm::dvec3 c = m1;
+        glm::dvec3 d = p1;
+
+        return
+            a * t * t * t +
+            b * t * t +
+            c * t +
+            d;
+    }
+
+    glm::dvec3 cubicBezier(double t, const glm::dvec3& cp1, const glm::dvec3& cp2,
+        const glm::dvec3& cp3, const glm::dvec3& cp4)
     {
         ghoul_assert(t >= 0 && t <= 1.0, "Interpolation variable out of range [0, 1]");
 
@@ -182,4 +218,3 @@ namespace openspace::autonavigation::interpolation {
     }
 
 } // interpolation
-

modules/autonavigation/helperfunctions.h

@@ -50,10 +50,15 @@ namespace openspace::autonavigation::interpolation {
     // Alternatively, add cubicBezier interpolation in ghoul and only use
     // ghoul's interpolator methods
 
-    glm::dvec3 cubicBezier(double t, const glm::dvec3 &cp1, const glm::dvec3 &cp2, 
-                                     const glm::dvec3 &cp3, const glm::dvec3 &cp4);
+    // Centripetal version alpha = 0, uniform for alpha = 0.5 and chordal for alpha = 1
+    glm::dvec3 catmullRom(double t, const glm::dvec3& p0, const glm::dvec3& p1,
+        const glm::dvec3& p2, const glm::dvec3& p3, double alpha = 0.5);
 
-    glm::dvec3 linear(double t, const glm::dvec3 &cp1, const glm::dvec3 &cp2);
+
+    glm::dvec3 cubicBezier(double t, const glm::dvec3& cp1, const glm::dvec3& cp2,
+        const glm::dvec3& cp3, const glm::dvec3& cp4);
+
+    glm::dvec3 linear(double t, const glm::dvec3& cp1, const glm::dvec3& cp2);
 
     glm::dvec3 hermite(double t, const glm::dvec3 &cp1, const glm::dvec3 &cp2,
                        const glm::dvec3 &tangent1, const glm::dvec3 &tangent2);

modules/autonavigation/pathsegment.cpp

@@ -74,6 +74,10 @@ const std::vector<glm::dvec3> PathSegment::getControlPoints() const {
 }
 
 CameraPose PathSegment::traversePath(double dt) {
+    if (!_curve || !_rotationInterpolator || !_speedFunction) {
+        LERROR("Cannot traverse path. Curvec type has not been properly defined.");
+    }
+
     // compute displacement along the path during this frame
     double displacement = 0.0;
     int steps = 2; // TODO: should possibly increase with larger risk of error

modules/autonavigation/rotationinterpolator.cpp

@@ -58,8 +58,6 @@ LookAtInterpolator::LookAtInterpolator(glm::dquat start, glm::dquat end,
     _path(path)
 {}
 
-// Look at start node until tStart, then turn to look at end node from tEnd.
-// OBS! Does not care about actual end and start value!!
 glm::dquat LookAtInterpolator::interpolate(double u) {
     double tStart = 0.15;
     double tEnd = 0.7;
@@ -72,60 +70,4 @@ glm::dquat LookAtInterpolator::interpolate(double u) {
     return helpers::getLookAtQuaternion(_path->positionAt(u), lookAtPos, startUpVec);
 }
 
-PiecewiseLookAtInterpolator::PiecewiseLookAtInterpolator(glm::dquat start, glm::dquat end, 
-                                                         glm::dvec3 startTargetPos, 
-                                                         glm::dvec3 endTargetPos, 
-                                                         PathCurve* path)
-    : RotationInterpolator(start, end),
-    _startTargetPos(startTargetPos),
-    _endTargetPos(endTargetPos),
-    _path(path)
-{}
-
-// Interpolate between a number of keyframes for orientation using SLERP
-glm::dquat PiecewiseLookAtInterpolator::interpolate(double u) {
-    ghoul_assert(_curve, "Rotation interpolation requires access to curve positions.");
-
-    // breakpoints for subintervals
-    const double u1 = 0.3;
-    const double u2 = 0.8; // TODO: these should probably be based on distance
-
-    glm::dvec3 startUpVec = _start * glm::dvec3(0.0, 1.0, 0.0);
-    glm::dvec3 endUpVec = _end * glm::dvec3(0.0, 1.0, 0.0);
-
-    glm::dquat lookAtStartQ = helpers::getLookAtQuaternion(
-        _path->positionAt(u1), 
-        _startTargetPos, 
-        startUpVec
-    );
-
-    glm::dquat lookAtEndQ = helpers::getLookAtQuaternion(
-        _path->positionAt(u2), 
-        _endTargetPos, 
-        endUpVec
-    );
-
-    std::vector<std::pair<glm::dquat, double>> keyframes{
-        {_start, 0.0},
-        {lookAtStartQ, u1},
-        {lookAtEndQ, u2},
-        {_end, 1.0}
-    };
-
-    // Find the current segment and compute interpolation
-    glm::dquat result;
-    for (int i = 0; i < keyframes.size() - 1; ++i) {
-        double ui = keyframes[i].second;
-        double uNext = keyframes[i + 1].second;
-        if (u <= uNext) {
-            double uScaled = (u - ui) / (uNext - ui);
-            uScaled = ghoul::quadraticEaseInOut(uScaled);
-            result = glm::slerp(keyframes[i].first, keyframes[i + 1].first, uScaled);
-            break;
-        }
-    }
-
-    return result;
-}
-
 } // namespace openspace::autonavigation

modules/autonavigation/rotationinterpolator.h

@@ -49,6 +49,8 @@ public:
     glm::dquat interpolate(double u);
 };
 
+// Look at start node until tStart, then turn to look at end node from tEnd.
+// OBS! Does not care about actual end and start value!! I.e. Not an interpolation!
 class LookAtInterpolator : public RotationInterpolator {
 public:
     LookAtInterpolator(glm::dquat start, glm::dquat end, glm::dvec3 startLookAtPos,
@@ -61,18 +63,6 @@ private:
     PathCurve* _path = nullptr;
 };
 
-class PiecewiseLookAtInterpolator : public RotationInterpolator {
-public:
-    PiecewiseLookAtInterpolator(glm::dquat start, glm::dquat end,
-        glm::dvec3 startTargetPos, glm::dvec3 endTargetPos, PathCurve* path);
-    glm::dquat interpolate(double u);
-
-private:
-    glm::dvec3 _startTargetPos;
-    glm::dvec3 _endTargetPos;
-    PathCurve* _path = nullptr;
-};
-
 } // namespace openspace::autonavigation
 
 #endif // __OPENSPACE_MODULE_AUTONAVIGATION___ROTATIONINTERPOLATOR___H__