Merge branch 'camera-paths/curve-parameter-bug' into thesis/2019/camera-paths

modules/autonavigation/pathcurves.cpp

@@ -40,32 +40,69 @@ namespace openspace::autonavigation {
 PathCurve::~PathCurve() {}
 
 const double PathCurve::length() const {
-    return _length;
+    return _totalLength;
 }
 
-// Approximate the curve length using Simpson's rule
-double PathCurve::arcLength(double limit) {
-    const int n = 30; // resolution, must be even for Simpson's rule
-    const double h = limit / (double)n;
+glm::dvec3 PathCurve::positionAt(double relativeLength) {
+    double u = curveParameter(relativeLength * _totalLength); // TODO: only use relative length?
+    return interpolate(u);
+}
 
-    // Points to be multiplied by 1
-    double endPoints = glm::length(positionAt(0.0 + h) - positionAt(0.0)) + glm::length(positionAt(1.0) - positionAt(1.0 - h));
+// Compute the curve parameter from an arc length value, using a combination of
+// Newton's method and bisection. Source:
+// https://www.geometrictools.com/Documentation/MovingAlongCurveSpecifiedSpeed.pdf
+// Input s is a length value, in the range [0, _length]
+// Returns curve parameter in range [0, 1]
+double PathCurve::curveParameter(double s) {
+    if (s <= Epsilon) return 0.0;
+    if (s >= _totalLength) return 1.0; 
 
-    // Points to be multiplied by 4
-    double times4 = 0.0;
-    for (int i = 1; i < n; i += 2) {
-        double t = h * i;
-        times4 += glm::length(positionAt(t + h) - positionAt(t));
+    unsigned int segmentIndex;
+    for (segmentIndex = 1; segmentIndex < _nrSegments; ++segmentIndex) {
+        if (s <= _lengthSums[segmentIndex]) 
+            break;
     }
 
-    // Points to be multiplied by 2
-    double times2 = 0.0;
-    for (int i = 2; i < n; i += 2) {
-        double t = h * i;
-        times2 += glm::length(positionAt(t + h) - positionAt(t));
+    // initial guess for Newton's method
+    double segmentS = s - _lengthSums[segmentIndex - 1];
+    double segmentLength = _lengths[segmentIndex];
+
+    const double uMin = _parameterIntervals[segmentIndex - 1];
+    const double uMax = _parameterIntervals[segmentIndex];
+    double u = uMin + (uMax - uMin) * (segmentS / segmentLength);
+
+    const int nrIterations = 40; 
+
+    // initialize root bounding limits for bisection
+    double lower = uMin;
+    double upper = uMax;
+
+    for (int i = 0; i < nrIterations; ++i) {
+        double F = arcLength(uMin, u) - segmentS; 
+
+        const double tolerance = 0.1; // meters. Note that distances are very large
+        if (std::abs(F) <= tolerance) {
+            return u;
+        }
+
+        // generate a candidate for Newton's method        
+        double dfdu = approximatedDerivative(u, Epsilon); // > 0                            
+        double uCandidate = u - F / dfdu;
+
+        // update root-bounding interval and test candidate
+        if (F > 0) {  // => candidate < u <= upper
+            upper = u;
+            u = (uCandidate <= lower) ? (upper + lower) / 2.0 : uCandidate;
+        }
+        else { // F < 0 => lower <= u < candidate 
+            lower = u;
+            u = (uCandidate >= upper) ? (upper + lower) / 2.0 : uCandidate;
+        }
     }
 
-    return (h / 3.0) * (endPoints + 4.0 * times4 + 2.0 *times2);
+    // No root was found based on the number of iterations and tolerance. However, it is
+    // safe to report the last computed u value, since it is within the segment interval
+    return u;
 }
 
 // TODO: remove when not needed
@@ -74,6 +111,83 @@ std::vector<glm::dvec3> PathCurve::getPoints() {
     return _points;
 }
 
+void PathCurve::initParameterIntervals() {
+    ghoul_assert(_nrSegments > 0, "Cannot have a curve with zero segments!");
+    _parameterIntervals.clear();
+    _parameterIntervals.reserve(_nrSegments + 1);
+
+    // compute initial values, to be able to compute lengths
+    double dt = 1.0 / _nrSegments;
+    _parameterIntervals.push_back(0.0);
+    for (unsigned int i = 1; i < _nrSegments; i++) {
+        _parameterIntervals.push_back(dt * i);
+    }
+    _parameterIntervals.push_back(1.0);
+
+    // lengths
+    _lengths.clear();
+    _lengths.reserve(_nrSegments + 1);
+    _lengthSums.clear();
+    _lengthSums.reserve(_nrSegments + 1);
+
+    _lengths.push_back(0.0);
+    _lengthSums.push_back(0.0);
+    for (unsigned int i = 1; i <= _nrSegments; i++) {
+        double u = _parameterIntervals[i];
+        double uPrev = _parameterIntervals[i - 1];
+        _lengths.push_back(arcLength(uPrev, u)); // OBS! Is this length computed well enough?
+        _lengthSums.push_back(_lengthSums[i - 1] + _lengths[i]);
+    }
+    _totalLength = _lengthSums.back();
+
+    // scale parameterIntervals to better match arc lengths
+    for (unsigned int i = 1; i <= _nrSegments; i++) {
+        _parameterIntervals[i] = _lengthSums[i] / _totalLength;
+    }
+}
+
+double PathCurve::approximatedDerivative(double u, double h) {
+    if (u <= h) {
+        return (1.0 / h) * glm::length(interpolate(0.0 + h) - interpolate(0.0));
+    }
+    if (u >= 1.0 - h) {
+        return (1.0 / h) * glm::length(interpolate(1.0) - interpolate(1.0 - h));
+    }
+    return (0.5 / h) * glm::length(interpolate(u + h) - interpolate(u - h));
+}
+
+double PathCurve::arcLength(double limit) {
+    return arcLength(0.0, limit);
+}
+
+// Approximate the arc length using 5-point Gaussian quadrature
+// https://en.wikipedia.org/wiki/Gaussian_quadrature
+double PathCurve::arcLength(double lowerLimit, double upperLimit) {
+    double a = lowerLimit;
+    double b = upperLimit;
+
+    struct GaussLengendreCoefficient {
+        double abscissa; // xi
+        double weight;   // wi
+    };
+
+    static constexpr GaussLengendreCoefficient coefficients[] =
+    {
+        { 0.0, 0.5688889 },
+        { -0.5384693, 0.47862867 },
+        { 0.5384693, 0.47862867 },
+        { -0.90617985, 0.23692688 },
+        { 0.90617985, 0.23692688 }
+    };
+
+    double length = 0.0;
+    for (auto coefficient : coefficients) {
+        double const t = 0.5 * ((b - a)*coefficient.abscissa + (b + a)); // change of interval to [a, b] from [-1, 1] (also 0.5 * (b - a) below)
+        length += approximatedDerivative(t) * coefficient.weight;
+    }
+    return 0.5 * (b - a) * length;
+}
+
 Bezier3Curve::Bezier3Curve(const Waypoint& start, const Waypoint& end) {
     glm::dvec3 startNodePos = start.node()->worldPosition();
     glm::dvec3 endNodePos = end.node()->worldPosition();
@@ -153,18 +267,11 @@ Bezier3Curve::Bezier3Curve(const Waypoint& start, const Waypoint& end) {
 
     _nrSegments = (unsigned int)std::floor((_points.size() - 1) / 3.0);
 
-    // default values for the curve parameter - equally spaced
-    for (double t = 0.0; t <= 1.0; t += 1.0 / _nrSegments) {
-        _parameterIntervals.push_back(t);
-    }
-
-    _length = arcLength(1.0);
-
-    initParameterIntervals();
+   initParameterIntervals();
 }
 
 // Interpolate a list of control points and knot times
-glm::dvec3 Bezier3Curve::positionAt(double u) {
+glm::dvec3 Bezier3Curve::interpolate(double u) {
     ghoul_assert(u >= 0 && u <= 1.0, "Interpolation variable out of range [0, 1]");
     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!");
@@ -192,27 +299,14 @@ glm::dvec3 Bezier3Curve::positionAt(double u) {
         _points[idx + 2], _points[idx + 3]);
 }
 
-// compute curve parameter intervals based on relative arc length
-void Bezier3Curve::initParameterIntervals() {
-    std::vector<double> newIntervals;
-    double dt = 1.0 / _nrSegments;
-
-    newIntervals.push_back(0.0);
-    for (unsigned int i = 1; i < _nrSegments; i++) {
-        newIntervals.push_back(arcLength(dt * i) / _length);
-    }
-    newIntervals.push_back(1.0);
-
-    _parameterIntervals.swap(newIntervals);
-}
-
 LinearCurve::LinearCurve(const Waypoint& start, const Waypoint& end) {
     _points.push_back(start.position());
     _points.push_back(end.position());
-    _length = glm::distance(end.position(), start.position());
+    _nrSegments = 1;
+    initParameterIntervals();
 }
 
-glm::dvec3 LinearCurve::positionAt(double u) {
+glm::dvec3 LinearCurve::interpolate(double u) {
     ghoul_assert(u >= 0 && u <= 1.0, "Interpolation variable out of range [0, 1]");
     return interpolation::linear(u, _points[0], _points[1]);
 }

modules/autonavigation/pathcurves.h

@@ -42,34 +42,42 @@ public:
     virtual ~PathCurve() = 0;
 
     const double length() const;
-    double arcLength(double limit = 1.0);
+    glm::dvec3 positionAt(double relativeLength);
 
-    // u is interpolation parameter in [0,1] (relative length)
-    virtual glm::dvec3 positionAt(double u) = 0;
+    // compute curve parameter that matches the input arc length s
+    double curveParameter(double s);
+
+    virtual glm::dvec3 interpolate(double u) = 0;
 
     std::vector<glm::dvec3> getPoints(); // for debugging
 
 protected:
+    // TODO: give a better name after experimental curve types have been added
+    void initParameterIntervals();
+
+    double approximatedDerivative(double u, double h = 1E-7);
+    double arcLength(double limit = 1.0);
+    double arcLength(double lowerLimit, double upperLimit);
+
     std::vector<glm::dvec3> _points; 
-    double _length; // the total length of the curve (approximated)
+    unsigned int _nrSegments;
+
+    std::vector<double> _parameterIntervals;
+    std::vector<double> _lengths;
+    std::vector<double> _lengthSums;
+    double _totalLength;
 };
 
 class Bezier3Curve : public PathCurve {
 public:
     Bezier3Curve(const Waypoint& start, const Waypoint& end);
-    glm::dvec3 positionAt(double u);
-
-private:
-    void initParameterIntervals(); // TODO: Move this logic out to base class
-
-    std::vector<double> _parameterIntervals;
-    unsigned int _nrSegments;
+    glm::dvec3 interpolate(double u);
 };
 
 class LinearCurve : public PathCurve {
 public:
     LinearCurve(const Waypoint& start, const Waypoint& end);
-    glm::dvec3 positionAt(double u);
+    glm::dvec3 interpolate(double u);
 };
 
 } // namespace openspace::autonavigation

modules/autonavigation/pathsegment.cpp

@@ -33,6 +33,8 @@
 
 namespace {
     constexpr const char* _loggerCat = "PathSegment";
+
+    const double Epsilon = 1E-7;
 } // namespace
 
 namespace openspace::autonavigation {
@@ -84,18 +86,17 @@ CameraPose PathSegment::traversePath(double dt) {
     double h = dt / steps;
     for (int i = 0; i < steps; ++i) {
         double t = _progressedTime + i * h;
-        double speed = 0.5 * (speedAtTime(t - 0.5*h) + speedAtTime(t + 0.5*h)); // midpoint method
+        double speed = 0.5 * (speedAtTime(t - 0.01*h) + speedAtTime(t + 0.01*h)); // average
+        //LINFO(fmt::format("Speed = {}", speed));
         displacement += h * speed;
     }
 
     _traveledDistance += displacement;
-
     double relativeDisplacement = _traveledDistance / pathLength();
-    relativeDisplacement = std::max(0.0, std::min(relativeDisplacement, 1.0));
 
     // TEST: 
     //LINFO("-----------------------------------");
-    //LINFO(fmt::format("u = {}", relativeDisplacement));
+    //LINFO(fmt::format("relativeDisplacement = {}", relativeDisplacement));
     //LINFO(fmt::format("progressedTime = {}", _progressedTime));
 
     _progressedTime += dt;

modules/autonavigation/speedfunction.cpp

@@ -81,7 +81,7 @@ double CubicDampenedSpeed::value(double t) const {
     }
 
     // avoid zero speed
-    speed += 0.001;
+    speed += 0.001; // OBS! This value gets really big for large distances..
     return speed;
 }