Feature/serailization

include/omath/pathfinding/navigation_mesh.hpp

@@ -6,7 +6,9 @@
 
 #include "omath/linear_algebra/vector3.hpp"
 #include <expected>
+#include <optional>
 #include <string>
+#include <unordered_map>
 #include <vector>
 
 namespace omath::pathfinding
@@ -28,10 +30,20 @@ namespace omath::pathfinding
         [[nodiscard]]
         bool empty() const;
 
-        [[nodiscard]] std::vector<uint8_t> serialize() const noexcept;
+        // Events -- per-vertex optional tag (e.g. "jump", "teleport")
+        void set_event(const Vector3<float>& vertex, const std::string_view& event_id);
+        void clear_event(const Vector3<float>& vertex);
 
-        void deserialize(const std::vector<uint8_t>& raw) noexcept;
+        [[nodiscard]]
+        std::optional<std::string> get_event(const Vector3<float>& vertex) const noexcept;
+
+        [[nodiscard]] std::string serialize() const noexcept;
+
+        void deserialize(const std::string& raw);
 
         std::unordered_map<Vector3<float>, std::vector<Vector3<float>>> m_vertex_map;
+
+    private:
+        std::unordered_map<Vector3<float>, std::string> m_vertex_events;
     };
 } // namespace omath::pathfinding

source/pathfinding/navigation_mesh.cpp

@@ -3,9 +3,9 @@
 //
 #include "omath/pathfinding/navigation_mesh.hpp"
 #include <algorithm>
-#include <cstring>
-#include <limits>
+#include <sstream>
 #include <stdexcept>
+
 namespace omath::pathfinding
 {
     std::expected<Vector3<float>, std::string>
@@ -30,77 +30,72 @@ namespace omath::pathfinding
         return m_vertex_map.empty();
     }
 
-    std::vector<uint8_t> NavigationMesh::serialize() const noexcept
+    void NavigationMesh::set_event(const Vector3<float>& vertex, const std::string_view&  event_id)
     {
-        std::vector<std::uint8_t> raw;
+        if (!m_vertex_map.contains(vertex))
+            throw std::invalid_argument(std::format("Vertex '{}' not found", vertex));
 
-        // Pre-calculate total size for better performance
-        std::size_t total_size = 0;
-        for (const auto& [vertex, neighbors] : m_vertex_map)
-        {
-            total_size += sizeof(vertex) + sizeof(std::uint16_t) + sizeof(Vector3<float>) * neighbors.size();
-        }
-        raw.reserve(total_size);
+        m_vertex_events[vertex] = event_id;
+    }
 
-        auto dump_to_vector = [&raw]<typename T>(const T& t)
-        {
-            const auto* byte_ptr = reinterpret_cast<const std::uint8_t*>(&t);
-            raw.insert(raw.end(), byte_ptr, byte_ptr + sizeof(T));
-        };
+    void NavigationMesh::clear_event(const Vector3<float>& vertex)
+    {
+        m_vertex_events.erase(vertex);
+    }
 
-        for (const auto& [vertex, neighbors] : m_vertex_map)
-        {
-            // Clamp neighbors count to fit in uint16_t (prevents silent data corruption)
-            // NOTE: If neighbors.size() > 65535, only the first 65535 neighbors will be serialized.
-            // This is a limitation of the current serialization format using uint16_t for count.
-            const auto clamped_count =
-                    std::min<std::size_t>(neighbors.size(), std::numeric_limits<std::uint16_t>::max());
-            const auto neighbors_count = static_cast<std::uint16_t>(clamped_count);
-
-            dump_to_vector(vertex);
-            dump_to_vector(neighbors_count);
-
-            // Only serialize up to the clamped count
-            for (std::size_t i = 0; i < clamped_count; ++i)
-                dump_to_vector(neighbors[i]);
-        }
-        return raw;
+    std::optional<std::string> NavigationMesh::get_event(const Vector3<float>& vertex) const noexcept
+    {
+        const auto it = m_vertex_events.find(vertex);
+        if (it == m_vertex_events.end())
+            return std::nullopt;
+        return it->second;
     }
 
