mirror/OpenSpace
1
0
Fork 0
mirror of https://github.com/OpenSpace/OpenSpace.git synced 2026-03-07 21:08:33 -06:00
Code Issues Packages Projects Releases Wiki Activity

Reimplement arc length reparameterization using a map of (u, s) samples

Browse Source 
Removes (or at least reduces) fluctuation in speed and avoids real time computations. Could eb improved by using something else than a linear interpolation between samples
This commit is contained in:
Emma Broman
2021-06-09 11:36:18 +02:00
parent 73bb375377
commit 1d54a00a25
4 changed files with 78 additions and 121 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
modules/autonavigation/curves/avoidcollisioncurve.cpp
View File

@@ -105,7 +105,7 @@ AvoidCollisionCurve::AvoidCollisionCurve(const Waypoint& start, const Waypoint&
_nSegments = static_cast<int>(_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<double>::iterator segmentEndIt =
std::lower_bound(_parameterIntervals.begin(), _parameterIntervals.end(), u);
unsigned int index = static_cast<int>((segmentEndIt - 1) - _parameterIntervals.begin());
std::lower_bound(_curveParameterSteps.begin(), _curveParameterSteps.end(), u);
unsigned int index = static_cast<int>((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(

4
modules/autonavigation/curves/zoomoutoverviewcurve.cpp
View File

@@ -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

167
modules/autonavigation/pathcurve.cpp
View File

@@ -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<glm::dvec3> 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) {

18
modules/autonavigation/pathcurve.h
View File

@@ -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<glm::dvec3> 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<glm::dvec3> _points;
unsigned int _nSegments;
std::vector<double> _parameterIntervals;
std::vector<double> _lengths;
std::vector<double> _lengthSums;
std::vector<double> _curveParameterSteps; // per segment
std::vector<double> _arcLengthSums; // per segment
double _totalLength;
struct ParameterPair {
double u; // curve parameter
double s; // arc length parameter
};
std::vector<ParameterPair> _parameterSamples;
};
class LinearCurve : public PathCurve {