From 9e44985f13e67037e87f5bb178141b2363e3bfe3 Mon Sep 17 00:00:00 2001
From: Karel Hovorka <git@karel-hovorka.eu>
Date: Sun, 4 Dec 2022 11:50:18 +0100
Subject: [PATCH] Navigation fixes.

---
 .../spaceEngineers/navigation/Navigation.kt   |  9 ++++---
 .../map/labrecruits/LabRecruitsMapBuilder.kt  |  2 +-
 .../map/LabRecruitsControllerRunner.kt        | 27 +++++++++++++++++++
 .../Navigation/NavGraphEditor.cs              |  9 +++----
 4 files changed, 37 insertions(+), 10 deletions(-)
 create mode 100644 JvmClient/src/jvmMain/kotlin/spaceEngineers/util/generator/map/LabRecruitsControllerRunner.kt

diff --git a/JvmClient/src/commonMain/kotlin/spaceEngineers/navigation/Navigation.kt b/JvmClient/src/commonMain/kotlin/spaceEngineers/navigation/Navigation.kt
index 1fb9b9390..a717f7808 100644
--- a/JvmClient/src/commonMain/kotlin/spaceEngineers/navigation/Navigation.kt
+++ b/JvmClient/src/commonMain/kotlin/spaceEngineers/navigation/Navigation.kt
@@ -64,19 +64,20 @@ class CharacterNavigation(
             error("Graph is empty!")
         }
 
+        val maxAllowedDistance = LARGE_BLOCK_CUBE_SIDE_SIZE * 1.15
         val targetNode = graph.nodes
             .minByOrNull { it.data.distanceTo(target.position) } ?: error("Target not found in the graph")
-        if (targetNode.data.distanceTo(target.position) > LARGE_BLOCK_CUBE_SIDE_SIZE) {
+        if (targetNode.data.distanceTo(target.position) > maxAllowedDistance) {
             error(
                 "Target block $id is too far, distance to the closest navigable node: ${
                     targetNode.data.distanceTo(
                         target.position
                     )
-                }, (${targetNode.id})"
+                }, (${targetNode.id}). Allowed max distance is ${maxAllowedDistance}"
             )
         }
-
-        val startNode = allBlocks.minByOrNull { it.position.distanceTo(blockObservation.character.position) }
+        val graphNodes = graph.nodes.map { it.id }
+        val startNode = allBlocks.filter { it.id in graphNodes}.minByOrNull { it.position.distanceTo(blockObservation.character.position) }
             ?: error("No nodes found!")
 
         val path = pathFinder.findPath(graph.toDirectedGraph(), targetNode.id, startNode.id)
diff --git a/JvmClient/src/commonMain/kotlin/spaceEngineers/util/generator/map/labrecruits/LabRecruitsMapBuilder.kt b/JvmClient/src/commonMain/kotlin/spaceEngineers/util/generator/map/labrecruits/LabRecruitsMapBuilder.kt
index e2d171035..c65f0312a 100644
--- a/JvmClient/src/commonMain/kotlin/spaceEngineers/util/generator/map/labrecruits/LabRecruitsMapBuilder.kt
+++ b/JvmClient/src/commonMain/kotlin/spaceEngineers/util/generator/map/labrecruits/LabRecruitsMapBuilder.kt
@@ -21,7 +21,7 @@ class LabRecruitsMapBuilder(
         val gridId = mapPlacer.generate()
         createGroups(gridId, map)
         mapButtons(map)
-        sleep(50)
+        sleep(1500)
         closeAllDoors(gridId, map)
     }
 
diff --git a/JvmClient/src/jvmMain/kotlin/spaceEngineers/util/generator/map/LabRecruitsControllerRunner.kt b/JvmClient/src/jvmMain/kotlin/spaceEngineers/util/generator/map/LabRecruitsControllerRunner.kt
new file mode 100644
index 000000000..534a2580b
--- /dev/null
+++ b/JvmClient/src/jvmMain/kotlin/spaceEngineers/util/generator/map/LabRecruitsControllerRunner.kt
@@ -0,0 +1,27 @@
+package spaceEngineers.util.generator.map
+
+import kotlinx.coroutines.runBlocking
+import spaceEngineers.controller.SpaceEngineersJavaProxyBuilder
+import spaceEngineers.controller.extend
+import spaceEngineers.labrecruits.LabRecruitsController
+
+
+fun main() = runBlocking {
+    val batch = SpaceEngineersJavaProxyBuilder().localhost()
+    val se = batch.extend()
+    val labRecruitsController = LabRecruitsController(se)
+    with(labRecruitsController) {
+        println("goto b0")
+        goToButton("b0")
+
+        println("press b0")
+        pressButton("b0")
+
+        println("goto door0")
+        goToDoor("door0")
+
+        println("goto door1")
+        goToDoor("door1")
+    }
+
+}
diff --git a/Source/Ivxr.SePlugin/Navigation/NavGraphEditor.cs b/Source/Ivxr.SePlugin/Navigation/NavGraphEditor.cs
index a8d0e5874..e00cba009 100644
--- a/Source/Ivxr.SePlugin/Navigation/NavGraphEditor.cs
+++ b/Source/Ivxr.SePlugin/Navigation/NavGraphEditor.cs
@@ -48,11 +48,11 @@ public FatNavGraph GetGraph(string gridId)
 
             // Get some look-ahead
             var sphere = m_lowLevelObserver.GetBoundingSphere(null, m_lowLevelObserver.Radius * 3d);
-
-            // TODO: offset the start position to be below the character's feet
+            
+            var orientationUp = GuessWhichSideIsUp(sourceGrid);
             return CreateGraph(m_lowLevelObserver.ConvertToSeGrid(sourceGrid, sphere),
-                m_lowLevelObserver.CurrentPlayerPosition(),
-                GuessWhichSideIsUp(sourceGrid));
+                m_lowLevelObserver.CurrentPlayerPosition() - orientationUp * 2.5,
+                orientationUp);
         }
 
         internal FatNavGraph CreateGraph(CubeGrid grid, Vector3D start, Vector3I up)
@@ -75,7 +75,6 @@ internal FatNavGraph CreateGraph(CubeGrid grid, Vector3D start, Vector3I up)
 
                 map[currentPosition].Visited = true;
 
-                // check for obstacles (2 blocks above the site)
                 if (IsObstructed(currentPosition, map, up))
                     continue;