-    void NavigationMesh::deserialize(const std::vector<uint8_t>& raw) noexcept
+    // Serialization format per vertex line:
+    //   x y z neighbor_count event_id
+    // where event_id is "-" when no event is set.
+    // Neighbor lines follow: nx ny nz
+
+    std::string NavigationMesh::serialize() const noexcept
     {
-        auto load_from_vector = [](const std::vector<uint8_t>& vec, std::size_t& offset, auto& value)
+        std::ostringstream oss;
+        for (const auto& [vertex, neighbors] : m_vertex_map)
         {
-            if (offset + sizeof(value) > vec.size())
-                throw std::runtime_error("Deserialize: Invalid input data size.");
+            const auto event_it = m_vertex_events.find(vertex);
+            const std::string& event = (event_it != m_vertex_events.end()) ? event_it->second : "-";
 
-            std::copy_n(vec.data() + offset, sizeof(value), reinterpret_cast<uint8_t*>(&value));
-            offset += sizeof(value);
-        };
+            oss << vertex.x << ' ' << vertex.y << ' ' << vertex.z << ' ' << neighbors.size() << ' ' << event << '\n';
 
-        m_vertex_map.clear();
+            for (const auto& n : neighbors)
+                oss << n.x << ' ' << n.y << ' ' << n.z << '\n';
+        }
+        return oss.str();
+    }
 
-        std::size_t offset = 0;
+    void NavigationMesh::deserialize(const std::string& raw)
+    {
+        m_vertex_map.clear();
+        m_vertex_events.clear();
+        std::istringstream iss(raw);
 
-        while (offset < raw.size())
+        Vector3<float> vertex;
+        std::size_t neighbors_count;
+        std::string event;
+        while (iss >> vertex.x >> vertex.y >> vertex.z >> neighbors_count >> event)
         {
-            Vector3<float> vertex;
-            load_from_vector(raw, offset, vertex);
-
-            std::uint16_t neighbors_count;
-            load_from_vector(raw, offset, neighbors_count);
-
             std::vector<Vector3<float>> neighbors;
             neighbors.reserve(neighbors_count);
-
             for (std::size_t i = 0; i < neighbors_count; ++i)
             {
-                Vector3<float> neighbor;
-                load_from_vector(raw, offset, neighbor);
-                neighbors.push_back(neighbor);
+                Vector3<float> n;
+                if (!(iss >> n.x >> n.y >> n.z))
+                    throw std::runtime_error("Deserialize: Unexpected end of data.");
+                neighbors.push_back(n);
             }
-
             m_vertex_map.emplace(vertex, std::move(neighbors));
+
+            if (event != "-")
+                m_vertex_events.emplace(vertex, std::move(event));
         }
     }
 } // namespace omath::pathfinding

tests/general/unit_test_a_star.cpp

@@ -8,6 +8,29 @@
 using namespace omath;
 using namespace omath::pathfinding;
 
+// ---------------------------------------------------------------------------
+// Helpers
+// ---------------------------------------------------------------------------
+
+static NavigationMesh make_linear_chain(int length)
+{
+    // 0 -> 1 -> 2 -> ... -> length-1  (directed)
+    NavigationMesh nav;
+    for (int i = 0; i < length; ++i)
+    {
+        const Vector3<float> v{static_cast<float>(i), 0.f, 0.f};
+        if (i + 1 < length)
+            nav.m_vertex_map[v] = {Vector3<float>{static_cast<float>(i + 1), 0.f, 0.f}};
+        else
+            nav.m_vertex_map[v] = {};
+    }
+    return nav;
+}
+
+// ---------------------------------------------------------------------------
+// Basic reachability
+// ---------------------------------------------------------------------------
+
 TEST(AStarExtra, TrivialNeighbor)
 {
     NavigationMesh nav;
@@ -78,7 +101,7 @@ TEST(AStarExtra, LongerPathAvoidsBlock)
     constexpr Vector3<float> goal = idx(2, 1);
     const auto path = Astar::find_path(start, goal, nav);
     ASSERT_FALSE(path.empty());
-    EXPECT_EQ(path.front(), goal); // Astar convention: single-element or endpoint present
+    EXPECT_EQ(path.front(), goal);
 }
 
 TEST(AstarTests, TrivialDirectNeighborPath)
