mirror of
https://github.com/OpenSpace/OpenSpace.git
synced 2026-02-13 07:01:11 -06:00
236 lines
8.9 KiB
C++
236 lines
8.9 KiB
C++
/*****************************************************************************************
|
|
* *
|
|
* OpenSpace *
|
|
* *
|
|
* Copyright (c) 2014-2021 *
|
|
* *
|
|
* 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/pathcurve.h>
|
|
|
|
#include <modules/autonavigation/helperfunctions.h>
|
|
#include <openspace/query/query.h>
|
|
#include <openspace/scene/scenegraphnode.h>
|
|
#include <ghoul/logging/logmanager.h>
|
|
#include <glm/gtx/projection.hpp>
|
|
#include <algorithm>
|
|
#include <vector>
|
|
|
|
namespace {
|
|
constexpr const char* _loggerCat = "PathCurve";
|
|
constexpr const int NrSamplesPerSegment = 100;
|
|
} // namespace
|
|
|
|
namespace openspace::pathnavigation {
|
|
|
|
PathCurve::~PathCurve() {}
|
|
|
|
const double PathCurve::length() const {
|
|
return _totalLength;
|
|
}
|
|
|
|
glm::dvec3 PathCurve::positionAt(double relativeDistance) {
|
|
const double u = curveParameter(relativeDistance * _totalLength);
|
|
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, _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 = 1;
|
|
while (s > _lengthSums[segmentIndex]) {
|
|
segmentIndex++;
|
|
}
|
|
|
|
const int startIndex = (segmentIndex - 1) * NrSamplesPerSegment;
|
|
const int endIndex = segmentIndex * NrSamplesPerSegment + 1;
|
|
|
|
const double segmentS = s - _lengthSums[segmentIndex - 1];
|
|
const double uMin = _curveParameterSteps[segmentIndex - 1];
|
|
const double uMax = _curveParameterSteps[segmentIndex];
|
|
|
|
// Use samples to find an initial guess for Newton's method
|
|
// 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;
|
|
}
|
|
);
|
|
|
|
const ParameterPair& sample = *sampleIterator;
|
|
const ParameterPair& prevSample = *(sampleIterator - 1);
|
|
const double uPrev = prevSample.u;
|
|
const double sPrev = prevSample.s;
|
|
const double slope = (sample.u - uPrev) / (sample.s - sPrev);
|
|
double u = uPrev + slope * (s - sPrev);
|
|
|
|
constexpr const int maxIterations = 50;
|
|
|
|
// Initialize root bounding limits for bisection
|
|
double lower = uMin;
|
|
double upper = uMax;
|
|
|
|
for (int i = 0; i < maxIterations; ++i) {
|
|
double F = arcLength(uMin, u) - segmentS;
|
|
|
|
// The error we tolerate, in meters. Note that distances are very large
|
|
constexpr const double tolerance = 0.005;
|
|
if (std::abs(F) <= tolerance) {
|
|
return u;
|
|
}
|
|
|
|
// Generate a candidate for Newton's method
|
|
double dfdu = approximatedDerivative(u); // > 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;
|
|
}
|
|
}
|
|
|
|
// 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;
|
|
}
|
|
|
|
std::vector<glm::dvec3> PathCurve::points() {
|
|
return _points;
|
|
}
|
|
|
|
void PathCurve::initializeParameterData() {
|
|
_nSegments = static_cast<int>(_points.size() - 3);
|
|
|
|
ghoul_assert(_nSegments > 0, "Cannot have a curve with zero segments!");
|
|
|
|
_curveParameterSteps.clear();
|
|
_lengthSums.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++) {
|
|
_curveParameterSteps.push_back(dt * i);
|
|
}
|
|
_curveParameterSteps.push_back(1.0);
|
|
|
|
// Arc lengths
|
|
_lengthSums.reserve(_nSegments + 1);
|
|
_lengthSums.push_back(0.0);
|
|
for (unsigned int i = 1; i <= _nSegments; i++) {
|
|
double u = _curveParameterSteps[i];
|
|
double uPrev = _curveParameterSteps[i - 1];
|
|
double length = arcLength(uPrev, u);
|
|
_lengthSums.push_back(_lengthSums[i - 1] + length);
|
|
}
|
|
_totalLength = _lengthSums.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 = _lengthSums[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) {
|
|
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);
|
|
}
|
|
|
|
double PathCurve::arcLength(double lowerLimit, double upperLimit) {
|
|
return helpers::fivePointGaussianQuadrature(
|
|
lowerLimit,
|
|
upperLimit,
|
|
[this](double u) { return approximatedDerivative(u); }
|
|
);
|
|
}
|
|
|
|
glm::dvec3 PathCurve::interpolate(double u) {
|
|
ghoul_assert(u >= 0 && u <= 1.0, "Interpolation variable out of range [0, 1]");
|
|
|
|
if (u < 0.0) {
|
|
return _points[1];
|
|
}
|
|
if (u > 1.0) {
|
|
return *(_points.end() - 2);
|
|
}
|
|
|
|
std::vector<double>::iterator segmentEndIt =
|
|
std::lower_bound(_curveParameterSteps.begin(), _curveParameterSteps.end(), u);
|
|
|
|
const int index =
|
|
static_cast<int>((segmentEndIt - 1) - _curveParameterSteps.begin());
|
|
|
|
double segmentStart = _curveParameterSteps[index];
|
|
double segmentDuration = (_curveParameterSteps[index + 1] - segmentStart);
|
|
double uSegment = (u - segmentStart) / segmentDuration;
|
|
|
|
return interpolation::catmullRom(
|
|
uSegment,
|
|
_points[index],
|
|
_points[index + 1],
|
|
_points[index + 2],
|
|
_points[index + 3],
|
|
1.0 // chordal version
|
|
);
|
|
}
|
|
|
|
LinearCurve::LinearCurve(const Waypoint& start, const Waypoint& end) {
|
|
_points.push_back(start.position());
|
|
_points.push_back(start.position());
|
|
_points.push_back(end.position());
|
|
_points.push_back(end.position());
|
|
initializeParameterData();
|
|
}
|
|
|
|
} // namespace openspace::pathnavigation
|