From 0fe7f9c47300970706c98a1fb3fefed5a135643b Mon Sep 17 00:00:00 2001
From: Emma Broman <emma95.broman@hotmail.com>
Date: Wed, 27 May 2020 10:47:48 +0200
Subject: [PATCH] Compute end point smarter if target is close to another node

---
 .../autonavigation/autonavigationhandler.cpp  | 43 +++++++++++++++++--
 .../autonavigation/autonavigationhandler.h    |  1 +
 2 files changed, 41 insertions(+), 3 deletions(-)

diff --git a/modules/autonavigation/autonavigationhandler.cpp b/modules/autonavigation/autonavigationhandler.cpp
index 55d145ae3d..5c51f35fd6 100644
--- a/modules/autonavigation/autonavigationhandler.cpp
+++ b/modules/autonavigation/autonavigationhandler.cpp
@@ -524,6 +524,34 @@ void AutoNavigationHandler::addStopDetails(const Instruction* ins) {
     _stops.push_back(stopEntry);
 }
 
+// Test if the node lies within a given proximity radius of any relevant node in the scene
+SceneGraphNode* AutoNavigationHandler::findNodeNearTarget(const SceneGraphNode* node) {
+    glm::dvec3 nodePosition = node->worldPosition();
+    std::string nodeId = node->identifier();
+
+    const float proximityRadiusFactor = 3.0f;
+
+    for (SceneGraphNode* n : _relevantNodes) {
+        if (n->identifier() == nodeId)
+            continue;
+
+        float proximityRadius = proximityRadiusFactor * n->boundingSphere();
+        const glm::dmat4 invModelTransform = glm::inverse(n->modelTransform());
+        glm::dvec3 positionModelCoords = invModelTransform * glm::dvec4(nodePosition, 1.0);
+
+        bool isClose = helpers::isPointInsideSphere(
+            positionModelCoords, 
+            glm::dvec3(0.0, 0.0, 0.0), 
+            proximityRadius
+        );
+
+        if (isClose) 
+            return n;
+    }
+
+    return nullptr;
+}
+
 // OBS! The desired default waypoint may vary between curve types. 
 // TODO: let the curves compute the default positions instead
 Waypoint AutoNavigationHandler::computeDefaultWaypoint(const TargetNodeInstruction* ins) {
@@ -532,16 +560,25 @@ Waypoint AutoNavigationHandler::computeDefaultWaypoint(const TargetNodeInstructi
         LERROR(fmt::format("Could not find target node '{}'", ins->nodeIdentifier));
         return Waypoint();
     }
+
     glm::dvec3 nodePos = targetNode->worldPosition();
-    glm::dvec3 nodeToPrev = lastWayPoint().position() - nodePos;
+    glm::dvec3 stepDirection = glm::normalize(lastWayPoint().position() - nodePos);
+
+    // If the node is close to another node in the scene, make sure that the
+    // position is set to minimize risk of collision
+    SceneGraphNode* closeNode = findNodeNearTarget(targetNode);
+    if (closeNode) {
+        stepDirection = glm::normalize(nodePos - closeNode->worldPosition());
+    }
 
     const double radius = WaypointNodeDetails::findValidBoundingSphere(targetNode);
-    const double defaultHeight = 2 * radius;
+    const double defaultHeight = 2.0 * radius;
 
     bool hasHeight = ins->height.has_value();
     double height = hasHeight ? ins->height.value() : defaultHeight;
 
-    glm::dvec3 targetPos = nodePos + glm::normalize(nodeToPrev) * (radius + height);
+    glm::dvec3 targetPos = nodePos + stepDirection * (radius + height);
+
     glm::dquat targetRot = helpers::getLookAtQuaternion(
         targetPos,
         targetNode->worldPosition(),
diff --git a/modules/autonavigation/autonavigationhandler.h b/modules/autonavigation/autonavigationhandler.h
index 1beb57343c..42954992b4 100644
--- a/modules/autonavigation/autonavigationhandler.h
+++ b/modules/autonavigation/autonavigationhandler.h
@@ -78,6 +78,7 @@ private:
     void addSegment(Instruction* ins, int index);
     void addStopDetails(const Instruction* ins);
 
+    SceneGraphNode* findNodeNearTarget(const SceneGraphNode* node);
     Waypoint computeDefaultWaypoint(const TargetNodeInstruction* ins);
 
     std::vector<SceneGraphNode*> findRelevantNodes();