Reparameterization of Bezier3 now works

modules/autonavigation/helperfunctions.cpp

@@ -58,36 +58,8 @@ namespace openspace::autonavigation::interpolation {
     glm::dvec3 linear(double t, const glm::dvec3 &cp1, const glm::dvec3 &cp2) {
         return cp1 * (1.0 - t) + cp2 * t;
     }
-    /*
-    // Interpolate a list of control points and knot times
-    glm::dvec3 piecewiseCubicBezier(double t, std::vector<double> &times, const std::vector<glm::dvec3> &points) {
-        size_t n = points.size();
-        ghoul_assert(n > 4, "Minimum of four control points needed for interpolation!");
-        ghoul_assert((n - 1) % 3 == 0, "A vector containing 3n + 1 control points must be provided!");
-
-        size_t nrSegments = (size_t)std::floor((n - 1) / 3.0);
-        int nrTimes = times.size();
-        ghoul_assert(nrSegments == nrTimes - 1, "Number of interval times must match number of intervals");
-        
-        if (abs(1.0-t) < 0.00001) 
-            return points.back();
-        if (nrTimes < 1) {
-            return points[0];
-        }
-   
-        // find current segmant and scale time
-        std::vector<double>::iterator low = std::lower_bound(times.begin(), times.end(), t);
-        int segmentIdx = low - times.begin(); 
-
-        double tSegment = (t - times[segmentIdx]) / (times[segmentIdx + 1] - times[segmentIdx]);
-       
-        int idx = segmentIdx * 3;
-
-        return cubicBezier(tSegment, points[idx], points[idx + 1], 
-            points[idx + 2], points[idx + 3]);
-    }
-    */
 
+    // TODO: remove
     glm::dvec3 piecewiseLinear(double t, const std::vector<glm::dvec3> &points) {
         size_t n = points.size();
         ghoul_assert(n > 2, "Minimum of two control points needed for interpolation!");

modules/autonavigation/helperfunctions.h

@@ -53,9 +53,7 @@ namespace openspace::autonavigation::interpolation {
 
     glm::dvec3 linear(double t, const glm::dvec3 &cp1, const glm::dvec3 &cp2);
 
-    // Inside bezier3
-    // glm::dvec3 piecewiseCubicBezier(double t, std::vector<double> &times, const std::vector<glm::dvec3> &points);
-
+    // TODO: remove
     glm::dvec3 piecewiseLinear(double t, const std::vector<glm::dvec3> &controlPoints);
 
 } // namespace

modules/autonavigation/pathcurves.cpp

@@ -141,42 +141,37 @@ Bezier3Curve::Bezier3Curve(CameraState& start, CameraState& end) {
        
     reparameterizeByArcLength();
 }
-/*
-glm::dvec3 Bezier3Curve::valueAt(double t) {
-    return interpolation::piecewiseCubicBezier(t, _intervalTimes, _points);
-}
-*/
 
 // Interpolate a list of control points and knot times
 glm::dvec3 Bezier3Curve::valueAt(double t) {
-    size_t n = _points.size();
-    ghoul_assert(n > 4, "Minimum of four control points needed for interpolation!");
-    ghoul_assert((n - 1) % 3 == 0, "A vector containing 3n + 1 control points must be provided!");
+    size_t nrPoints = _points.size();
+    size_t nrTimes = _intervalTimes.size();
+    unsigned int nrSegments = (unsigned int)std::floor((nrPoints - 1) / 3.0);
 
-    size_t nrSegments = (size_t)std::floor((n - 1) / 3.0);
-    int nrTimes = _intervalTimes.size();
-    ghoul_assert(nrSegments == nrTimes - 1, "Number of interval times must match number of intervals");
+    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(1.0 - t) < 0.00001)
-        return _points.back();
-    if (nrTimes < 1) {
-        return _points[0];
-    }
+    if (abs(t) < 0.000001)
+        return _points.front();
 
-    // find current segmant and scale time
-    std::vector<double>::iterator low = std::lower_bound(_intervalTimes.begin(), _intervalTimes.end(), t);
-    int segmentIdx = low - _intervalTimes.begin();
+    // compute current segment, by first finding iterator to the first time that is larger than t 
+    std::vector<double>::iterator segmentEndIt = 
+        std::lower_bound(_intervalTimes.begin(), _intervalTimes.end(), t);
+    unsigned int segmentIdx = (segmentEndIt - 1) - _intervalTimes.begin();
 
-    double tSegment = (t - _intervalTimes[segmentIdx]) / (_intervalTimes[segmentIdx + 1] - _intervalTimes[segmentIdx]);
+    double segmentStart = _intervalTimes[segmentIdx];
+    double segmentDuration = (_intervalTimes[segmentIdx + 1] - _intervalTimes[segmentIdx]);
+    double tSegment = (t - segmentStart) / segmentDuration;
 
-    int idx = segmentIdx * 3;
+    unsigned int idx = segmentIdx * 3;
 
     return interpolation::cubicBezier(tSegment, _points[idx], _points[idx + 1],
         _points[idx + 2], _points[idx + 3]);
 }
 
 void Bezier3Curve::reparameterizeByArcLength() {
-    // For bezier every third is a knot shaed beetween segments
+    // For bezier every third is a knot shared beetween segments
     int nrIntervals = (int)std::floor((float)(_points.size() - 1) / 3.0); //TODO valivate!
 
     if (nrIntervals == 1) {

modules/autonavigation/pathsegment.cpp

@@ -76,7 +76,8 @@ const std::vector<glm::dvec3> PathSegment::getControlPoints() const {
 }
 
 glm::dvec3 PathSegment::getPositionAt(double t) const {
-    t = ghoul::cubicEaseInOut(t); 
+    //t = ghoul::cubicEaseInOut(t); 
+    // TODO: move along curve based on displacement instead
     return _curve->valueAt(t);    
 }