From 819229222991caafb9759077efc1ffac88569306 Mon Sep 17 00:00:00 2001
From: Emma Broman <emma.broman@liu.se>
Date: Thu, 6 Oct 2022 22:25:27 +0200
Subject: [PATCH] Move `findNodeNearTarget` function from path to pathnavigator

To reduce conflicts between wisdome installation branch and master
---
 include/openspace/navigation/pathnavigator.h |  7 ++++
 src/navigation/path.cpp                      | 39 +-------------------
 src/navigation/pathnavigator.cpp             | 39 ++++++++++++++++++++
 3 files changed, 47 insertions(+), 38 deletions(-)

diff --git a/include/openspace/navigation/pathnavigator.h b/include/openspace/navigation/pathnavigator.h
index c684abbadf..8f3d8d1895 100644
--- a/include/openspace/navigation/pathnavigator.h
+++ b/include/openspace/navigation/pathnavigator.h
@@ -80,6 +80,13 @@ public:
 
     const std::vector<SceneGraphNode*>& relevantNodes();
 
+    /**
+    * Find a node close to the given node. Closeness is determined by a factor times
+    * the bounding sphere of the object
+    * \return pointer to the SGN if one was found, nullptr otherwise
+    */
+    static SceneGraphNode* findNodeNearTarget(const SceneGraphNode* node);
+
     /**
     * \return The Lua library that contains all Lua functions available to affect the
     * path navigation
diff --git a/src/navigation/path.cpp b/src/navigation/path.cpp
index 10b9004471..0b3fd62ed2 100644
--- a/src/navigation/path.cpp
+++ b/src/navigation/path.cpp
@@ -36,7 +36,6 @@
 #include <openspace/rendering/renderable.h>
 #include <openspace/scene/scenegraphnode.h>
 #include <openspace/query/query.h>
-#include <openspace/util/collisionhelper.h>
 #include <openspace/util/universalhelpers.h>
 #include <ghoul/logging/logmanager.h>
 #include <ghoul/misc/interpolator.h>
@@ -440,50 +439,14 @@ Waypoint waypointFromCamera() {
     return Waypoint{ pos, rot, node };
 }
 
-SceneGraphNode* findNodeNearTarget(const SceneGraphNode* node) {
-    const std::vector<SceneGraphNode*>& relevantNodes =
-        global::navigationHandler->pathNavigator().relevantNodes();
-
-    for (SceneGraphNode* n : relevantNodes) {
-        bool isSame = (n->identifier() == node->identifier());
-        // If the nodes are in the very same position, they are probably representing
-        // the same object
-        isSame |=
-            glm::distance(n->worldPosition(), node->worldPosition()) < LengthEpsilon;
-
-        if (isSame) {
-            continue;
-        }
-
-        constexpr float proximityRadiusFactor = 3.f;
-
-        const float bs = static_cast<float>(n->boundingSphere());
-        const float proximityRadius = proximityRadiusFactor * bs;
-        const glm::dvec3 posInModelCoords =
-            glm::inverse(n->modelTransform()) * glm::dvec4(node->worldPosition(), 1.0);
-
-        bool isClose = collision::isPointInsideSphere(
-            posInModelCoords,
-            glm::dvec3(0.0, 0.0, 0.0),
-            proximityRadius
-        );
-
-        if (isClose) {
-            return n;
-        }
-    }
-
-    return nullptr;
-}
-
 // Compute a target position close to the specified target node, using knowledge of
 // the start point and a desired distance from the node's center
 glm::dvec3 computeGoodStepDirection(const SceneGraphNode* targetNode,
                                     const Waypoint& startPoint)
 {
     const glm::dvec3 nodePos = targetNode->worldPosition();
-    const SceneGraphNode* closeNode = findNodeNearTarget(targetNode);
     const SceneGraphNode* sun = sceneGraphNode(SunIdentifier);
+    const SceneGraphNode* closeNode = PathNavigator::findNodeNearTarget(targetNode);
 
     // @TODO (2021-07-09, emmbr): Not nice to depend on a specific scene graph node,
     // as it might not exist. Ideally, each SGN could know about their preferred
diff --git a/src/navigation/pathnavigator.cpp b/src/navigation/pathnavigator.cpp
index 7ab680b944..a4b74cc394 100644
--- a/src/navigation/pathnavigator.cpp
+++ b/src/navigation/pathnavigator.cpp
@@ -38,6 +38,7 @@
 #include <openspace/scene/scenegraphnode.h>
 #include <openspace/scripting/lualibrary.h>
 #include <openspace/scripting/scriptengine.h>
+#include <openspace/util/collisionhelper.h>
 #include <openspace/util/timemanager.h>
 #include <openspace/util/updatestructures.h>
 #include <ghoul/filesystem/file.h>
@@ -497,6 +498,44 @@ void PathNavigator::findRelevantNodes() {
     _relevantNodes = resultingNodes;
 }
 
+
+SceneGraphNode* PathNavigator::findNodeNearTarget(const SceneGraphNode* node) {
+    constexpr float LengthEpsilon = 1e-5f;
+    const std::vector<SceneGraphNode*>& relNodes =
+        global::navigationHandler->pathNavigator().relevantNodes();
+
+    for (SceneGraphNode* n : relNodes) {
+        bool isSame = (n->identifier() == node->identifier());
+        // If the nodes are in the very same position, they are probably representing
+        // the same object
+        isSame |=
+            glm::distance(n->worldPosition(), node->worldPosition()) < LengthEpsilon;
+
+        if (isSame) {
+            continue;
+        }
+
+        constexpr float proximityRadiusFactor = 3.f;
+
+        const float bs = static_cast<float>(n->boundingSphere());
+        const float proximityRadius = proximityRadiusFactor * bs;
+        const glm::dvec3 posInModelCoords =
+            glm::inverse(n->modelTransform()) * glm::dvec4(node->worldPosition(), 1.0);
+
+        bool isClose = collision::isPointInsideSphere(
+            posInModelCoords,
+            glm::dvec3(0.0, 0.0, 0.0),
+            proximityRadius
+        );
+
+        if (isClose) {
+            return n;
+        }
+    }
+
+    return nullptr;
+}
+
 void PathNavigator::removeRollRotation(CameraPose& pose, double deltaTime) {
     const glm::dvec3 anchorPos = anchor()->worldPosition();
     const glm::dvec3 cameraDir = glm::normalize(