mirror of
https://github.com/OpenSpace/OpenSpace.git
synced 2026-03-11 07:48:37 -05:00
315 lines
13 KiB
C++
315 lines
13 KiB
C++
/*****************************************************************************************
|
|
* *
|
|
* OpenSpace *
|
|
* *
|
|
* Copyright (c) 2014-2019 *
|
|
* *
|
|
* 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/pathcurves.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>
|
|
|
|
namespace {
|
|
constexpr const char* _loggerCat = "PathCurve";
|
|
const double Epsilon = 1E-7;
|
|
} // namespace
|
|
|
|
namespace openspace::autonavigation {
|
|
|
|
PathCurve::~PathCurve() {}
|
|
|
|
const double PathCurve::length() const {
|
|
return _totalLength;
|
|
}
|
|
|
|
glm::dvec3 PathCurve::positionAt(double relativeLength) {
|
|
double u = curveParameter(relativeLength * _totalLength); // TODO: only use relative length?
|
|
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]
|
|
// Returns curve parameter in range [0, 1]
|
|
double PathCurve::curveParameter(double s) {
|
|
if (s <= Epsilon) return 0.0;
|
|
if (s >= _totalLength) return 1.0;
|
|
|
|
unsigned int segmentIndex;
|
|
for (segmentIndex = 1; segmentIndex < _nrSegments; ++segmentIndex) {
|
|
if (s <= _lengthSums[segmentIndex])
|
|
break;
|
|
}
|
|
|
|
// 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;
|
|
}
|
|
}
|
|
|
|
// 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
|
|
// Created for debugging
|
|
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();
|
|
|
|
double startNodeRadius = start.nodeDetails.validBoundingSphere;
|
|
double endNodeRadius = end.nodeDetails.validBoundingSphere;
|
|
|
|
glm::dvec3 startNodeToStartPos = start.position() - startNodePos;
|
|
glm::dvec3 endNodeToEndPos = end.position() - endNodePos;
|
|
|
|
double startTangentLength = 2.0 * startNodeRadius;
|
|
double endTangentLength = 2.0 * endNodeRadius;
|
|
glm::dvec3 startTangentDirection = normalize(startNodeToStartPos);
|
|
glm::dvec3 endTangentDirection = normalize(endNodeToEndPos);
|
|
|
|
// Start by going outwards
|
|
_points.push_back(start.position());
|
|
_points.push_back(start.position() + startTangentLength * startTangentDirection);
|
|
|
|
const std::string& startNode = start.nodeDetails.identifier;
|
|
const std::string& endNode = end.nodeDetails.identifier;
|
|
|
|
if (startNode != endNode) {
|
|
|
|
glm::dvec3 startNodeToEndNode = endNodePos - startNodePos;
|
|
glm::dvec3 startToEndDirection = normalize(end.position() - start.position());
|
|
|
|
// Assuming we move straigh out to point to a distance proportional to radius, angle is enough to check collision risk
|
|
double cosStartAngle = glm::dot(startTangentDirection, startToEndDirection);
|
|
double cosEndAngle = glm::dot(endTangentDirection, startToEndDirection);
|
|
|
|
//TODO: investigate suitable values, could be risky close to surface..
|
|
bool TARGET_BEHIND_STARTNODE = cosStartAngle < -0.8;
|
|
bool TARGET_BEHIND_ENDNODE = cosEndAngle > 0.8;
|
|
bool TARGET_IN_OPPOSITE_DIRECTION = cosStartAngle > 0.7;
|
|
|
|
// Avoid collision with startnode by adding control points on the side of it
|
|
if (TARGET_BEHIND_STARTNODE) {
|
|
glm::dvec3 parallell = glm::proj(startNodeToStartPos, startNodeToEndNode);
|
|
glm::dvec3 orthogonal = normalize(startNodeToStartPos - parallell);
|
|
double dist = 5.0 * startNodeRadius;
|
|
glm::dvec3 extraKnot = startNodePos + dist * orthogonal;
|
|
|
|
_points.push_back(extraKnot + parallell);
|
|
_points.push_back(extraKnot);
|
|
_points.push_back(extraKnot - parallell);
|
|
}
|
|
|
|
// Zoom out, to get a better understanding in a 180 degree turn situation
|
|
if (TARGET_IN_OPPOSITE_DIRECTION) {
|
|
glm::dvec3 parallell = glm::proj(startNodeToStartPos, startNodeToEndNode);
|
|
glm::dvec3 orthogonal = normalize(startNodeToStartPos - parallell);
|
|
double dist = 0.5 * glm::length(startNodeToEndNode);
|
|
// Distant middle point
|
|
glm::dvec3 extraKnot = startNodePos + dist * normalize(parallell) + 3.0 * dist * orthogonal;
|
|
|
|
_points.push_back(extraKnot - 0.3 * dist * normalize(parallell));
|
|
_points.push_back(extraKnot);
|
|
_points.push_back(extraKnot + 0.3 * dist * normalize(parallell));
|
|
}
|
|
|
|
// Avoid collision with endnode by adding control points on the side of it
|
|
if (TARGET_BEHIND_ENDNODE) {
|
|
glm::dvec3 parallell = glm::proj(endNodeToEndPos, startNodeToEndNode);
|
|
glm::dvec3 orthogonal = normalize(endNodeToEndPos - parallell);
|
|
double dist = 5.0 * endNodeRadius;
|
|
glm::dvec3 extraKnot = endNodePos + dist * orthogonal;
|
|
|
|
_points.push_back(extraKnot - parallell);
|
|
_points.push_back(extraKnot);
|
|
_points.push_back(extraKnot + parallell);
|
|
}
|
|
}
|
|
|
|
_points.push_back(end.position() + endTangentLength * endTangentDirection);
|
|
_points.push_back(end.position());
|
|
|
|
_nrSegments = (unsigned int)std::floor((_points.size() - 1) / 3.0);
|
|
|
|
initParameterIntervals();
|
|
}
|
|
|
|
// Interpolate a list of control points and knot times
|
|
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!");
|
|
ghoul_assert(_nrSegments == (_parameterIntervals.size() - 1), "Number of interval times must match number of intervals");
|
|
|
|
if (u <= Epsilon)
|
|
return _points.front();
|
|
|
|
if (u >= 1.0 - Epsilon)
|
|
return _points.back();
|
|
|
|
// compute current segment, by first finding iterator to the first value that is larger than s
|
|
std::vector<double>::iterator segmentEndIt =
|
|
std::lower_bound(_parameterIntervals.begin(), _parameterIntervals.end(), u);
|
|
unsigned int segmentIdx = (unsigned int)((segmentEndIt - 1) - _parameterIntervals.begin());
|
|
|
|
double segmentStart = _parameterIntervals[segmentIdx];
|
|
double segmentDuration = (_parameterIntervals[segmentIdx + 1] - segmentStart);
|
|
double uScaled = (u - segmentStart) / segmentDuration;
|
|
|
|
unsigned int idx = segmentIdx * 3;
|
|
|
|
// Interpolate using De Casteljau's algorithm
|
|
return interpolation::cubicBezier(uScaled, _points[idx], _points[idx + 1],
|
|
_points[idx + 2], _points[idx + 3]);
|
|
}
|
|
|
|
LinearCurve::LinearCurve(const Waypoint& start, const Waypoint& end) {
|
|
_points.push_back(start.position());
|
|
_points.push_back(end.position());
|
|
_nrSegments = 1;
|
|
initParameterIntervals();
|
|
}
|
|
|
|
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]);
|
|
}
|
|
|
|
} // namespace openspace::autonavigation
|