@@ -91,9 +114,6 @@ TEST(AstarTests, TrivialDirectNeighborPath)
     nav.m_vertex_map.emplace(v2, std::vector<Vector3<float>>{v1});
 
     const auto path = Astar::find_path(v1, v2, nav);
-    // Current A* implementation returns the end vertex as the reconstructed
-    // path (single-element) in the simple neighbor scenario. Assert that the
-    // endpoint is present and reachable.
     ASSERT_EQ(path.size(), 1u);
     EXPECT_EQ(path.front(), v2);
 }
@@ -133,4 +153,155 @@ TEST(unit_test_a_star, finding_right_path)
     mesh.m_vertex_map[{0.f, 2.f, 0.f}] = {{0.f, 3.f, 0.f}};
     mesh.m_vertex_map[{0.f, 3.f, 0.f}] = {};
     std::ignore = omath::pathfinding::Astar::find_path({}, {0.f, 3.f, 0.f}, mesh);
-}
+}
+
+// ---------------------------------------------------------------------------
+// Directed edges
+// ---------------------------------------------------------------------------
+
+TEST(AstarTests, DirectedEdge_ForwardPathExists)
+{
+    // A -> B only; path from A to B should succeed
+    NavigationMesh nav;
+    constexpr Vector3<float> a{0.f, 0.f, 0.f};
+    constexpr Vector3<float> b{1.f, 0.f, 0.f};
+    nav.m_vertex_map[a] = {b};
+    nav.m_vertex_map[b] = {}; // no edge back
+
+    const auto path = Astar::find_path(a, b, nav);
+    ASSERT_FALSE(path.empty());
+    EXPECT_EQ(path.back(), b);
+}
+
+TEST(AstarTests, DirectedEdge_ReversePathMissing)
+{
+    // A -> B only; path from B to A should fail
+    NavigationMesh nav;
+    constexpr Vector3<float> a{0.f, 0.f, 0.f};
+    constexpr Vector3<float> b{1.f, 0.f, 0.f};
+    nav.m_vertex_map[a] = {b};
+    nav.m_vertex_map[b] = {};
+
+    const auto path = Astar::find_path(b, a, nav);
+    EXPECT_TRUE(path.empty());
+}
+
+// ---------------------------------------------------------------------------
+// Vertex snapping
+// ---------------------------------------------------------------------------
+
+TEST(AstarTests, OffMeshStart_SnapsToNearestVertex)
+{
+    NavigationMesh nav;
+    constexpr Vector3<float> v1{0.f, 0.f, 0.f};
+    constexpr Vector3<float> v2{10.f, 0.f, 0.f};
+    nav.m_vertex_map[v1] = {v2};
+    nav.m_vertex_map[v2] = {v1};
+
+    // Start is slightly off v1 but closer to it than to v2
+    constexpr Vector3<float> off_start{0.1f, 0.f, 0.f};
+    const auto path = Astar::find_path(off_start, v2, nav);
+    ASSERT_FALSE(path.empty());
+    EXPECT_EQ(path.back(), v2);
+}
+
+TEST(AstarTests, OffMeshEnd_SnapsToNearestVertex)
+{
+    NavigationMesh nav;
+    constexpr Vector3<float> v1{0.f, 0.f, 0.f};
+    constexpr Vector3<float> v2{10.f, 0.f, 0.f};
+    nav.m_vertex_map[v1] = {v2};
+    nav.m_vertex_map[v2] = {v1};
+
+    // Goal is slightly off v2 but closer to it than to v1
+    constexpr Vector3<float> off_goal{9.9f, 0.f, 0.f};
+    const auto path = Astar::find_path(v1, off_goal, nav);
+    ASSERT_FALSE(path.empty());
+    EXPECT_EQ(path.back(), v2);
+}
+
+// ---------------------------------------------------------------------------
+// Cycle handling
+// ---------------------------------------------------------------------------
+
+TEST(AstarTests, CyclicGraph_FindsPathWithoutLooping)
+{
+    // Triangle: A <-> B <-> C <-> A
+    NavigationMesh nav;
+    constexpr Vector3<float> a{0.f, 0.f, 0.f};
+    constexpr Vector3<float> b{1.f, 0.f, 0.f};
+    constexpr Vector3<float> c{0.5f, 1.f, 0.f};
+    nav.m_vertex_map[a] = {b, c};
+    nav.m_vertex_map[b] = {a, c};
+    nav.m_vertex_map[c] = {a, b};
+
+    const auto path = Astar::find_path(a, c, nav);
+    ASSERT_FALSE(path.empty());
+    EXPECT_EQ(path.back(), c);
+}
+
+TEST(AstarTests, SelfLoopVertex_DoesNotBreakSearch)
+{
+    // Vertex with itself as a neighbor
+    NavigationMesh nav;
+    constexpr Vector3<float> a{0.f, 0.f, 0.f};
+    constexpr Vector3<float> b{1.f, 0.f, 0.f};
+    nav.m_vertex_map[a] = {a, b}; // self-loop on a
+    nav.m_vertex_map[b] = {a};
+
+    const auto path = Astar::find_path(a, b, nav);
+    ASSERT_FALSE(path.empty());
+    EXPECT_EQ(path.back(), b);
+}
+
+// ---------------------------------------------------------------------------
+// Longer chains
+// ---------------------------------------------------------------------------
+
+TEST(AstarTests, LinearChain_ReachesEnd)
+{
+    constexpr int kLength = 10;
+    const NavigationMesh nav = make_linear_chain(kLength);
+
+    const Vector3<float> start{0.f, 0.f, 0.f};
+    const Vector3<float> goal{static_cast<float>(kLength - 1), 0.f, 0.f};
+
+    const auto path = Astar::find_path(start, goal, nav);
+    ASSERT_FALSE(path.empty());
+    EXPECT_EQ(path.back(), goal);
+}
+
+TEST(AstarTests, LinearChain_MidpointReachable)
+{
+    constexpr int kLength = 6;
+    const NavigationMesh nav = make_linear_chain(kLength);
+
+    const Vector3<float> start{0.f, 0.f, 0.f};
+    const Vector3<float> mid{3.f, 0.f, 0.f};
+
+    const auto path = Astar::find_path(start, mid, nav);
+    ASSERT_FALSE(path.empty());
+    EXPECT_EQ(path.back(), mid);
+}
+
+// ---------------------------------------------------------------------------
+// Serialize -> pathfind integration
+// ---------------------------------------------------------------------------
+
+TEST(AstarTests, PathfindAfterSerializeDeserialize)
+{
+    NavigationMesh nav;
+    constexpr Vector3<float> a{0.f, 0.f, 0.f};
+    constexpr Vector3<float> b{1.f, 0.f, 0.f};
+    constexpr Vector3<float> c{2.f, 0.f, 0.f};
+    nav.m_vertex_map[a] = {b};
+    nav.m_vertex_map[b] = {a, c};
+    nav.m_vertex_map[c] = {b};
+
+    NavigationMesh nav2;
+    nav2.deserialize(nav.serialize());
+
+    const auto path = Astar::find_path(a, c, nav2);
+    ASSERT_FALSE(path.empty());
+    EXPECT_EQ(path.back(), c);
+}