diff --git a/data/assets/scene/solarsystem/planets/earth/moon/moon.asset b/data/assets/scene/solarsystem/planets/earth/moon/moon.asset index 851e8d135c..3b6ce13768 100644 --- a/data/assets/scene/solarsystem/planets/earth/moon/moon.asset +++ b/data/assets/scene/solarsystem/planets/earth/moon/moon.asset @@ -165,6 +165,7 @@ local Moon = { LabelsColor = {1.0, 1.0, 0.0, 1.0} } }, + Tag = { "moon_solarSystem", "moon_giants" }, GUI = { Path = "/Solar System/Planets/Earth/Moon" } diff --git a/modules/autonavigation/CMakeLists.txt b/modules/autonavigation/CMakeLists.txt index cace06e36d..d49a32127b 100644 --- a/modules/autonavigation/CMakeLists.txt +++ b/modules/autonavigation/CMakeLists.txt @@ -28,6 +28,7 @@ set(HEADER_FILES ${CMAKE_CURRENT_SOURCE_DIR}/atnodenavigator.h ${CMAKE_CURRENT_SOURCE_DIR}/autonavigationmodule.h ${CMAKE_CURRENT_SOURCE_DIR}/autonavigationhandler.h + ${CMAKE_CURRENT_SOURCE_DIR}/avoidcollisioncurve.h ${CMAKE_CURRENT_SOURCE_DIR}/helperfunctions.h ${CMAKE_CURRENT_SOURCE_DIR}/instruction.h ${CMAKE_CURRENT_SOURCE_DIR}/pathspecification.h @@ -44,6 +45,7 @@ set(SOURCE_FILES ${CMAKE_CURRENT_SOURCE_DIR}/autonavigationmodule.cpp ${CMAKE_CURRENT_SOURCE_DIR}/autonavigationmodule_lua.inl ${CMAKE_CURRENT_SOURCE_DIR}/autonavigationhandler.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/avoidcollisioncurve.cpp ${CMAKE_CURRENT_SOURCE_DIR}/helperfunctions.cpp ${CMAKE_CURRENT_SOURCE_DIR}/instruction.cpp ${CMAKE_CURRENT_SOURCE_DIR}/pathspecification.cpp diff --git a/modules/autonavigation/autonavigationhandler.cpp b/modules/autonavigation/autonavigationhandler.cpp index da46829200..67ab23be2b 100644 --- a/modules/autonavigation/autonavigationhandler.cpp +++ b/modules/autonavigation/autonavigationhandler.cpp @@ -89,6 +89,7 @@ AutoNavigationHandler::AutoNavigationHandler() addPropertySubOwner(_atNodeNavigator); _defaultCurveOption.addOptions({ + { CurveType::AvoidCollision, "AvoidCollision" }, { CurveType::Bezier3, "Bezier3" }, { CurveType::Linear, "Linear" } }); diff --git a/modules/autonavigation/avoidcollisioncurve.cpp b/modules/autonavigation/avoidcollisioncurve.cpp new file mode 100644 index 0000000000..4d7f5a8e5e --- /dev/null +++ b/modules/autonavigation/avoidcollisioncurve.cpp @@ -0,0 +1,264 @@ +/***************************************************************************************** +* * +* 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 + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace { + constexpr const char* _loggerCat = "AvoidCollisionCurve"; + const double Epsilon = 1E-7; +} // namespace + +namespace openspace::autonavigation { + +AvoidCollisionCurve::AvoidCollisionCurve(const CameraState& start, const CameraState& end) { + // default rotation interpolation + _rotationInterpolator = RotationInterpolator{ start, end, this, Slerp }; + + _points.push_back(start.position); + _points.push_back(end.position); + + std::vector relevantNodes = findRelevantNodes(); + + // Create extra points to avoid collision + removeCollisions(relevantNodes); + + int nrSegments = _points.size() - 1; + + // default values for the curve parameter - equally spaced + for (double t = 0.0; t <= 1.0; t += 1.0 / (double)nrSegments) { + _parameterIntervals.push_back(t); + } + + _length = arcLength(1.0); + + initParameterIntervals(); +} + +// Interpolate a list of control points and knot times +glm::dvec3 AvoidCollisionCurve::positionAt(double u) { + return interpolatePoints(u); +} + +// TODO: refactor +bool isRelevant(const SceneGraphNode* node) { + // TODO: move this list somewhere else + const std::vector relevantTags{ + "planet_solarSystem", + "moon_solarSystem" + // TODO + }; + + // does the node match any of the relevant tags? + const std::vector tags = node->tags(); + auto result = std::find_first_of(relevantTags.begin(), relevantTags.end(), tags.begin(), tags.end()); + + // does not match any tags => not interesting + if (result == relevantTags.end()) { + return false; + } + + return node->renderable() && (node->boundingSphere() > 0.0); +} + +std::vector AvoidCollisionCurve::findRelevantNodes() { + // TODO: make more efficient + const std::vector& allNodes = + global::renderEngine.scene()->allSceneGraphNodes(); + + std::vector result{}; + std::copy_if(allNodes.begin(), allNodes.end(), std::back_inserter(result), isRelevant); + + return result; +} + +void AvoidCollisionCurve::removeCollisions(std::vector& relevantNodes, int step) { + int nrSegments = _points.size() - 1; + int maxSteps = 10; + + // TODO: handle better / present warning if early out + if (step > maxSteps) return; + + // go through all segments and check for collisions + for (int i = 0; i < nrSegments; ++i) { + const glm::dvec3 lineStart = _points[i]; + const glm::dvec3 lineEnd = _points[i + 1]; + + for (SceneGraphNode* node : relevantNodes) { + // do collision check in relative coordinates, to avoid huge numbers + const glm::dmat4 modelTransform = node->modelTransform(); + glm::dvec3 p1 = glm::inverse(modelTransform) * glm::dvec4(lineStart, 1.0); + glm::dvec3 p2 = glm::inverse(modelTransform) * glm::dvec4(lineEnd, 1.0); + + // sphere to check for collision + double bs = node->boundingSphere(); + double radius = bs; + // TODO: add a buffer (i.e. sue a little larger sphere), when we have + // behaviour for leaving a target. Don't want to pass too close to objects + glm::dvec3 center = glm::dvec3(0.0, 0.0, 0.0); + glm::dvec3 point; + + bool collision = helpers::lineSphereIntersection(p1, p2, center, radius, point); + + // convert back to world coordinates + glm::dvec3 pointWorld = modelTransform * glm::dvec4(point, 1.0); + + if (collision) { + LINFO(fmt::format("Collision with node: {}!", node->identifier())); + + // to avoid collision, take a step in an orhtogonal direction of the + // collision point and add a new point + glm::dvec3 lineDir = glm::normalize(lineEnd - lineStart); + glm::dvec3 nodePos = node->worldPosition(); + glm::dvec3 collisionPointToCenter = nodePos - pointWorld; + glm::dvec3 parallell = glm::proj(collisionPointToCenter, lineDir); + glm::dvec3 orthogonal = collisionPointToCenter - parallell; + + double distance = 2.0 * radius; + glm::dvec3 extraKnot = pointWorld - distance * glm::normalize(orthogonal); + _points.insert(_points.begin() + i + 1, extraKnot); + + // TODO: maybe make this more efficient, and make sure that we have a base case. + removeCollisions(relevantNodes, ++step); + break; + } + } + } +} + +// compute curve parameter intervals based on relative arc length +void AvoidCollisionCurve::initParameterIntervals() { + std::vector newIntervals; + int nrSegments = _points.size() - 1; + for (double t = 0.0; t <= 1.0; t += 1.0 / (double)nrSegments) { + newIntervals.push_back(arcLength(t) / _length); + } + _parameterIntervals.swap(newIntervals); +} + +// Catmull-Rom inspired interpolation of the curve points +glm::dvec3 AvoidCollisionCurve::interpolatePoints(double u) +{ + ghoul_assert(_points.size() == _parameterIntervals.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; + const glm::dvec3 start = _points.front(); + const glm::dvec3 end = _points.back(); + + if (u <= 0.0) return _points.front(); + if (u >= 1.0) return _points.back(); + + if (nrSegments == 1) { + return roundedSpline(u, start, start, end, end); + } + + // compute current segment index + std::vector::const_iterator segmentEndIt = + std::lower_bound(_parameterIntervals.begin(), _parameterIntervals.end(), u); + unsigned int idx = (segmentEndIt - 1) - _parameterIntervals.begin(); + + double segmentStart = _parameterIntervals[idx]; + double segmentDuration = (_parameterIntervals[idx + 1] - _parameterIntervals[idx]); + double tScaled = (u - segmentStart) / segmentDuration; + + glm::dvec3 first = (idx == 0) ? start : _points[idx - 1]; + glm::dvec3 last = (idx == (nrSegments - 1)) ? end : _points[idx + 2]; + + return roundedSpline(tScaled, first, _points[idx], _points[idx + 1], last); +} + +glm::dvec3 AvoidCollisionCurve::roundedSpline(double t, const glm::dvec3 &a, + const glm::dvec3 &b, const glm::dvec3 &c, const glm::dvec3 &d) +{ + ghoul_assert(t >= 0 && t <= 1.0, "Interpolation variable out of range [0, 1]"); + + if (t <= 0.0) return b; + if (t >= 1.0) return c; + + auto isNormalizable = [](const glm::dvec3 v) { + return !(abs(glm::length(v)) < Epsilon); + }; + + // find velocities at b and c + glm::dvec3 cb = c - b; + if (!isNormalizable(cb)) { + return b; + } + + glm::dvec3 ab = a - b; + + // a and b are the same point. try finding a substitue + if (!isNormalizable(ab)) { + ab = b - c; + if (!isNormalizable(ab)) { + ab = glm::dvec3(0.0, 1.0, 0.0); + } + } + + glm::dvec3 bVelocity = glm::normalize(cb) - glm::normalize(ab); + bVelocity = isNormalizable(bVelocity) ? + glm::normalize(bVelocity) : glm::dvec3(0.0, 1.0, 0.0); + + glm::dvec3 dc = d - c; + // d and c are the same point. try finding a substitue + if (!isNormalizable(dc)) { + dc = c - b; + if (!isNormalizable(dc)) { + dc = glm::dvec3(0.0, 1.0, 0.0); + } + } + + glm::dvec3 bc = (-1.0)*cb; + glm::dvec3 cVelocity = glm::normalize(dc) - glm::normalize(bc); + cVelocity = isNormalizable(cVelocity) ? + glm::normalize(cVelocity) : glm::dvec3(0.0, 1.0, 0.0); + + double cbDistance = glm::length(cb); + double tangetLength = cbDistance; + + // Distances in space can be extremely large, so we dont want to let the tangents have the complete length. + const double tangentlengthThreshold = 1E10; // TODO: What's a good threshold?? ALSO make global + if (tangetLength > tangentlengthThreshold) + tangetLength = tangentlengthThreshold; + + // the resulting curve gets much better and more predictable when using a genric + // hermite curve compared to the Catmull Rom implementation + return interpolation::hermite(t, b, c, bVelocity * tangetLength, cVelocity * tangetLength); + + //return interpolation::catmullRom(t, b - bVelocity * cbDistance, b, c, c + cVelocity * cbDistance); +} +} // namespace openspace::autonavigation diff --git a/modules/autonavigation/avoidcollisioncurve.h b/modules/autonavigation/avoidcollisioncurve.h new file mode 100644 index 0000000000..6f0d4005dc --- /dev/null +++ b/modules/autonavigation/avoidcollisioncurve.h @@ -0,0 +1,50 @@ +/***************************************************************************************** + * * + * 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. * + ****************************************************************************************/ + +#ifndef __OPENSPACE_MODULE_AUTONAVIGATION___AVOIDCOLLISIONCURVE___H__ +#define __OPENSPACE_MODULE_AUTONAVIGATION___AVOIDCOLLISIONCURVE___H__ + +#include + +namespace openspace::autonavigation { + +class AvoidCollisionCurve : public PathCurve { +public: + AvoidCollisionCurve(const CameraState& start, const CameraState& end); + glm::dvec3 positionAt(double u); + +private: + std::vector _parameterIntervals; + + std::vector findRelevantNodes(); + void removeCollisions(std::vector& relevantNodes, int step = 0); + void initParameterIntervals(); + glm::dvec3 interpolatePoints(double u); + glm::dvec3 roundedSpline(double u, const glm::dvec3 &a, const glm::dvec3 &b, + const glm::dvec3 &c, const glm::dvec3 &d); +}; + +} // namespace openspace::autonavigation + +#endif // __OPENSPACE_MODULE_AUTONAVIGATION___AVOIDCOLLISIONCURVE___H__ diff --git a/modules/autonavigation/helperfunctions.cpp b/modules/autonavigation/helperfunctions.cpp index d60fc35302..cd50d67775 100644 --- a/modules/autonavigation/helperfunctions.cpp +++ b/modules/autonavigation/helperfunctions.cpp @@ -23,6 +23,12 @@ ****************************************************************************************/ #include +#include + +namespace { + constexpr const char* _loggerCat = "Helpers"; + const double Epsilon = 1E-7; +} // namespace namespace openspace::autonavigation::helpers { @@ -39,6 +45,49 @@ namespace openspace::autonavigation::helpers { return glm::normalize(glm::inverse(glm::quat_cast(lookAtMat))); } + /* + 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, 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; + 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 -= r * r; + + long double intersectionTest = b * b - 4.0 * a * c; + + // no intersection + if (std::abs(a) < Epsilon || intersectionTest < 0.0) { + return false; + } + else { + // only care about the first intersection point if we have two + double t = (-b - std::sqrt(intersectionTest)) / (2.0 *a); + + // outside the line segment? + if (t < 0.0 || t > 1.0) return false; + + // TEST + LINFO("Collision!! at t = " + std::to_string(t)); + + intersectionPoint = p1 + t * dp; + return true; + } + } + } // helpers namespace openspace::autonavigation::interpolation { @@ -46,6 +95,8 @@ namespace openspace::autonavigation::interpolation { glm::dvec3 cubicBezier(double t, const glm::dvec3 &cp1, const glm::dvec3 &cp2, const glm::dvec3 &cp3, const glm::dvec3 &cp4 ) { + ghoul_assert(t >= 0 && t <= 1.0, "Interpolation variable out of range [0, 1]"); + double a = 1.0 - t; return cp1 * a * a * a + cp2 * t * a * a * 3.0 @@ -54,8 +105,102 @@ namespace openspace::autonavigation::interpolation { } glm::dvec3 linear(double t, const glm::dvec3 &cp1, const glm::dvec3 &cp2) { + ghoul_assert(t >= 0 && t <= 1.0, "Interpolation variable out of range [0, 1]"); + return cp1 * (1.0 - t) + cp2 * t; } + glm::dvec3 catmullRom(double t, const glm::dvec3 &p0, const glm::dvec3 &p1, + const glm::dvec3 &p2, const glm::dvec3 &p3) + { + 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; + + glm::dvec3 result = ( + p0 * (-0.5 * t3 + t2 - 0.5 * t) + + p1 * ( 1.5 * t3 - 2.5 * t2 + 1.0) + + p2 * (-1.5 * t3 + 2.0 * t2 + 0.5 * t) + + p3 * ( 0.5 * t3 - 0.5 * t2) + ); + + return result; + } + + 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; + + glm::dvec3 result = (a0 * p1) + (a1 * p2) + (b0 * tangent1) + (b1 * tangent2); + return result; + } + + // 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!"); + ghoul_assert(_nrSegments == (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 interpolation::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 interpolation::linear(tScaled, points[idx], points[idx + 1]); + } + } // interpolation diff --git a/modules/autonavigation/helperfunctions.h b/modules/autonavigation/helperfunctions.h index c3051d2577..f7258b06fa 100644 --- a/modules/autonavigation/helperfunctions.h +++ b/modules/autonavigation/helperfunctions.h @@ -39,6 +39,9 @@ namespace openspace::autonavigation::helpers { glm::dquat getLookAtQuaternion(glm::dvec3 eye, glm::dvec3 center, glm::dvec3 up); + bool lineSphereIntersection(glm::dvec3 linePoint1, glm::dvec3 linePoint2, + glm::dvec3 sphereCenter, double spehereRadius, glm::dvec3& intersectionPoint); + } // namespace namespace openspace::autonavigation::interpolation { @@ -52,5 +55,17 @@ namespace openspace::autonavigation::interpolation { glm::dvec3 linear(double t, const glm::dvec3 &cp1, const glm::dvec3 &cp2); + glm::dvec3 catmullRom(double t, const glm::dvec3 &a, const glm::dvec3 &b, + const glm::dvec3 &c, const glm::dvec3 &d); + + 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 #endif diff --git a/modules/autonavigation/pathcurves.cpp b/modules/autonavigation/pathcurves.cpp index 4ce3329a36..e738c577b2 100644 --- a/modules/autonavigation/pathcurves.cpp +++ b/modules/autonavigation/pathcurves.cpp @@ -25,10 +25,16 @@ #include #include +#include +#include +#include #include -#include #include +#include +#include #include +#include +#include namespace { constexpr const char* _loggerCat = "PathCurve"; @@ -165,35 +171,7 @@ Bezier3Curve::Bezier3Curve(const Waypoint& start, const Waypoint& end) { // Interpolate a list of control points and knot times glm::dvec3 Bezier3Curve::positionAt(double u) { - ghoul_assert(u >= 0 && u <= 1.0, "Interpolation variable out of range [0, 1]"); - - size_t nrPoints = _points.size(); - size_t nrTimes = _parameterIntervals.size(); - - ghoul_assert(nrPoints > 4, "Minimum of four control points needed for interpolation!"); - ghoul_assert((nrPoints - 1) % 3 == 0, "A vector containing 3n + 1 control points must be provided!"); - ghoul_assert(_nrSegments == (nrTimes - 1), "Number of interval times must match number of intervals"); - - if (abs(u) < Epsilon) - return _points.front(); - - if (abs(1.0 - u) < Epsilon) - return _points.back(); - - // compute current segment, by first finding iterator to the first value that is larger than s - std::vector::iterator segmentEndIt = - std::lower_bound(_parameterIntervals.begin(), _parameterIntervals.end(), u); - unsigned int segmentIdx = (segmentEndIt - 1) - _parameterIntervals.begin(); - - double segmentStart = _parameterIntervals[segmentIdx]; - double segmentDuration = (_parameterIntervals[segmentIdx + 1] - _parameterIntervals[segmentIdx]); - double sScaled = (u - segmentStart) / segmentDuration; - - unsigned int idx = segmentIdx * 3; - - // Interpolate using De Casteljau's algorithm - return interpolation::cubicBezier(sScaled, _points[idx], _points[idx + 1], - _points[idx + 2], _points[idx + 3]); + return interpolation::piecewiseCubicBezier(u, _points, _parameterIntervals); } // compute curve parameter intervals based on relative arc length diff --git a/modules/autonavigation/pathcurves.h b/modules/autonavigation/pathcurves.h index 5c27643878..8964bf2a70 100644 --- a/modules/autonavigation/pathcurves.h +++ b/modules/autonavigation/pathcurves.h @@ -33,6 +33,7 @@ namespace openspace::autonavigation { enum CurveType { + AvoidCollision, Bezier3, Linear, }; @@ -54,6 +55,8 @@ protected: double _length; // the total length of the curve (approximated) }; +// TODO: Put path curve classes in separate files + class Bezier3Curve : public PathCurve { public: Bezier3Curve(const Waypoint& start, const Waypoint& end); diff --git a/modules/autonavigation/pathsegment.cpp b/modules/autonavigation/pathsegment.cpp index 99d7a8da8b..defdf33eaf 100644 --- a/modules/autonavigation/pathsegment.cpp +++ b/modules/autonavigation/pathsegment.cpp @@ -24,6 +24,7 @@ #include +#include #include #include #include @@ -123,6 +124,10 @@ void PathSegment::initCurve() { switch (_curveType) { + case CurveType::AvoidCollision: + _curve = std::make_unique(_start, _end); + break; + case CurveType::Bezier3: _curve = std::make_unique(_start, _end); _rotationInterpolator = std::make_unique(