diff --git a/include/openspace/navigation/pathhelperfunctions.h b/include/openspace/navigation/pathhelperfunctions.h index b14266d31c..1007b18040 100644 --- a/include/openspace/navigation/pathhelperfunctions.h +++ b/include/openspace/navigation/pathhelperfunctions.h @@ -38,29 +38,56 @@ namespace openspace::helpers { // Make interpolator parameter t [0,1] progress only inside a subinterval double shiftAndScale(double t, double newStart, double newEnd); + /* + * Compute a quaternion that represents the rotation looking from \p eye to + * \p center, with the specified \p up direction + */ glm::dquat lookAtQuaternion(glm::dvec3 eye, glm::dvec3 center, glm::dvec3 up); + /* + * Compute a view direction vector from a quaternion representing a rotation + */ glm::dvec3 viewDirection(const glm::dquat& q); - bool lineSphereIntersection(glm::dvec3 linePoint1, glm::dvec3 linePoint2, - glm::dvec3 sphereCenter, double spehereRadius, glm::dvec3& intersectionPoint); + /* + * Calculate the intersection of a line and a sphere. + * The line segment is defined from \p p1 to \p p2. + * The sphere is defined by the radius \p r and center point \p center. + * The resulting intersection point is stored in the \p intersectionPoint parameter. + * + * In the case of two intersection points, only care anout the first one. + * + * \return True if the line between \p p1 and \p p2 intersects the sphere given by + * \p r and \p center, and false otherwise + */ + bool lineSphereIntersection(glm::dvec3 p1, glm::dvec3 p2, glm::dvec3 center, + double r, glm::dvec3& intersectionPoint); + /* + * Check if the point p is inside of the sphere defined by radius r and center + * point c + */ bool isPointInsideSphere(const glm::dvec3& p, const glm::dvec3& c, double r); + /* + * Approximate integral of function f over inteval [t0, t1] using Simpson's rule. + * The integer n is the number of partitions and must be an even number. + */ double simpsonsRule(double t0, double t1, int n, std::function f); + /* + * Approximate integral of function f over inteval [t0, t1] using + * 5-point Gauss-Legendre quadrature + * https://en.wikipedia.org/wiki/Gaussian_quadrature + */ double fivePointGaussianQuadrature(double t0, double t1, std::function f); - glm::dquat easedSlerp(const glm::dquat q1, const glm::dquat q2, double t); - } // namespace openspace::helpers namespace openspace::splines { - // TODO: make all these into template functions. - // Alternatively, add cubicBezier interpolation in ghoul and only use - // ghoul's interpolator methods + // TODO: Move these to ghoul's interpolator file (and make template versions) // Centripetal version alpha = 0, uniform for alpha = 0.5 and chordal for alpha = 1 glm::dvec3 catmullRom(double t, const glm::dvec3& p0, const glm::dvec3& p1, @@ -71,14 +98,5 @@ namespace openspace::splines { glm::dvec3 linear(double t, const glm::dvec3& cp1, const glm::dvec3& cp2); - glm::dvec3 hermite(double t, const glm::dvec3 &cp1, const glm::dvec3 &cp2, - const glm::dvec3 &tangent1, const glm::dvec3 &tangent2); - - glm::dvec3 piecewiseCubicBezier(double t, const std::vector& points, - const std::vector& tKnots); - - glm::dvec3 piecewiseLinear(double t, const std::vector& points, - const std::vector& tKnots); - -} // namespace openspace::splines +} // namespace openspace::splines #endif // __OPENSPACE_CORE___PATHHELPERFUNCTIONS___H__ diff --git a/src/navigation/path.cpp b/src/navigation/path.cpp index 2cfbe75312..bc952cd2ac 100644 --- a/src/navigation/path.cpp +++ b/src/navigation/path.cpp @@ -59,14 +59,14 @@ namespace { // (Node): The target node of the camera path. Not optional for 'Node' instructions std::optional target; - // (Node): An optional position in relation to the target node, in model + // (Node): An optional position in relation to the target node, in model // coordinates (meters) std::optional position; // (Node): An optional height in relation to the target node, in meters std::optional height; - // (Node): If true, the up direction of the node is taken into account when + // (Node): If true, the up direction of the node is taken into account when // computing the wayopoint for this instruction std::optional useTargetUpDirection; @@ -169,7 +169,12 @@ glm::dquat Path::interpolateRotation(double t) const { switch (_curveType) { case CurveType::AvoidCollision: case CurveType::Linear: - return helpers::easedSlerp(_start.rotation(), _end.rotation(), t); + { + // Eased slerp rotation between t = 0.1 and t = 0.9 + double tScaled = helpers::shiftAndScale(t, 0.1, 0.9); + tScaled = ghoul::sineEaseInOut(tScaled); + return glm::slerp(_start.rotation(), _end.rotation(), tScaled); + } case CurveType::ZoomOutOverview: { const double t1 = 0.2; @@ -188,7 +193,7 @@ glm::dquat Path::interpolateRotation(double t) const { const glm::dvec3 inFrontOfStart = startPos + inFrontDistance * viewDir; const double tScaled = ghoul::cubicEaseInOut(t / t1); - lookAtPos = + lookAtPos = ghoul::interpolateLinear(tScaled, inFrontOfStart, startNodePos); } else if (t <= t2) { @@ -370,9 +375,9 @@ Waypoint computeDefaultWaypoint(const NodeInfo& info, glm::dvec3 up = global::navigationHandler->camera()->lookUpVectorWorldSpace(); if (info.useTargetUpDirection) { - // @TODO (emmbr 2020-11-17) For now, this is hardcoded to look good for Earth, - // which is where it matters the most. A better solution would be to make each - // sgn aware of its own 'up' and query + // @TODO (emmbr 2020-11-17) For now, this is hardcoded to look good for Earth, + // which is where it matters the most. A better solution would be to make each + // sgn aware of its own 'up' and query up = targetNode->worldRotationMatrix() * glm::dvec3(0.0, 0.0, 1.0); } @@ -382,8 +387,8 @@ Waypoint computeDefaultWaypoint(const NodeInfo& info, return Waypoint(targetPos, targetRot, info.identifier); } -Path createPathFromDictionary(const ghoul::Dictionary& dictionary, - Path::CurveType curveType) +Path createPathFromDictionary(const ghoul::Dictionary& dictionary, + Path::CurveType curveType) { const Parameters p = codegen::bake(dictionary); diff --git a/src/navigation/pathhelperfunctions.cpp b/src/navigation/pathhelperfunctions.cpp index ea05d095ef..93f8010eb1 100644 --- a/src/navigation/pathhelperfunctions.cpp +++ b/src/navigation/pathhelperfunctions.cpp @@ -36,7 +36,7 @@ namespace openspace::helpers { // Shift and scale to a subinterval [start,end] double shiftAndScale(double t, double start, double end) { - ghoul_assert(0.0 < start && start < end&& end < 1.0, + ghoul_assert(0.0 < start && start < end && end < 1.0, "Values must be 0.0 < start < end < 1.0!"); double tScaled = t / (end - start) - start; return std::max(0.0, std::min(tScaled, 1.0)); @@ -52,38 +52,37 @@ namespace openspace::helpers { }; /* - * Calculate the intersection of a line and a sphere - * The line segment is defined from p1 to p2 - * The sphere is of radius r and centered at sc - * There are potentially two points of intersection given by - * p = p1 + mu1 (p2 - p1) - * p = p1 + mu2 (p2 - p1) * Source: http://paulbourke.net/geometry/circlesphere/raysphere.c */ - bool lineSphereIntersection(glm::dvec3 p1, glm::dvec3 p2, glm::dvec3 sc, + bool lineSphereIntersection(glm::dvec3 p1, glm::dvec3 p2, glm::dvec3 center, double r, glm::dvec3& intersectionPoint) { long double a, b, c; glm::dvec3 dp = p2 - p1; a = dp.x * dp.x + dp.y * dp.y + dp.z * dp.z; - b = 2 * (dp.x * (p1.x - sc.x) + dp.y * (p1.y - sc.y) + dp.z * (p1.z - sc.z)); - c = sc.x * sc.x + sc.y * sc.y + sc.z * sc.z; + b = 2.0 * (dp.x * (p1.x - center.x) + dp.y * (p1.y - center.y) + + dp.z * (p1.z - center.z)); + c = center.x * center.x + center.y * center.y + center.z * center.z; c += p1.x * p1.x + p1.y * p1.y + p1.z * p1.z; - c -= 2 * (sc.x * p1.x + sc.y * p1.y + sc.z * p1.z); + c -= 2.0 * (center.x * p1.x + center.y * p1.y + center.z * p1.z); c -= r * r; long double intersectionTest = b * b - 4.0 * a * c; - // no intersection - if (std::abs(a) < Epsilon || intersectionTest < 0.0) { + // No intersection + if (std::abs(a) < 0 || intersectionTest < 0.0) { return false; } + // Intersection else { - // only care about the first intersection point if we have two + // Only care about the first intersection point if we have two const double t = (-b - std::sqrt(intersectionTest)) / (2.0 *a); - if (t <= Epsilon || t >= abs(1.0 - Epsilon)) return false; + // Check if utside of line segment between p1 and p2 + if (t <= 0 || t >= 1.0) { + return false; + } intersectionPoint = p1 + t * dp; return true; @@ -99,6 +98,9 @@ namespace openspace::helpers { } double simpsonsRule(double t0, double t1, int n, std::function f) { + ghoul_assert(n % 2 == 0, "n must be an even number"); + ghoul_assert(n >= 2, "Number of partitions, n, must be at least 2"); + const double h = (t1 - t0) / static_cast(n); const double endpoints = f(t0) + f(t1); double times4 = 0.0; @@ -119,10 +121,6 @@ namespace openspace::helpers { return (h / 3) * (endpoints + 4 * times4 + 2 * times2); } - /* - * Approximate area under a function using 5-point Gaussian quadrature - * https://en.wikipedia.org/wiki/Gaussian_quadrature - */ double fivePointGaussianQuadrature(double t0, double t1, std::function f) { @@ -150,12 +148,6 @@ namespace openspace::helpers { return 0.5 * (b - a) * sum; } - glm::dquat easedSlerp(const glm::dquat q1, const glm::dquat q2, double t) { - double tScaled = helpers::shiftAndScale(t, 0.1, 0.9); - tScaled = ghoul::sineEaseInOut(tScaled); - return glm::slerp(q1, q2, tScaled); - } - } // namespace openspace::helpers namespace openspace::splines { @@ -206,90 +198,4 @@ namespace openspace::splines { ghoul_assert(t >= 0 && t <= 1.0, "Interpolation variable out of range [0, 1]"); return cp1 * (1.0 - t) + cp2 * t; } - - glm::dvec3 hermite(double t, const glm::dvec3 &p1, const glm::dvec3 &p2, - const glm::dvec3 &tangent1, const glm::dvec3 &tangent2) - { - ghoul_assert(t >= 0 && t <= 1.0, "Interpolation variable out of range [0, 1]"); - - if (t <= 0.0) return p1; - if (t >= 1.0) return p2; - - const double t2 = t * t; - const double t3 = t2 * t; - - // Calculate basis functions - double const a0 = (2.0 * t3) - (3.0 * t2) + 1.0; - double const a1 = (-2.0 * t3) + (3.0 * t2); - double const b0 = t3 - (2.0 * t2) + t; - double const b1 = t3 - t2; - - return (a0 * p1) + (a1 * p2) + (b0 * tangent1) + (b1 * tangent2); - } - - // Uniform if tKnots are equally spaced, or else non uniform - glm::dvec3 piecewiseCubicBezier(double t, const std::vector& points, - const std::vector& tKnots) - { - 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" - ); - int nrSegments = (points.size() - 1) / 3; - ghoul_assert( - rSegments == (tKnots.size() - 1), - "Number of interval times must match number of segments" - ); - - if (t <= 0.0) return points.front(); - if (t >= 1.0) return points.back(); - - // Compute current segment index - std::vector::const_iterator segmentEndIt = - std::lower_bound(tKnots.begin(), tKnots.end(), t); - unsigned int segmentIdx = (segmentEndIt - 1) - tKnots.begin(); - - double segmentStart = tKnots[segmentIdx]; - double segmentDuration = (tKnots[segmentIdx + 1] - tKnots[segmentIdx]); - double tScaled = (t - segmentStart) / segmentDuration; - - unsigned int idx = segmentIdx * 3; - - // Interpolate using De Casteljau's algorithm - return cubicBezier( - tScaled, - points[idx], - points[idx + 1], - points[idx + 2], - points[idx + 3] - ); - } - - glm::dvec3 piecewiseLinear(double t, const std::vector& points, - const std::vector& tKnots) - { - ghoul_assert(points.size() == tKnots.size(), "Must have equal number of points and times!"); - ghoul_assert(points.size() > 2, "Minimum of two control points needed for interpolation!"); - - size_t nrSegments = points.size() - 1; - - if (t <= 0.0) return points.front(); - if (t >= 1.0) return points.back(); - - // compute current segment index - std::vector::const_iterator segmentEndIt = - std::lower_bound(tKnots.begin(), tKnots.end(), t); - unsigned int idx = (segmentEndIt - 1) - tKnots.begin(); - - double segmentStart = tKnots[idx]; - double segmentDuration = (tKnots[idx + 1] - tKnots[idx]); - double tScaled = (t - segmentStart) / segmentDuration; - - return linear(tScaled, points[idx], points[idx + 1]); - } - -} // namespace openspace::splines +} // namespace openspace::splines