diff --git a/modules/autonavigation/curves/avoidcollisioncurve.cpp b/modules/autonavigation/curves/avoidcollisioncurve.cpp index 5f7d7875e5..784d331099 100644 --- a/modules/autonavigation/curves/avoidcollisioncurve.cpp +++ b/modules/autonavigation/curves/avoidcollisioncurve.cpp @@ -105,7 +105,7 @@ AvoidCollisionCurve::AvoidCollisionCurve(const Waypoint& start, const Waypoint& _nSegments = static_cast(_points.size() - 3); - initParameterIntervals(); + initializeParameterData(); } // Interpolate a list of control points and knot times @@ -118,11 +118,11 @@ glm::dvec3 AvoidCollisionCurve::interpolate(double u) { } std::vector::iterator segmentEndIt = - std::lower_bound(_parameterIntervals.begin(), _parameterIntervals.end(), u); - unsigned int index = static_cast((segmentEndIt - 1) - _parameterIntervals.begin()); + std::lower_bound(_curveParameterSteps.begin(), _curveParameterSteps.end(), u); + unsigned int index = static_cast((segmentEndIt - 1) - _curveParameterSteps.begin()); - double segmentStart = _parameterIntervals[index]; - double segmentDuration = (_parameterIntervals[index + 1] - _parameterIntervals[index]); + double segmentStart = _curveParameterSteps[index]; + double segmentDuration = (_curveParameterSteps[index + 1] - _curveParameterSteps[index]); double uSegment = (u - segmentStart) / segmentDuration; return interpolation::catmullRom( diff --git a/modules/autonavigation/curves/zoomoutoverviewcurve.cpp b/modules/autonavigation/curves/zoomoutoverviewcurve.cpp index 3bfd47c1fa..a9aeabd37a 100644 --- a/modules/autonavigation/curves/zoomoutoverviewcurve.cpp +++ b/modules/autonavigation/curves/zoomoutoverviewcurve.cpp @@ -88,7 +88,7 @@ ZoomOutOverviewCurve::ZoomOutOverviewCurve(const Waypoint& start, const Waypoint _nSegments = (unsigned int)std::floor((_points.size() - 1) / 3.0); - initParameterIntervals(); + initializeParameterData(); } glm::dvec3 ZoomOutOverviewCurve::interpolate(double u) { @@ -99,7 +99,7 @@ glm::dvec3 ZoomOutOverviewCurve::interpolate(double u) { return _points.back(); } - return interpolation::piecewiseCubicBezier(u, _points, _parameterIntervals); + return interpolation::piecewiseCubicBezier(u, _points, _curveParameterSteps); } } // namespace openspace::autonavigation diff --git a/modules/autonavigation/pathcurve.cpp b/modules/autonavigation/pathcurve.cpp index 3e91d41dea..fbcd96bc19 100644 --- a/modules/autonavigation/pathcurve.cpp +++ b/modules/autonavigation/pathcurve.cpp @@ -34,6 +34,7 @@ namespace { constexpr const char* _loggerCat = "PathCurve"; + constexpr const int NrSamplesPerSegment = 100; } // namespace namespace openspace::autonavigation { @@ -49,137 +50,85 @@ glm::dvec3 PathCurve::positionAt(double relativeLength) { return interpolate(u); } -// 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] +// Compute the curve parameter from an arc length value +// Input s is a length value, in the range [0, _totalLength] // Returns curve parameter in range [0, 1] double PathCurve::curveParameter(double s) { if (s <= 0.0) return 0.0; if (s >= _totalLength) return 1.0; - unsigned int segmentIndex; - for (segmentIndex = 1; segmentIndex < _nSegments; ++segmentIndex) { - if (s <= _lengthSums[segmentIndex]) { - break; - } + unsigned int segmentIndex = 1; + while (s > _arcLengthSums[segmentIndex]) { + segmentIndex++; } - double segmentS = s - _lengthSums[segmentIndex - 1]; - double segmentLength = _lengths[segmentIndex]; + const int startIndex = (segmentIndex - 1) * NrSamplesPerSegment; + const int endIndex = segmentIndex * NrSamplesPerSegment + 1; - const double uMin = _parameterIntervals[segmentIndex - 1]; - const double uMax = _parameterIntervals[segmentIndex]; + // Find first sample with s larger than input s + auto sampleIterator = std::upper_bound( + _parameterSamples.begin() + startIndex, + _parameterSamples.begin() + endIndex, + ParameterPair{ 0.0 , s }, // 0.0 is a dummy value for u + [](ParameterPair lhs, ParameterPair rhs) { + return lhs.s < rhs.s; + } + ); - // Compute curve parameter through linerar interpolation. This adds some variations - // in speed, especially in breakpoints between curve segments, but compared to the - // root bounding approach below the motion becomes much smoother. - // The root finding simply does not work well enough as of now. - return uMin + (uMax - uMin) * (segmentS / segmentLength); + const ParameterPair& sample = *sampleIterator; + const ParameterPair& prevSample = *(sampleIterator - 1); + const double uPrev = prevSample.u; + const double sPrev = prevSample.s; - - // ROOT FINDING USING NEWTON'S METHOD BELOW - - //// Initialize root bounding limits for bisection - //double lower = uMin; - //double upper = uMax; - //double u = uMin; - - //LINFO(fmt::format("Segment: {}", segmentIndex)); - //LINFO(fmt::format("Intitial guess u: {}", u)); - - //// The function we want to find the root for - //auto F = [this, segmentS, uMin](double u) -> double { - // return (arcLength(uMin, u) - segmentS); - //}; - - //// Start by doing a few bisections to find a good estimate - // (could potentially use linear guess as well) - //int counter = 0; - //while (upper - lower > 0.0001) { - // u = (upper + lower) / 2.0; - - // if (F(u) * F(lower) < 0.0) { - // upper = u; - // } - // else { - // lower = u; - // } - // counter++; - //} - - // OBS!! It actually seems like just using bisection returns a better, or at least as - // good, result compared to Newton's method. Is this because of a problem with the - // derivative or arc length computation? - - //LINFO(fmt::format("Bisected u: {}", u)); - //LINFO(fmt::format("nBisections: {}", counter)); - - //const int nIterations = 100; - - //for (int i = 0; i < nIterations; ++i) { - // double function = F(u); - - // const double tolerance = 0.001; - // if (std::abs(function) <= tolerance) { - // LINFO(fmt::format("nIterations: {}", i)); - // return u; - // } - - // // Generate a candidate for Newton's method - // double dfdu = approximatedDerivative(u, Epsilon); // > 0 - // double uCandidate = u - function / dfdu; - - // // Update root-bounding interval and test candidate - // if (function > 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; - // } - //} - - ////LINFO(fmt::format("Max iterations! ({})", nIterations)); - - //// 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; + // Linearly interpolate between samples + const double slope = (sample.u - uPrev) / (sample.s - sPrev); + return uPrev + slope * (s - sPrev); } std::vector PathCurve::points() { return _points; } -void PathCurve::initParameterIntervals() { +void PathCurve::initializeParameterData() { ghoul_assert(_nSegments > 0, "Cannot have a curve with zero segments!"); - _parameterIntervals.clear(); - _parameterIntervals.reserve(_nSegments + 1); - // Space out parameter intervals - double dt = 1.0 / _nSegments; - _parameterIntervals.push_back(0.0); + _curveParameterSteps.clear(); + _arcLengthSums.clear(); + _parameterSamples.clear(); + + // Evenly space out parameter intervals + _curveParameterSteps.reserve(_nSegments + 1); + const double dt = 1.0 / _nSegments; + _curveParameterSteps.push_back(0.0); for (unsigned int i = 1; i < _nSegments; i++) { - _parameterIntervals.push_back(dt * i); + _curveParameterSteps.push_back(dt * i); } - _parameterIntervals.push_back(1.0); + _curveParameterSteps.push_back(1.0); - // Lengths - _lengths.clear(); - _lengths.reserve(_nSegments + 1); - _lengthSums.clear(); - _lengthSums.reserve(_nSegments + 1); - - _lengths.push_back(0.0); - _lengthSums.push_back(0.0); + // Arc lengths + _arcLengthSums.reserve(_nSegments + 1); + _arcLengthSums.push_back(0.0); for (unsigned int i = 1; i <= _nSegments; i++) { - double u = _parameterIntervals[i]; - double uPrev = _parameterIntervals[i - 1]; - _lengths.push_back(arcLength(uPrev, u)); - _lengthSums.push_back(_lengthSums[i - 1] + _lengths[i]); + double u = _curveParameterSteps[i]; + double uPrev = _curveParameterSteps[i - 1]; + double length = arcLength(uPrev, u); + _arcLengthSums.push_back(_arcLengthSums[i - 1] + length); } - _totalLength = _lengthSums.back(); + _totalLength = _arcLengthSums.back(); + + // Compute a map of arc lengths s and curve parameters u, for reparameterization + _parameterSamples.reserve(NrSamplesPerSegment * _nSegments + 1); + const double uStep = 1.0 / (_nSegments * NrSamplesPerSegment); + for (unsigned int i = 0; i < _nSegments; i++) { + double uStart = _curveParameterSteps[i]; + double sStart = _arcLengthSums[i]; + for (int j = 0; j < NrSamplesPerSegment; ++j) { + double u = uStart + j * uStep; + double s = sStart + arcLength(uStart, u); + _parameterSamples.push_back({ u, s }); + } + } + _parameterSamples.push_back({ 1.0, _totalLength }); } double PathCurve::approximatedDerivative(double u, double h) { @@ -208,7 +157,7 @@ LinearCurve::LinearCurve(const Waypoint& start, const Waypoint& end) { _points.push_back(start.position()); _points.push_back(end.position()); _nSegments = 1; - initParameterIntervals(); + initializeParameterData(); } glm::dvec3 LinearCurve::interpolate(double u) { diff --git a/modules/autonavigation/pathcurve.h b/modules/autonavigation/pathcurve.h index 4eb514a472..22d5bc8a8c 100644 --- a/modules/autonavigation/pathcurve.h +++ b/modules/autonavigation/pathcurve.h @@ -38,7 +38,7 @@ public: const double length() const; glm::dvec3 positionAt(double relativeLength); - // Compute curve parameter that matches the input arc length s + // Compute curve parameter u that matches the input arc length s double curveParameter(double s); virtual glm::dvec3 interpolate(double u) = 0; @@ -46,7 +46,9 @@ public: std::vector points(); protected: - void initParameterIntervals(); + // Precompute information related to the pspline parameters, that are + // needed for arc length reparameterization + void initializeParameterData(); double approximatedDerivative(double u, double h = 0.0001); double arcLength(double limit = 1.0); @@ -55,10 +57,16 @@ protected: std::vector _points; unsigned int _nSegments; - std::vector _parameterIntervals; - std::vector _lengths; - std::vector _lengthSums; + std::vector _curveParameterSteps; // per segment + std::vector _arcLengthSums; // per segment double _totalLength; + + struct ParameterPair { + double u; // curve parameter + double s; // arc length parameter + }; + + std::vector _parameterSamples; }; class LinearCurve : public PathCurve {