diff --git a/include/omath/pathfinding/navigation_mesh.hpp b/include/omath/pathfinding/navigation_mesh.hpp index 35cc6e6..7111597 100644 --- a/include/omath/pathfinding/navigation_mesh.hpp +++ b/include/omath/pathfinding/navigation_mesh.hpp @@ -6,7 +6,9 @@ #include "omath/linear_algebra/vector3.hpp" #include +#include #include +#include #include namespace omath::pathfinding @@ -28,10 +30,20 @@ namespace omath::pathfinding [[nodiscard]] bool empty() const; - [[nodiscard]] std::vector serialize() const noexcept; + // Events -- per-vertex optional tag (e.g. "jump", "teleport") + void set_event(const Vector3& vertex, const std::string_view& event_id); + void clear_event(const Vector3& vertex); - void deserialize(const std::vector& raw) noexcept; + [[nodiscard]] + std::optional get_event(const Vector3& vertex) const noexcept; + + [[nodiscard]] std::string serialize() const noexcept; + + void deserialize(const std::string& raw); std::unordered_map, std::vector>> m_vertex_map; + + private: + std::unordered_map, std::string> m_vertex_events; }; } // namespace omath::pathfinding diff --git a/source/pathfinding/navigation_mesh.cpp b/source/pathfinding/navigation_mesh.cpp index 370bc8f..345c490 100644 --- a/source/pathfinding/navigation_mesh.cpp +++ b/source/pathfinding/navigation_mesh.cpp @@ -3,9 +3,9 @@ // #include "omath/pathfinding/navigation_mesh.hpp" #include -#include -#include +#include #include + namespace omath::pathfinding { std::expected, std::string> @@ -30,77 +30,72 @@ namespace omath::pathfinding return m_vertex_map.empty(); } - std::vector NavigationMesh::serialize() const noexcept + void NavigationMesh::set_event(const Vector3& vertex, const std::string_view& event_id) { - std::vector 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) * neighbors.size(); - } - raw.reserve(total_size); - - auto dump_to_vector = [&raw](const T& t) - { - const auto* byte_ptr = reinterpret_cast(&t); - raw.insert(raw.end(), byte_ptr, byte_ptr + sizeof(T)); - }; - - 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(neighbors.size(), std::numeric_limits::max()); - const auto neighbors_count = static_cast(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; + m_vertex_events[vertex] = event_id; } - void NavigationMesh::deserialize(const std::vector& raw) noexcept + void NavigationMesh::clear_event(const Vector3& vertex) { - auto load_from_vector = [](const std::vector& vec, std::size_t& offset, auto& value) + m_vertex_events.erase(vertex); + } + + std::optional NavigationMesh::get_event(const Vector3& vertex) const noexcept + { + const auto it = m_vertex_events.find(vertex); + if (it == m_vertex_events.end()) + return std::nullopt; + return it->second; + } + + // 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 + { + 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(&value)); - offset += sizeof(value); - }; + oss << vertex.x << ' ' << vertex.y << ' ' << vertex.z << ' ' << neighbors.size() << ' ' << event << '\n'; + for (const auto& n : neighbors) + oss << n.x << ' ' << n.y << ' ' << n.z << '\n'; + } + return oss.str(); + } + + void NavigationMesh::deserialize(const std::string& raw) + { m_vertex_map.clear(); + m_vertex_events.clear(); + std::istringstream iss(raw); - std::size_t offset = 0; - - while (offset < raw.size()) + Vector3 vertex; + std::size_t neighbors_count; + std::string event; + while (iss >> vertex.x >> vertex.y >> vertex.z >> neighbors_count >> event) { - Vector3 vertex; - load_from_vector(raw, offset, vertex); - - std::uint16_t neighbors_count; - load_from_vector(raw, offset, neighbors_count); - std::vector> neighbors; neighbors.reserve(neighbors_count); - for (std::size_t i = 0; i < neighbors_count; ++i) { - Vector3 neighbor; - load_from_vector(raw, offset, neighbor); - neighbors.push_back(neighbor); + Vector3 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 diff --git a/tests/general/unit_test_a_star.cpp b/tests/general/unit_test_a_star.cpp index fc7011a..772ba32 100644 --- a/tests/general/unit_test_a_star.cpp +++ b/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 v{static_cast(i), 0.f, 0.f}; + if (i + 1 < length) + nav.m_vertex_map[v] = {Vector3{static_cast(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 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>{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); -} \ No newline at end of file +} + +// --------------------------------------------------------------------------- +// Directed edges +// --------------------------------------------------------------------------- + +TEST(AstarTests, DirectedEdge_ForwardPathExists) +{ + // A -> B only; path from A to B should succeed + NavigationMesh nav; + constexpr Vector3 a{0.f, 0.f, 0.f}; + constexpr Vector3 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 a{0.f, 0.f, 0.f}; + constexpr Vector3 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 v1{0.f, 0.f, 0.f}; + constexpr Vector3 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 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 v1{0.f, 0.f, 0.f}; + constexpr Vector3 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 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 a{0.f, 0.f, 0.f}; + constexpr Vector3 b{1.f, 0.f, 0.f}; + constexpr Vector3 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 a{0.f, 0.f, 0.f}; + constexpr Vector3 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 start{0.f, 0.f, 0.f}; + const Vector3 goal{static_cast(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 start{0.f, 0.f, 0.f}; + const Vector3 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 a{0.f, 0.f, 0.f}; + constexpr Vector3 b{1.f, 0.f, 0.f}; + constexpr Vector3 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); +} diff --git a/tests/general/unit_test_navigation_mesh.cpp b/tests/general/unit_test_navigation_mesh.cpp index b5ba7b0..37cec31 100644 --- a/tests/general/unit_test_navigation_mesh.cpp +++ b/tests/general/unit_test_navigation_mesh.cpp @@ -7,19 +7,18 @@ using namespace omath::pathfinding; TEST(NavigationMeshTests, SerializeDeserializeRoundTrip) { NavigationMesh nav; - Vector3 a{0.f,0.f,0.f}; - Vector3 b{1.f,0.f,0.f}; - Vector3 c{0.f,1.f,0.f}; + Vector3 a{0.f, 0.f, 0.f}; + Vector3 b{1.f, 0.f, 0.f}; + Vector3 c{0.f, 1.f, 0.f}; - nav.m_vertex_map.emplace(a, std::vector>{b,c}); + nav.m_vertex_map.emplace(a, std::vector>{b, c}); nav.m_vertex_map.emplace(b, std::vector>{a}); nav.m_vertex_map.emplace(c, std::vector>{a}); - auto data = nav.serialize(); + std::string data = nav.serialize(); NavigationMesh nav2; EXPECT_NO_THROW(nav2.deserialize(data)); - // verify neighbors preserved EXPECT_EQ(nav2.m_vertex_map.size(), nav.m_vertex_map.size()); EXPECT_EQ(nav2.get_neighbors(a).size(), 2u); } @@ -27,7 +26,223 @@ TEST(NavigationMeshTests, SerializeDeserializeRoundTrip) TEST(NavigationMeshTests, GetClosestVertexWhenEmpty) { const NavigationMesh nav; - constexpr Vector3 p{5.f,5.f,5.f}; + constexpr Vector3 p{5.f, 5.f, 5.f}; const auto res = nav.get_closest_vertex(p); EXPECT_FALSE(res.has_value()); } + +TEST(NavigationMeshTests, SerializeEmptyMesh) +{ + const NavigationMesh nav; + const std::string data = nav.serialize(); + EXPECT_TRUE(data.empty()); +} + +TEST(NavigationMeshTests, DeserializeEmptyString) +{ + NavigationMesh nav; + EXPECT_NO_THROW(nav.deserialize("")); + EXPECT_TRUE(nav.empty()); +} + +TEST(NavigationMeshTests, SerializeProducesHumanReadableText) +{ + NavigationMesh nav; + nav.m_vertex_map.emplace(Vector3{1.f, 2.f, 3.f}, std::vector>{{4.f, 5.f, 6.f}}); + + const std::string data = nav.serialize(); + + // Must contain the vertex and neighbor coords as plain text + EXPECT_NE(data.find("1"), std::string::npos); + EXPECT_NE(data.find("2"), std::string::npos); + EXPECT_NE(data.find("3"), std::string::npos); + EXPECT_NE(data.find("4"), std::string::npos); + EXPECT_NE(data.find("5"), std::string::npos); + EXPECT_NE(data.find("6"), std::string::npos); +} + +TEST(NavigationMeshTests, DeserializeRestoresNeighborValues) +{ + NavigationMesh nav; + const Vector3 v{1.f, 2.f, 3.f}; + const Vector3 n1{4.f, 5.f, 6.f}; + const Vector3 n2{7.f, 8.f, 9.f}; + nav.m_vertex_map.emplace(v, std::vector>{n1, n2}); + + NavigationMesh nav2; + nav2.deserialize(nav.serialize()); + + ASSERT_EQ(nav2.m_vertex_map.count(v), 1u); + const auto& neighbors = nav2.get_neighbors(v); + ASSERT_EQ(neighbors.size(), 2u); + EXPECT_EQ(neighbors[0], n1); + EXPECT_EQ(neighbors[1], n2); +} + +TEST(NavigationMeshTests, DeserializeOverwritesPreviousData) +{ + NavigationMesh nav; + const Vector3 v{1.f, 0.f, 0.f}; + nav.m_vertex_map.emplace(v, std::vector>{}); + + // Load a different mesh into the same object + NavigationMesh other; + const Vector3 a{10.f, 20.f, 30.f}; + other.m_vertex_map.emplace(a, std::vector>{}); + + nav.deserialize(other.serialize()); + + EXPECT_EQ(nav.m_vertex_map.size(), 1u); + EXPECT_EQ(nav.m_vertex_map.count(v), 0u); + EXPECT_EQ(nav.m_vertex_map.count(a), 1u); +} + +TEST(NavigationMeshTests, RoundTripNegativeAndFractionalCoords) +{ + NavigationMesh nav; + const Vector3 v{-1.5f, 0.25f, -3.75f}; + const Vector3 n{100.f, -200.f, 0.001f}; + nav.m_vertex_map.emplace(v, std::vector>{n}); + + NavigationMesh nav2; + nav2.deserialize(nav.serialize()); + + ASSERT_EQ(nav2.m_vertex_map.count(v), 1u); + const auto& neighbors = nav2.get_neighbors(v); + ASSERT_EQ(neighbors.size(), 1u); + EXPECT_NEAR(neighbors[0].x, n.x, 1e-3f); + EXPECT_NEAR(neighbors[0].y, n.y, 1e-3f); + EXPECT_NEAR(neighbors[0].z, n.z, 1e-3f); +} + +TEST(NavigationMeshTests, GetClosestVertexReturnsNearest) +{ + NavigationMesh nav; + const Vector3 a{0.f, 0.f, 0.f}; + const Vector3 b{10.f, 0.f, 0.f}; + nav.m_vertex_map.emplace(a, std::vector>{}); + nav.m_vertex_map.emplace(b, std::vector>{}); + + const auto res = nav.get_closest_vertex({1.f, 0.f, 0.f}); + ASSERT_TRUE(res.has_value()); + EXPECT_EQ(res.value(), a); +} + +TEST(NavigationMeshTests, VertexWithNoNeighborsRoundTrip) +{ + NavigationMesh nav; + const Vector3 v{5.f, 5.f, 5.f}; + nav.m_vertex_map.emplace(v, std::vector>{}); + + NavigationMesh nav2; + nav2.deserialize(nav.serialize()); + + ASSERT_EQ(nav2.m_vertex_map.count(v), 1u); + EXPECT_TRUE(nav2.get_neighbors(v).empty()); +} + +// --------------------------------------------------------------------------- +// Vertex events +// --------------------------------------------------------------------------- + +TEST(NavigationMeshTests, SetEventOnNonExistentVertexThrows) +{ + NavigationMesh nav; + const Vector3 v{99.f, 99.f, 99.f}; + EXPECT_THROW(nav.set_event(v, "jump"), std::invalid_argument); +} + +TEST(NavigationMeshTests, EventNotSetByDefault) +{ + NavigationMesh nav; + const Vector3 v{0.f, 0.f, 0.f}; + nav.m_vertex_map.emplace(v, std::vector>{}); + + EXPECT_FALSE(nav.get_event(v).has_value()); +} + +TEST(NavigationMeshTests, SetAndGetEvent) +{ + NavigationMesh nav; + const Vector3 v{1.f, 0.f, 0.f}; + nav.m_vertex_map.emplace(v, std::vector>{}); + nav.set_event(v, "jump"); + + const auto event = nav.get_event(v); + ASSERT_TRUE(event.has_value()); + EXPECT_EQ(event.value(), "jump"); +} + +TEST(NavigationMeshTests, OverwriteEvent) +{ + NavigationMesh nav; + const Vector3 v{1.f, 0.f, 0.f}; + nav.m_vertex_map.emplace(v, std::vector>{}); + nav.set_event(v, "jump"); + nav.set_event(v, "teleport"); + + EXPECT_EQ(nav.get_event(v).value(), "teleport"); +} + +TEST(NavigationMeshTests, ClearEvent) +{ + NavigationMesh nav; + const Vector3 v{1.f, 0.f, 0.f}; + nav.m_vertex_map.emplace(v, std::vector>{}); + nav.set_event(v, "jump"); + nav.clear_event(v); + + EXPECT_FALSE(nav.get_event(v).has_value()); +} + +TEST(NavigationMeshTests, EventRoundTripSerialization) +{ + NavigationMesh nav; + const Vector3 a{0.f, 0.f, 0.f}; + const Vector3 b{1.f, 0.f, 0.f}; + nav.m_vertex_map.emplace(a, std::vector>{b}); + nav.m_vertex_map.emplace(b, std::vector>{}); + nav.set_event(b, "jump"); + + NavigationMesh nav2; + nav2.deserialize(nav.serialize()); + + ASSERT_FALSE(nav2.get_event(a).has_value()); + ASSERT_TRUE(nav2.get_event(b).has_value()); + EXPECT_EQ(nav2.get_event(b).value(), "jump"); +} + +TEST(NavigationMeshTests, MultipleEventsRoundTrip) +{ + NavigationMesh nav; + const Vector3 a{0.f, 0.f, 0.f}; + const Vector3 b{1.f, 0.f, 0.f}; + const Vector3 c{2.f, 0.f, 0.f}; + nav.m_vertex_map.emplace(a, std::vector>{}); + nav.m_vertex_map.emplace(b, std::vector>{}); + nav.m_vertex_map.emplace(c, std::vector>{}); + nav.set_event(a, "spawn"); + nav.set_event(c, "teleport"); + + NavigationMesh nav2; + nav2.deserialize(nav.serialize()); + + EXPECT_EQ(nav2.get_event(a).value(), "spawn"); + EXPECT_FALSE(nav2.get_event(b).has_value()); + EXPECT_EQ(nav2.get_event(c).value(), "teleport"); +} + +TEST(NavigationMeshTests, DeserializeClearsOldEvents) +{ + NavigationMesh nav; + const Vector3 v{0.f, 0.f, 0.f}; + nav.m_vertex_map.emplace(v, std::vector>{}); + nav.set_event(v, "jump"); + + // Deserialize a mesh that has no events + NavigationMesh empty_events; + empty_events.m_vertex_map.emplace(v, std::vector>{}); + + nav.deserialize(empty_events.serialize()); + EXPECT_FALSE(nav.get_event(v).has_value()); +}