Working on navigation

server/tests/PathFinderTests.cpp

@@ -2,7 +2,10 @@
 
 #include <Eigen/Core>
 #include <cstdlib>
+#include <filesystem>
+#include <fstream>
 #include <iostream>
+#include <stdexcept>
 
 namespace {
 int fail(const char* message)
@@ -10,26 +13,75 @@ int fail(const char* message)
     std::cout << "PathFinderTests: " << message << std::endl;
     return EXIT_FAILURE;
 }
+
+std::filesystem::path writeNavigationFixture()
+{
+    const auto path = std::filesystem::temp_directory_path() /
+        "shadow_over_bishkek_pathfinder_test.json";
+    std::ofstream file(path);
+    file << R"json({
+        "areas": [{
+            "available": true,
+            "name": "test_area",
+            "polygon": [[-10.0, 10.0], [10.0, 10.0], [10.0, -10.0], [-10.0, -10.0]]
+        }],
+        "floorY": 2.5,
+        "obstacles": [{
+            "name": "test_obstacle",
+            "polygon": [[-1.0, 1.0], [1.0, 1.0], [1.0, -1.0], [-1.0, -1.0]]
+        }]
+    })json";
+    if (!file)
+        throw std::runtime_error("Failed to create navigation test fixture");
+    return path;
+}
 }
 
 int main()
 {
+    const auto configPath = writeNavigationFixture();
     FRG::PathFinder pathFinder;
-    pathFinder.build();
+    pathFinder.build(configPath.string());
     if (!pathFinder.isReady()) return fail("Navmesh was not built");
 
-    const Eigen::Vector3f start(-50.f, 0.f, -50.f);
-    const Eigen::Vector3f end(50.f, 0.f, 50.f);
+    if (pathFinder.getArea().name != "test_area")
+        return fail("First navigation area was not loaded");
+    if (pathFinder.getArea().polygon.size() != 4)
+        return fail("Navigation area polygon was not loaded");
+    if (pathFinder.getObstaclePolygons().size() != 1 ||
+        pathFinder.getObstaclePolygons().front().name != "test_obstacle")
+        return fail("Navigation obstacles were not loaded");
+    if (static_cast<float>(pathFinder.getFloorY()) != 2.5f)
+        return fail("Navigation floorY was not loaded");
+
+    const Eigen::Vector3f start(-5.f, 0.f, -5.f);
+    const Eigen::Vector3f end(5.f, 0.f, 5.f);
     const auto path = pathFinder.findPathToNearest(start, end);
     if (path.size() < 2) return fail("Path query did not produce a route");
-    if (!pathFinder.isWalkable(Eigen::Vector3f(0.f, 0.f, 50.f)))
+    if (!pathFinder.isWalkable(Eigen::Vector3f(0.f, 0.f, 5.f)))
         return fail("Walkability query failed");
+    if (path.back().y() != 2.5f)
+        return fail("Path waypoint did not use floorY");
 
-    const auto clamped = pathFinder.clampToNavMesh(FRG::Vec2fp::fromFloat(150.f, 150.f));
-    if (static_cast<float>(clamped.x) > 100.01f ||
-        static_cast<float>(clamped.y) > 100.01f)
+    const auto clamped = pathFinder.clampToNavMesh(FRG::Vec2fp::fromFloat(50.f, 50.f));
+    if (static_cast<float>(clamped.x) > 10.01f ||
+        static_cast<float>(clamped.y) > 10.01f)
         return fail("Clamping returned a point outside the navmesh");
 
+    pathFinder.build(configPath.string());
+    if (pathFinder.getObstaclePolygons().size() != 1)
+        return fail("Repeated build accumulated navigation obstacles");
+
+    bool rejectedNonJson = false;
+    try {
+        pathFinder.build((configPath.parent_path() / "navigation.txt").string());
+    } catch (const std::runtime_error&) {
+        rejectedNonJson = true;
+    }
+    if (!rejectedNonJson)
+        return fail("Non-JSON navigation config was accepted");
+
+    std::filesystem::remove(configPath);
     std::cout << "PathFinderTests: OK" << std::endl;
     return EXIT_SUCCESS;
 }