diff --git a/modules/autonavigation/autonavigationhandler.cpp b/modules/autonavigation/autonavigationhandler.cpp index bce544074e..895becde40 100644 --- a/modules/autonavigation/autonavigationhandler.cpp +++ b/modules/autonavigation/autonavigationhandler.cpp @@ -30,6 +30,8 @@ #include #include #include +#include +#include #include #include #include @@ -38,6 +40,7 @@ #include #include #include +#include namespace { constexpr const char* _loggerCat = "AutoNavigationHandler"; @@ -73,6 +76,12 @@ namespace { "If enabled, the camera is controlled using the default stop behavior even when no path is playing." }; + constexpr const openspace::properties::Property::PropertyInfo RelevantNodeTagsInfo = { + "RelevantNodeTags", + "Relevant Node Tags", + "List of tags for the nodes that are relevant for path creation, for example when avoiding collisions." + }; + } // namespace namespace openspace::autonavigation { @@ -84,6 +93,7 @@ AutoNavigationHandler::AutoNavigationHandler() , _stopAtTargetsPerDefault(StopAtTargetsPerDefaultInfo, false) , _defaultStopBehavior(DefaultStopBehaviorInfo, properties::OptionProperty::DisplayType::Dropdown) , _applyStopBehaviorWhenIdle(ApplyStopBehaviorWhenIdleInfo, false) + , _relevantNodeTags(RelevantNodeTagsInfo) { addPropertySubOwner(_atNodeNavigator); @@ -106,6 +116,13 @@ AutoNavigationHandler::AutoNavigationHandler() addProperty(_defaultStopBehavior); addProperty(_applyStopBehaviorWhenIdle); + + // Add the relevant tags + _relevantNodeTags = std::vector{ + "planet_solarSystem", + "moon_solarSystem" + };; + addProperty(_relevantNodeTags); } AutoNavigationHandler::~AutoNavigationHandler() {} // NOLINT @@ -123,6 +140,10 @@ bool AutoNavigationHandler::hasFinished() const { return _currentSegmentIndex > lastIndex; } +const std::vector& AutoNavigationHandler::relevantNodes() const { + return _relevantNodes; +} + void AutoNavigationHandler::updateCamera(double deltaTime) { ghoul_assert(camera() != nullptr, "Camera must not be nullptr"); @@ -182,6 +203,10 @@ void AutoNavigationHandler::updateCamera(double deltaTime) { void AutoNavigationHandler::createPath(PathSpecification& spec) { clearPath(); + // TODO: do this in some initialize function instead, and update + // when list of tags is updated. Also depends on scene change? + _relevantNodes = findRelevantNodes(); + if (spec.stopAtTargetsSpecified()) { _stopAtTargetsPerDefault = spec.stopAtTargets(); LINFO("Property for stop at targets per default was overridden by path specification."); @@ -520,4 +545,31 @@ Waypoint AutoNavigationHandler::computeDefaultWaypoint(const TargetNodeInstructi return Waypoint{ targetPos, targetRot, ins->nodeIdentifier }; } +std::vector AutoNavigationHandler::findRelevantNodes() { + const std::vector& allNodes = + global::renderEngine.scene()->allSceneGraphNodes(); + + const std::vector relevantTags = _relevantNodeTags; + + if (allNodes.empty() || relevantTags.empty()) + return std::vector{}; + + auto isRelevant = [&](const SceneGraphNode* node) { + 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 resultingNodes{}; + std::copy_if(allNodes.begin(), allNodes.end(), std::back_inserter(resultingNodes), isRelevant); + + return resultingNodes; +} + } // namespace openspace::autonavigation diff --git a/modules/autonavigation/autonavigationhandler.h b/modules/autonavigation/autonavigationhandler.h index c72775f9c4..64fdc88f2d 100644 --- a/modules/autonavigation/autonavigationhandler.h +++ b/modules/autonavigation/autonavigationhandler.h @@ -29,6 +29,7 @@ #include #include #include +#include #include namespace openspace { @@ -51,6 +52,7 @@ public: Camera* camera() const; const SceneGraphNode* anchor() const; bool hasFinished() const; + const std::vector& relevantNodes() const; void updateCamera(double deltaTime); void createPath(PathSpecification& spec); @@ -78,6 +80,8 @@ private: Waypoint computeDefaultWaypoint(const TargetNodeInstruction* ins); + std::vector findRelevantNodes(); + // this list essentially represents the camera path std::vector> _pathSegments; @@ -96,6 +100,8 @@ private: bool _isPlaying = false; unsigned int _currentSegmentIndex = 0; + std::vector _relevantNodes; + properties::OptionProperty _defaultCurveOption; properties::BoolProperty _includeRoll; properties::BoolProperty _stopAtTargetsPerDefault; @@ -104,6 +110,8 @@ private: // for testing pause behaviors. // TODO: remove later, if it causes problems with regular navigation properties::BoolProperty _applyStopBehaviorWhenIdle; + + properties::StringListProperty _relevantNodeTags; }; } // namespace openspace::autonavigation diff --git a/modules/autonavigation/avoidcollisioncurve.cpp b/modules/autonavigation/avoidcollisioncurve.cpp index 20608eebb5..f944c2444a 100644 --- a/modules/autonavigation/avoidcollisioncurve.cpp +++ b/modules/autonavigation/avoidcollisioncurve.cpp @@ -24,12 +24,13 @@ #include +#include #include #include #include -#include -#include +#include #include +#include #include #include #include @@ -39,19 +40,17 @@ namespace { constexpr const char* _loggerCat = "AvoidCollisionCurve"; const double Epsilon = 1E-7; - // TODO: move this list somewhere else - const std::vector RelevantTags{ - "planet_solarSystem", - "moon_solarSystem" - // TODO - }; + const double CloseToNodeThresholdFactor = 3.0; + const double AvoidCollisionDistanceFactor = 3.0; + const int MaxAvoidCollisionSteps = 10; } // namespace namespace openspace::autonavigation { AvoidCollisionCurve::AvoidCollisionCurve(const Waypoint& start, const Waypoint& end) { - double thresholdFactor = 3.0; + AutoNavigationModule* module = global::moduleEngine.module(); + _relevantNodes = module->AutoNavigationHandler().relevantNodes(); glm::dvec3 startNodeCenter = start.node()->worldPosition(); glm::dvec3 endNodeCenter = end.node()->worldPosition(); @@ -60,15 +59,14 @@ AvoidCollisionCurve::AvoidCollisionCurve(const Waypoint& start, const Waypoint& start.rotation() * glm::dvec3(0.0, 0.0, -1.0) ); - // Add control points for a catmull-rom spline, first and last will no be intersected - // TODO: test other first and last control points, ex positive or negative view dir + // Add control points for a catmull-rom spline, first and last will not be intersected _points.push_back(start.position()); _points.push_back(start.position()); // Add an extra point to first go backwards if starting close to planet glm::dvec3 nodeCenterToStart = start.position() - startNodeCenter; double distanceToStartNode = glm::length(nodeCenterToStart); - if (distanceToStartNode < thresholdFactor * startNodeRadius) { + if (distanceToStartNode < CloseToNodeThresholdFactor * startNodeRadius) { double distance = startNodeRadius; glm::dvec3 newPos = start.position() + distance * glm::normalize(nodeCenterToStart); _points.push_back(newPos); @@ -78,25 +76,23 @@ AvoidCollisionCurve::AvoidCollisionCurve(const Waypoint& start, const Waypoint& //TODO: determine if its best to compare to end orientation or position glm::dvec3 startToEnd = end.position() - start.position(); double cosStartAngle = glm::dot(normalize(-startViewDirection), normalize(startToEnd)); - if (cosStartAngle > 0.7) { - glm::dquat middleRotation = glm::slerp(start.rotation(), end.rotation(), 0.5); // OBS! Rotation method is not necessarily slerp + + // TODO: reduce magical numbers / create constants + bool targetInOppositeDirection = cosStartAngle > 0.7; + if (targetInOppositeDirection) { + glm::dquat middleRotation = glm::slerp(start.rotation(), end.rotation(), 0.5); glm::dvec3 middleViewDir = glm::normalize(middleRotation * glm::dvec3(0.0, 0.0, -1.0)); double distance = 0.4 * glm::length(startToEnd); - glm::dvec3 newPos = start.position() + 0.2 * startToEnd - distance * middleViewDir; - _points.push_back(newPos); + glm::dvec3 newPosition = start.position() + 0.2 * startToEnd - distance * middleViewDir; + _points.push_back(newPosition); } - // TODO: Add a point to approach straigt towards a specific pose near planet - // TODO: Calculate nice end pose if not defined - _points.push_back(end.position()); _points.push_back(end.position()); - std::vector relevantNodes = findRelevantNodes(); - - // Create extra points to avoid collision - removeCollisions(relevantNodes); + // Create extra points to avoid collision + removeCollisions(); _nrSegments = _points.size() - 3; @@ -129,68 +125,43 @@ glm::dvec3 AvoidCollisionCurve::interpolate(double u) { ); } -std::vector AvoidCollisionCurve::findRelevantNodes() { - // TODO: make more efficient - const std::vector& allNodes = - global::renderEngine.scene()->allSceneGraphNodes(); - - auto isRelevant = [](const SceneGraphNode* node) { - 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 result{}; - std::copy_if(allNodes.begin(), allNodes.end(), std::back_inserter(result), isRelevant); - - return result; -} - -void AvoidCollisionCurve::removeCollisions(std::vector& relevantNodes, int step) { +// Try to reduce the risk of collision by approximating the curve with linear segments. +// If a collision happens, create a new point for the path to go through, in an attempt to +// avoid that collision +void AvoidCollisionCurve::removeCollisions(int step) { int nrSegments = _points.size() - 3; - int maxSteps = 5; - // TODO: handle better / present warning if early out - if (step > maxSteps) return; + if (step > MaxAvoidCollisionSteps) + return; // TODO: handle better / present warning if early out // go through all segments and check for collisions for (int i = 0; i < nrSegments; ++i) { const glm::dvec3 lineStart = _points[i + 1]; const glm::dvec3 lineEnd = _points[i + 2]; - for (SceneGraphNode* node : relevantNodes) { + 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; + double radius = node->boundingSphere(); // TODO: add a buffer (i.e. use 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 intersectionPoint; - bool collision = helpers::lineSphereIntersection(p1, p2, center, radius, intersectionPoint); - - // convert back to world coordinates glm::dvec3 intersectionPointWorld = modelTransform * glm::dvec4(intersectionPoint, 1.0); if (collision) { - LINFO(fmt::format("Collision with node: {}!", node->identifier())); + LINFO(fmt::format("Collision with node: {}!", node->identifier())); // TODO: remove // before we response to the collision, make sure none of the points are inside the node - bool isStartPointInside = helpers::isPointInsideSphere(p1, center, radius); - bool isEndPointInside = helpers::isPointInsideSphere(p2, center, radius); - bool cannotResolveCollision = (isStartPointInside || isEndPointInside); - if (cannotResolveCollision) { + bool isStartInsideNode = helpers::isPointInsideSphere(p1, center, radius); + bool isEndInsideNode = helpers::isPointInsideSphere(p2, center, radius); + if (isStartInsideNode || isEndInsideNode) { LWARNING(fmt::format( "Something went wrong! At least one point in the path is inside node: {}", node->identifier() @@ -206,12 +177,12 @@ void AvoidCollisionCurve::removeCollisions(std::vector& relevan glm::dvec3 parallell = glm::proj(collisionPointToCenter, lineDirection); glm::dvec3 orthogonal = collisionPointToCenter - parallell; - double avoidCollisionDistance = 3.0 * radius; + double avoidCollisionDistance = AvoidCollisionDistanceFactor * radius; glm::dvec3 extraKnot = intersectionPointWorld - avoidCollisionDistance * glm::normalize(orthogonal); _points.insert(_points.begin() + i + 2, extraKnot); // TODO: maybe make this more efficient, and make sure that we have a base case. - removeCollisions(relevantNodes, ++step); + removeCollisions(++step); break; } } diff --git a/modules/autonavigation/avoidcollisioncurve.h b/modules/autonavigation/avoidcollisioncurve.h index f4222eddc8..c07ff7f0e1 100644 --- a/modules/autonavigation/avoidcollisioncurve.h +++ b/modules/autonavigation/avoidcollisioncurve.h @@ -37,8 +37,9 @@ public: glm::dvec3 interpolate(double u); private: - std::vector findRelevantNodes(); - void removeCollisions(std::vector& relevantNodes, int step = 0); + void removeCollisions(int step = 0); + + std::vector _relevantNodes; }; } // namespace openspace::autonavigation diff --git a/modules/autonavigation/helperfunctions.cpp b/modules/autonavigation/helperfunctions.cpp index d427eefd55..eff8fbc93c 100644 --- a/modules/autonavigation/helperfunctions.cpp +++ b/modules/autonavigation/helperfunctions.cpp @@ -79,9 +79,6 @@ namespace openspace::autonavigation::helpers { if (t <= Epsilon || t >= abs(1.0 - Epsilon)) return false; - // TEST - LINFO("Collision!! at t = " + std::to_string(t)); - intersectionPoint = p1 + t * dp; return true; }