Prototype curve for creating collision free paths

data/assets/scene/solarsystem/planets/earth/moon/moon.asset

@@ -165,6 +165,7 @@ local Moon = {
             LabelsColor = {1.0, 1.0, 0.0, 1.0}
         }
     },
+    Tag = { "moon_solarSystem", "moon_giants" },
     GUI = {
         Path = "/Solar System/Planets/Earth/Moon"
     }

modules/autonavigation/CMakeLists.txt

@@ -28,6 +28,7 @@ set(HEADER_FILES
   ${CMAKE_CURRENT_SOURCE_DIR}/atnodenavigator.h
   ${CMAKE_CURRENT_SOURCE_DIR}/autonavigationmodule.h
   ${CMAKE_CURRENT_SOURCE_DIR}/autonavigationhandler.h
+  ${CMAKE_CURRENT_SOURCE_DIR}/avoidcollisioncurve.h
   ${CMAKE_CURRENT_SOURCE_DIR}/helperfunctions.h
   ${CMAKE_CURRENT_SOURCE_DIR}/instruction.h
   ${CMAKE_CURRENT_SOURCE_DIR}/pathspecification.h
@@ -44,6 +45,7 @@ set(SOURCE_FILES
   ${CMAKE_CURRENT_SOURCE_DIR}/autonavigationmodule.cpp
   ${CMAKE_CURRENT_SOURCE_DIR}/autonavigationmodule_lua.inl
   ${CMAKE_CURRENT_SOURCE_DIR}/autonavigationhandler.cpp
+  ${CMAKE_CURRENT_SOURCE_DIR}/avoidcollisioncurve.cpp
   ${CMAKE_CURRENT_SOURCE_DIR}/helperfunctions.cpp
   ${CMAKE_CURRENT_SOURCE_DIR}/instruction.cpp
   ${CMAKE_CURRENT_SOURCE_DIR}/pathspecification.cpp

modules/autonavigation/autonavigationhandler.cpp

@@ -89,6 +89,7 @@ AutoNavigationHandler::AutoNavigationHandler()
     addPropertySubOwner(_atNodeNavigator);
 
     _defaultCurveOption.addOptions({
+        { CurveType::AvoidCollision, "AvoidCollision" },
         { CurveType::Bezier3, "Bezier3" },
         { CurveType::Linear, "Linear" }
     });

modules/autonavigation/avoidcollisioncurve.cpp

@@ -0,0 +1,264 @@
+/*****************************************************************************************
+*                                                                                       *
+* OpenSpace                                                                             *
+*                                                                                       *
+* Copyright (c) 2014-2019                                                               *
+*                                                                                       *
+* Permission is hereby granted, free of charge, to any person obtaining a copy of this  *
+* software and associated documentation files (the "Software"), to deal in the Software *
+* without restriction, including without limitation the rights to use, copy, modify,    *
+* merge, publish, distribute, sublicense, and/or sell copies of the Software, and to    *
+* permit persons to whom the Software is furnished to do so, subject to the following   *
+* conditions:                                                                           *
+*                                                                                       *
+* The above copyright notice and this permission notice shall be included in all copies *
+* or substantial portions of the Software.                                              *
+*                                                                                       *
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,   *
+* INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A         *
+* PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT    *
+* HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF  *
+* CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE  *
+* OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                                         *
+****************************************************************************************/
+
+#include <modules/autonavigation/avoidcollisioncurve.h>
+
+#include <modules/autonavigation/helperfunctions.h>
+#include <openspace/engine/globals.h>
+#include <openspace/rendering/renderengine.h>
+#include <openspace/scene/scene.h>
+#include <openspace/query/query.h>
+#include <ghoul/logging/logmanager.h>
+#include <glm/gtx/intersect.hpp>
+#include <glm/gtx/perpendicular.hpp>
+#include <glm/gtx/projection.hpp>
+#include <algorithm>
+#include <vector>
+
+namespace {
+    constexpr const char* _loggerCat = "AvoidCollisionCurve";
+    const double Epsilon = 1E-7;
+} // namespace
+
+namespace openspace::autonavigation {
+
+AvoidCollisionCurve::AvoidCollisionCurve(const CameraState& start, const CameraState& end) {
+    // default rotation interpolation
+    _rotationInterpolator = RotationInterpolator{ start, end, this, Slerp };
+
+    _points.push_back(start.position);
+    _points.push_back(end.position);
+
+    std::vector<SceneGraphNode*> relevantNodes = findRelevantNodes();
+
+    // Create extra points to avoid collision
+    removeCollisions(relevantNodes);
+
+    int nrSegments = _points.size() - 1;
+
+    // default values for the curve parameter - equally spaced
+    for (double t = 0.0; t <= 1.0; t += 1.0 / (double)nrSegments) {
+        _parameterIntervals.push_back(t);
+    }
+
+    _length = arcLength(1.0);
+
+    initParameterIntervals();
+}
+
+// Interpolate a list of control points and knot times
+glm::dvec3 AvoidCollisionCurve::positionAt(double u) {
+    return interpolatePoints(u);
+}
+
+// TODO: refactor
+bool isRelevant(const SceneGraphNode* node) {
+    // TODO: move this list somewhere else
+    const std::vector<std::string> relevantTags{
+        "planet_solarSystem",
+        "moon_solarSystem"
+        // TODO
+    };
+
+    // does the node match any of the relevant tags?
+    const std::vector<std::string> tags = node->tags();
+    auto result = std::find_first_of(relevantTags.begin(), relevantTags.end(), tags.begin(), tags.end());
+
+    // does not match any tags => not interesting
+    if (result == relevantTags.end()) {
+        return false;
+    }
+
+    return node->renderable() && (node->boundingSphere() > 0.0);
+}
+
+std::vector<SceneGraphNode*> AvoidCollisionCurve::findRelevantNodes() {
+    // TODO: make more efficient
+    const std::vector<SceneGraphNode*>& allNodes =
+        global::renderEngine.scene()->allSceneGraphNodes();
+
+    std::vector<SceneGraphNode*> result{};
+    std::copy_if(allNodes.begin(), allNodes.end(), std::back_inserter(result), isRelevant);
+
+    return result;
+}
+
+void AvoidCollisionCurve::removeCollisions(std::vector<SceneGraphNode*>& relevantNodes, int step) {
+    int nrSegments = _points.size() - 1;
+    int maxSteps = 10;
+
+    // TODO: handle better / present warning if early out
+    if (step > maxSteps) return;
+
+    // go through all segments and check for collisions
+    for (int i = 0; i < nrSegments; ++i) {
+        const glm::dvec3 lineStart = _points[i];
+        const glm::dvec3 lineEnd = _points[i + 1];
+
+        for (SceneGraphNode* node : relevantNodes) {
+            // do collision check in relative coordinates, to avoid huge numbers 
+            const glm::dmat4 modelTransform = node->modelTransform();
+            glm::dvec3 p1 = glm::inverse(modelTransform) * glm::dvec4(lineStart, 1.0);
+            glm::dvec3 p2 = glm::inverse(modelTransform) * glm::dvec4(lineEnd, 1.0);
+
+            // sphere to check for collision
+            double bs = node->boundingSphere();
+            double radius = bs; 
+            // TODO: add a buffer (i.e. sue a little larger sphere), when we have 
+            // behaviour for leaving a target. Don't want to pass too close to objects
+            glm::dvec3 center = glm::dvec3(0.0, 0.0, 0.0);
+            glm::dvec3 point;
+
+            bool collision = helpers::lineSphereIntersection(p1, p2, center, radius, point);
+
+            // convert back to world coordinates 
+            glm::dvec3 pointWorld = modelTransform * glm::dvec4(point, 1.0);
+
+            if (collision) {
+                LINFO(fmt::format("Collision with node: {}!", node->identifier()));
+
+                // to avoid collision, take a step in an orhtogonal direction of the 
+                // collision point and add a new point
+                glm::dvec3 lineDir = glm::normalize(lineEnd - lineStart);
+                glm::dvec3 nodePos = node->worldPosition();
+                glm::dvec3 collisionPointToCenter = nodePos - pointWorld;
+                glm::dvec3 parallell = glm::proj(collisionPointToCenter, lineDir);
+                glm::dvec3 orthogonal = collisionPointToCenter - parallell;
+
+                double distance = 2.0 * radius;
+                glm::dvec3 extraKnot = pointWorld - distance * glm::normalize(orthogonal);
+                _points.insert(_points.begin() + i + 1, extraKnot);
+
+                // TODO: maybe make this more efficient, and make sure that we have a base case. 
+                removeCollisions(relevantNodes, ++step);
+                break;
+            }
+        }
+    }
+}
+
+// compute curve parameter intervals based on relative arc length
+void AvoidCollisionCurve::initParameterIntervals() {
+    std::vector<double> newIntervals;
+    int nrSegments = _points.size() - 1;
+    for (double t = 0.0; t <= 1.0; t += 1.0 / (double)nrSegments) {
+        newIntervals.push_back(arcLength(t) / _length);
+    }
+    _parameterIntervals.swap(newIntervals);
+}
+
+// Catmull-Rom inspired interpolation of the curve points
+glm::dvec3 AvoidCollisionCurve::interpolatePoints(double u)
+{
+    ghoul_assert(_points.size() == _parameterIntervals.size(), 
+        "Must have equal number of points and times!");
+    ghoul_assert(_points.size() > 2, "Minimum of two control points needed for interpolation!");
+
+    size_t nrSegments = _points.size() - 1;
+    const glm::dvec3 start = _points.front();
+    const glm::dvec3 end = _points.back();
+
+    if (u <= 0.0) return _points.front();
+    if (u >= 1.0) return _points.back();
+
+    if (nrSegments == 1) {
+        return roundedSpline(u, start, start, end, end);
+    }
+
+    // compute current segment index
+    std::vector<double>::const_iterator segmentEndIt =
+        std::lower_bound(_parameterIntervals.begin(), _parameterIntervals.end(), u);
+    unsigned int idx = (segmentEndIt - 1) - _parameterIntervals.begin();
+
+    double segmentStart = _parameterIntervals[idx];
+    double segmentDuration = (_parameterIntervals[idx + 1] - _parameterIntervals[idx]);
+    double tScaled = (u - segmentStart) / segmentDuration;
+
+    glm::dvec3 first = (idx == 0) ? start : _points[idx - 1];
+    glm::dvec3 last = (idx == (nrSegments - 1)) ? end : _points[idx + 2];
+
+    return roundedSpline(tScaled, first, _points[idx], _points[idx + 1], last);
+}
+
+glm::dvec3 AvoidCollisionCurve::roundedSpline(double t, const glm::dvec3 &a, 
+              const glm::dvec3 &b, const glm::dvec3 &c, const glm::dvec3 &d)
+{
+    ghoul_assert(t >= 0 && t <= 1.0, "Interpolation variable out of range [0, 1]");
+
+    if (t <= 0.0) return b;
+    if (t >= 1.0) return c;
+
+    auto isNormalizable = [](const glm::dvec3 v) {
+        return !(abs(glm::length(v)) < Epsilon);
+    };
+
+    // find velocities at b and c
+    glm::dvec3 cb = c - b;
+    if (!isNormalizable(cb)) {
+        return b;
+    }
+
+    glm::dvec3 ab = a - b;
+
+    // a and b are the same point. try finding a substitue
+    if (!isNormalizable(ab)) {
+        ab = b - c;
+        if (!isNormalizable(ab)) {
+            ab = glm::dvec3(0.0, 1.0, 0.0);
+        }
+    }
+
+    glm::dvec3 bVelocity = glm::normalize(cb) - glm::normalize(ab);
+    bVelocity = isNormalizable(bVelocity) ?
+        glm::normalize(bVelocity) : glm::dvec3(0.0, 1.0, 0.0);
+
+    glm::dvec3 dc = d - c;
+    // d and c are the same point. try finding a substitue
+    if (!isNormalizable(dc)) {
+        dc = c - b;
+        if (!isNormalizable(dc)) {
+            dc = glm::dvec3(0.0, 1.0, 0.0); 
+        }
+    }
+
+    glm::dvec3 bc = (-1.0)*cb;
+    glm::dvec3 cVelocity = glm::normalize(dc) - glm::normalize(bc);
+    cVelocity = isNormalizable(cVelocity) ?
+        glm::normalize(cVelocity) : glm::dvec3(0.0, 1.0, 0.0);
+
+    double cbDistance = glm::length(cb);
+    double tangetLength = cbDistance;
+
+    // Distances in space can be extremely large, so we dont want to let the tangents have the complete length. 
+    const double tangentlengthThreshold = 1E10; // TODO: What's a good threshold?? ALSO make global
+    if (tangetLength > tangentlengthThreshold)
+        tangetLength = tangentlengthThreshold;
+
+    // the resulting curve gets much better and more predictable when using a genric 
+    // hermite curve compared to the Catmull Rom implementation
+    return interpolation::hermite(t, b, c, bVelocity * tangetLength, cVelocity * tangetLength);
+
+    //return interpolation::catmullRom(t, b - bVelocity * cbDistance, b, c, c + cVelocity * cbDistance);
+}
+} // namespace openspace::autonavigation

modules/autonavigation/avoidcollisioncurve.h

@@ -0,0 +1,50 @@
+/*****************************************************************************************
+ *                                                                                       *
+ * OpenSpace                                                                             *
+ *                                                                                       *
+ * Copyright (c) 2014-2019                                                               *
+ *                                                                                       *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of this  *
+ * software and associated documentation files (the "Software"), to deal in the Software *
+ * without restriction, including without limitation the rights to use, copy, modify,    *
+ * merge, publish, distribute, sublicense, and/or sell copies of the Software, and to    *
+ * permit persons to whom the Software is furnished to do so, subject to the following   *
+ * conditions:                                                                           *
+ *                                                                                       *
+ * The above copyright notice and this permission notice shall be included in all copies *
+ * or substantial portions of the Software.                                              *
+ *                                                                                       *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,   *
+ * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A         *
+ * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT    *
+ * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF  *
+ * CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE  *
+ * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                                         *
+ ****************************************************************************************/
+
+#ifndef __OPENSPACE_MODULE_AUTONAVIGATION___AVOIDCOLLISIONCURVE___H__
+#define __OPENSPACE_MODULE_AUTONAVIGATION___AVOIDCOLLISIONCURVE___H__
+
+#include <modules/autonavigation/pathcurves.h>
+
+namespace openspace::autonavigation {
+
+class AvoidCollisionCurve : public PathCurve {
+public:
+    AvoidCollisionCurve(const CameraState& start, const CameraState& end);
+    glm::dvec3 positionAt(double u);
+
+private:
+    std::vector<double> _parameterIntervals;
+
+    std::vector<SceneGraphNode*> findRelevantNodes();
+    void removeCollisions(std::vector<SceneGraphNode*>& relevantNodes, int step = 0);
+    void initParameterIntervals();
+    glm::dvec3 interpolatePoints(double u);
+    glm::dvec3 roundedSpline(double u, const glm::dvec3 &a, const glm::dvec3 &b,
+                                       const glm::dvec3 &c, const glm::dvec3 &d);
+};
+
+} // namespace openspace::autonavigation
+
+#endif // __OPENSPACE_MODULE_AUTONAVIGATION___AVOIDCOLLISIONCURVE___H__

modules/autonavigation/helperfunctions.cpp

@@ -23,6 +23,12 @@
  ****************************************************************************************/
 
 #include <modules/autonavigation/helperfunctions.h>
+#include <ghoul/logging/logmanager.h>
+
+namespace {
+    constexpr const char* _loggerCat = "Helpers";
+    const double Epsilon = 1E-7;
+} // namespace
 
 namespace openspace::autonavigation::helpers {
 
@@ -39,6 +45,49 @@ namespace openspace::autonavigation::helpers {
         return glm::normalize(glm::inverse(glm::quat_cast(lookAtMat)));
     }
 
+    /*
+       Calculate the intersection of a line and a sphere
+       The line segment is defined from p1 to p2
+       The sphere is of radius r and centered at sc
+       There are potentially two points of intersection given by
+       p = p1 + mu1 (p2 - p1)
+       p = p1 + mu2 (p2 - p1)
+       Source: http://paulbourke.net/geometry/circlesphere/raysphere.c
+    */
+    bool lineSphereIntersection(glm::dvec3 p1, glm::dvec3 p2, glm::dvec3 sc,  double r, 
+                                                         glm::dvec3& intersectionPoint)
+    {   
+        long double a, b, c;
+        glm::dvec3 dp = p2 - p1;
+
+        a = dp.x * dp.x + dp.y * dp.y + dp.z * dp.z;
+        b = 2 * (dp.x * (p1.x - sc.x) + dp.y * (p1.y - sc.y) + dp.z * (p1.z - sc.z));
+        c = sc.x * sc.x + sc.y * sc.y + sc.z * sc.z;
+        c += p1.x * p1.x + p1.y * p1.y + p1.z * p1.z;
+        c -= 2 * (sc.x * p1.x + sc.y * p1.y + sc.z * p1.z);
+        c -= r * r;
+
+        long double intersectionTest = b * b - 4.0 * a * c;
+
+        // no intersection
+        if (std::abs(a) < Epsilon || intersectionTest < 0.0) {
+            return false;
+        }
+        else {
+            // only care about the first intersection point if we have two
+            double t = (-b - std::sqrt(intersectionTest)) / (2.0 *a);
+
+            // outside the line segment?
+            if (t < 0.0 || t > 1.0) return false;
+
+            // TEST
+            LINFO("Collision!! at t = " + std::to_string(t));
+
+            intersectionPoint = p1 + t * dp;
+            return true;
+        }
+    }
+
 } // helpers
 
 namespace openspace::autonavigation::interpolation {
@@ -46,6 +95,8 @@ 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 )
     {
+        ghoul_assert(t >= 0 && t <= 1.0, "Interpolation variable out of range [0, 1]");
+
         double a = 1.0 - t;
         return cp1 * a * a * a
             + cp2 * t * a * a * 3.0
@@ -54,8 +105,102 @@ namespace openspace::autonavigation::interpolation {
     }
 
     glm::dvec3 linear(double t, const glm::dvec3 &cp1, const glm::dvec3 &cp2) {
+        ghoul_assert(t >= 0 && t <= 1.0, "Interpolation variable out of range [0, 1]");
+
         return cp1 * (1.0 - t) + cp2 * t;
     }
 
+    glm::dvec3 catmullRom(double t, const glm::dvec3 &p0, const glm::dvec3 &p1,
+                                    const glm::dvec3 &p2, const glm::dvec3 &p3)
+    {
+        ghoul_assert(t >= 0 && t <= 1.0, "Interpolation variable out of range [0, 1]");
+
+        if (t <= 0.0) return p1;
+        if (t >= 1.0) return p2;
+
+        const double t2 = t * t; 
+        const double t3 = t2 * t;
+
+        glm::dvec3 result = (
+            p0 * (-0.5 * t3 + t2 - 0.5 * t) +
+            p1 * ( 1.5 * t3 - 2.5 * t2 + 1.0) +
+            p2 * (-1.5 * t3 + 2.0 * t2 + 0.5 * t) +
+            p3 * ( 0.5 * t3 - 0.5 * t2)
+            );
+
+        return result;
+    }
+
+    glm::dvec3 hermite(double t, const glm::dvec3 &p1, const glm::dvec3 &p2,
+                       const glm::dvec3 &tangent1, const glm::dvec3 &tangent2)
+    {
+        ghoul_assert(t >= 0 && t <= 1.0, "Interpolation variable out of range [0, 1]");
+
+        if (t <= 0.0) return p1;
+        if (t >= 1.0) return p2;
+
+        const double t2 = t * t;
+        const double t3 = t2 * t;
+
+        // calculate basis functions
+        double const a0 = (2.0*t3) - (3.0*t2) + 1.0;
+        double const a1 = (-2.0*t3) + (3.0*t2);
+        double const b0 = t3 - (2.0*t2) + t;
+        double const b1 = t3 - t2;
+
+        glm::dvec3 result = (a0 * p1) + (a1 * p2) + (b0 * tangent1) + (b1 * tangent2);
+        return result;
+    }
+
+    // uniform if tKnots are equally spaced, or else non uniform
+    glm::dvec3 piecewiseCubicBezier(double t, const std::vector<glm::dvec3>& points, 
+                                                  const std::vector<double>& tKnots) 
+    {
+        ghoul_assert(points.size() > 4, "Minimum of four control points needed for interpolation!");
+        ghoul_assert((points.size() - 1) % 3 == 0, "A vector containing 3n + 1 control points must be provided!");
+        ghoul_assert(_nrSegments == (tKnots.size() - 1), "Number of interval times must match number of segments");
+
+        if (t <= 0.0) return points.front();
+        if (t >= 1.0) return points.back();
+
+        // compute current segment index
+        std::vector<double>::const_iterator segmentEndIt =
+            std::lower_bound(tKnots.begin(), tKnots.end(), t);
+        unsigned int segmentIdx = (segmentEndIt - 1) - tKnots.begin();
+
+        double segmentStart = tKnots[segmentIdx];
+        double segmentDuration = (tKnots[segmentIdx + 1] - tKnots[segmentIdx]);
+        double tScaled = (t - segmentStart) / segmentDuration;
+
+        unsigned int idx = segmentIdx * 3;
+
+        // Interpolate using De Casteljau's algorithm
+        return interpolation::cubicBezier(tScaled, points[idx], points[idx + 1],
+            points[idx + 2], points[idx + 3]);
+    }
+
+    glm::dvec3 piecewiseLinear(double t, const std::vector<glm::dvec3>& points, 
+                                             const std::vector<double>& tKnots)
+    {
+        ghoul_assert(points.size() == tKnots.size(), "Must have equal number of points and times!");
+        ghoul_assert(points.size() > 2, "Minimum of two control points needed for interpolation!");
+
+        size_t nrSegments = points.size() - 1;
+
+        if (t <= 0.0) return points.front();
+        if (t >= 1.0) return points.back();
+
+        // compute current segment index
+        std::vector<double>::const_iterator segmentEndIt =
+            std::lower_bound(tKnots.begin(), tKnots.end(), t);
+        unsigned int idx = (segmentEndIt - 1) - tKnots.begin();
+
+        double segmentStart = tKnots[idx];
+        double segmentDuration = (tKnots[idx + 1] - tKnots[idx]);
+        double tScaled = (t - segmentStart) / segmentDuration;
+
+        return interpolation::linear(tScaled, points[idx], points[idx + 1]);
+    }
+
 } // interpolation
 

modules/autonavigation/helperfunctions.h

@@ -39,6 +39,9 @@ namespace openspace::autonavigation::helpers {
 
     glm::dquat getLookAtQuaternion(glm::dvec3 eye, glm::dvec3 center, glm::dvec3 up);
 
+    bool lineSphereIntersection(glm::dvec3 linePoint1, glm::dvec3 linePoint2,
+        glm::dvec3 sphereCenter, double spehereRadius, glm::dvec3& intersectionPoint);
+
 } // namespace
 
 namespace openspace::autonavigation::interpolation {
@@ -52,5 +55,17 @@ namespace openspace::autonavigation::interpolation {
 
     glm::dvec3 linear(double t, const glm::dvec3 &cp1, const glm::dvec3 &cp2);
 
+    glm::dvec3 catmullRom(double t, const glm::dvec3 &a, const glm::dvec3 &b,
+                                    const glm::dvec3 &c, const glm::dvec3 &d);
+
+    glm::dvec3 hermite(double t, const glm::dvec3 &cp1, const glm::dvec3 &cp2,
+                       const glm::dvec3 &tangent1, const glm::dvec3 &tangent2);
+
+    glm::dvec3 piecewiseCubicBezier(double t, const std::vector<glm::dvec3>& points,
+                                                  const std::vector<double>& tKnots);
+
+    glm::dvec3 piecewiseLinear(double t, const std::vector<glm::dvec3>& points, 
+                                             const std::vector<double>& tKnots);
+
 } // namespace
 #endif

modules/autonavigation/pathcurves.cpp

@@ -25,10 +25,16 @@
 #include <modules/autonavigation/pathcurves.h>
 
 #include <modules/autonavigation/helperfunctions.h>
+#include <openspace/engine/globals.h>
+#include <openspace/rendering/renderengine.h>
+#include <openspace/scene/scene.h>
 #include <openspace/query/query.h>
-#include <openspace/scene/scenegraphnode.h>
 #include <ghoul/logging/logmanager.h>
+#include <glm/gtx/intersect.hpp>
+#include <glm/gtx/perpendicular.hpp>
 #include <glm/gtx/projection.hpp>
+#include <algorithm>
+#include <vector>
 
 namespace {
     constexpr const char* _loggerCat = "PathCurve";
@@ -165,35 +171,7 @@ Bezier3Curve::Bezier3Curve(const Waypoint& start, const Waypoint& end) {
 
 // Interpolate a list of control points and knot times
 glm::dvec3 Bezier3Curve::positionAt(double u) {
-    ghoul_assert(u >= 0 && u <= 1.0, "Interpolation variable out of range [0, 1]");
-
-    size_t nrPoints = _points.size();
-    size_t nrTimes = _parameterIntervals.size();
-
-    ghoul_assert(nrPoints > 4, "Minimum of four control points needed for interpolation!");
-    ghoul_assert((nrPoints - 1) % 3 == 0, "A vector containing 3n + 1 control points must be provided!");
-    ghoul_assert(_nrSegments == (nrTimes - 1), "Number of interval times must match number of intervals");
-
-    if (abs(u) < Epsilon)
-        return _points.front();
-
-    if (abs(1.0 - u) < Epsilon)
-        return _points.back();
-
-    // compute current segment, by first finding iterator to the first value that is larger than s 
-    std::vector<double>::iterator segmentEndIt = 
-        std::lower_bound(_parameterIntervals.begin(), _parameterIntervals.end(), u);
-    unsigned int segmentIdx = (segmentEndIt - 1) - _parameterIntervals.begin();
-
-    double segmentStart = _parameterIntervals[segmentIdx];
-    double segmentDuration = (_parameterIntervals[segmentIdx + 1] - _parameterIntervals[segmentIdx]);
-    double sScaled = (u - segmentStart) / segmentDuration;
-
-    unsigned int idx = segmentIdx * 3;
-
-    // Interpolate using De Casteljau's algorithm
-    return interpolation::cubicBezier(sScaled, _points[idx], _points[idx + 1],
-        _points[idx + 2], _points[idx + 3]);
+    return interpolation::piecewiseCubicBezier(u, _points, _parameterIntervals);
 }
 
 // compute curve parameter intervals based on relative arc length

modules/autonavigation/pathcurves.h

@@ -33,6 +33,7 @@
 namespace openspace::autonavigation {
 
 enum CurveType {
+    AvoidCollision,
     Bezier3,
     Linear,
 };
@@ -54,6 +55,8 @@ protected:
     double _length; // the total length of the curve (approximated)
 };
 
+// TODO: Put path curve classes in separate files
+
 class Bezier3Curve : public PathCurve {
 public:
     Bezier3Curve(const Waypoint& start, const Waypoint& end);

modules/autonavigation/pathsegment.cpp

@@ -24,6 +24,7 @@
 
 #include <modules/autonavigation/pathsegment.h>
 
+#include <modules/autonavigation/avoidcollisioncurve.h>
 #include <modules/autonavigation/pathcurves.h>
 #include <openspace/engine/globals.h>
 #include <openspace/scene/scenegraphnode.h>
@@ -123,6 +124,10 @@ void PathSegment::initCurve() {
 
     switch (_curveType) 
     {
+    case CurveType::AvoidCollision:
+        _curve = std::make_unique<AvoidCollisionCurve>(_start, _end);
+        break;
+
     case CurveType::Bezier3:
         _curve = std::make_unique<Bezier3Curve>(_start, _end);
         _rotationInterpolator = std::make_unique<LookAtInterpolator>(