Merge pull request #95 from orange-cpp/pathfinding_improvement

removed brackets

Author: orange-cpp

.clang-format

@@ -56,7 +56,7 @@ SpaceBeforeParensOptions:
  AfterIfMacros: true
  AfterOverloadedOperator: false
  BeforeNonEmptyParentheses: false
-SpaceBeforeRangeBasedForLoopColon: false
+SpaceBeforeRangeBasedForLoopColon: true
 SpaceInEmptyParentheses: false
 SpacesInCStyleCastParentheses: false
 SpacesInConditionalStatement: false

include/omath/pathfinding/a_star.hpp

@@ -22,9 +22,5 @@ namespace omath::pathfinding
  static std::vector<Vector3<float>>
  reconstruct_final_path(const std::unordered_map<Vector3<float>, PathNode>& closed_list,
  const Vector3<float>& current) noexcept;
-
- [[nodiscard]]
- static auto get_perfect_node(const std::unordered_map<Vector3<float>, PathNode>& open_list,
- const Vector3<float>& end_vertex) noexcept;
  };
 } // namespace omath::pathfinding

source/pathfinding/a_star.cpp

@@ -4,9 +4,25 @@
 #include "omath/pathfinding/a_star.hpp"
 #include <algorithm>
 #include <optional>
+#include <queue>
 #include <unordered_map>
 #include <unordered_set>
 
+namespace
+{
+ struct OpenListNode final
+ {
+ omath::Vector3<float> position;
+ float f_cost;
+
+ [[nodiscard]]
+ bool operator>(const OpenListNode& other) const noexcept
+ {
+ return f_cost > other.f_cost;
+ }
+ };
+}
+
 namespace omath::pathfinding
 {
  struct PathNode final
@@ -37,23 +53,13 @@ namespace omath::pathfinding
  std::ranges::reverse(path);
  return path;
  }
- auto Astar::get_perfect_node(const std::unordered_map<Vector3<float>, PathNode>& open_list,
- const Vector3<float>& end_vertex) noexcept
- {
- return std::ranges::min_element(open_list,
- [&end_vertex](const auto& a, const auto& b)
- {
- const float fa = a.second.g_cost + a.first.distance_to(end_vertex);
- const float fb = b.second.g_cost + b.first.distance_to(end_vertex);
- return fa < fb;
- });
- }
 
  std::vector<Vector3<float>> Astar::find_path(const Vector3<float>& start, const Vector3<float>& end,
  const NavigationMesh& nav_mesh) noexcept
  {
  std::unordered_map<Vector3<float>, PathNode> closed_list;
- std::unordered_map<Vector3<float>, PathNode> open_list;
+ std::unordered_map<Vector3<float>, PathNode> node_data;
+ std::priority_queue<OpenListNode, std::vector<OpenListNode>, std::greater<>> open_list;
 
  auto maybe_start_vertex = nav_mesh.get_closest_vertex(start);
  auto maybe_end_vertex = nav_mesh.get_closest_vertex(end);
@@ -64,20 +70,27 @@ namespace omath::pathfinding
  const auto start_vertex = maybe_start_vertex.value();
  const auto end_vertex = maybe_end_vertex.value();
 
- open_list.emplace(start_vertex, PathNode{std::nullopt, 0.f});
+ node_data.emplace(start_vertex, PathNode{std::nullopt, 0.f});
+ open_list.push({start_vertex, start_vertex.distance_to(end_vertex)});
 
  while (!open_list.empty())
  {
- auto current_it = get_perfect_node(open_list, end_vertex);
+ auto current = open_list.top().position;
+ open_list.pop();
+
+ if (closed_list.contains(current))
+ continue;
+
+ auto current_node_it = node_data.find(current);
+ if (current_node_it == node_data.end())
+ continue;
 
- const auto current = current_it->first;
- const auto current_node = current_it->second;
+ const auto current_node = current_node_it->second;
 
  if (current == end_vertex)
  return reconstruct_final_path(closed_list, current);
 
  closed_list.emplace(current, current_node);
- open_list.erase(current_it);
 
  for (const auto& neighbor: nav_mesh.get_neighbors(current))
  {
@@ -86,11 +99,14 @@ namespace omath::pathfinding
 
  const float tentative_g_cost = current_node.g_cost + neighbor.distance_to(current);
 
- // ReSharper disable once CppTooWideScopeInitStatement
- const auto open_it = open_list.find(neighbor);
+ auto node_it = node_data.find(neighbor);
 
- if (open_it == open_list.end() || tentative_g_cost < open_it->second.g_cost)
- open_list[neighbor] = PathNode{current, tentative_g_cost};
+ if (node_it == node_data.end() || tentative_g_cost < node_it->second.g_cost)
+ {
+ node_data[neighbor] = PathNode{current, tentative_g_cost};
+ const float f_cost = tentative_g_cost + neighbor.distance_to(end_vertex);
+ open_list.push({neighbor, f_cost});
+ }
  }
  }
 

source/pathfinding/navigation_mesh.cpp

@@ -3,6 +3,8 @@
 //
 #include "omath/pathfinding/navigation_mesh.hpp"
 #include <algorithm>
+#include <cstring>
+#include <limits>
 #include <stdexcept>
 namespace omath::pathfinding
 {
@@ -30,55 +32,68 @@ namespace omath::pathfinding
 
  std::vector<uint8_t> NavigationMesh::serialize() const noexcept
  {
- auto dump_to_vector = []<typename T>(const T& t, std::vector<uint8_t>& vec)
- {
- for (size_t i = 0; i < sizeof(t); i++)
- vec.push_back(*(reinterpret_cast<const uint8_t*>(&t) + i));
- };
-
- std::vector<uint8_t> raw;
+ std::vector<std::uint8_t> raw;
 
- for (const auto& [vertex, neighbors]: m_vertex_map)
+ // Pre-calculate total size for better performance
+ std::size_t total_size = 0;
+ for (const auto& [vertex, neighbors] : m_vertex_map)
  {
- const auto neighbors_count = neighbors.size();
+ total_size += sizeof(vertex) + sizeof(std::uint16_t) + sizeof(Vector3<float>) * neighbors.size();
+ }
+ raw.reserve(total_size);
 
- dump_to_vector(vertex, raw);
- dump_to_vector(neighbors_count, raw);
+ 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));
+ };
 
- for (const auto& neighbor: neighbors)
- dump_to_vector(neighbor, raw);
+ 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;
  }
 
  void NavigationMesh::deserialize(const std::vector<uint8_t>& raw) noexcept
  {
- auto load_from_vector = [](const std::vector<uint8_t>& vec, size_t& offset, auto& value)
+ auto load_from_vector = [](const std::vector<uint8_t>& vec, std::size_t& offset, auto& value)
  {
  if (offset + sizeof(value) > vec.size())
- {
  throw std::runtime_error("Deserialize: Invalid input data size.");
- }
+
  std::copy_n(vec.data() + offset, sizeof(value), reinterpret_cast<uint8_t*>(&value));
  offset += sizeof(value);
  };
 
  m_vertex_map.clear();
 
- size_t offset = 0;
+ std::size_t offset = 0;
 
  while (offset < raw.size())
  {
  Vector3<float> vertex;
  load_from_vector(raw, offset, vertex);
 
- uint16_t neighbors_count;
+ std::uint16_t neighbors_count;
  load_from_vector(raw, offset, neighbors_count);
 
  std::vector<Vector3<float>> neighbors;
  neighbors.reserve(neighbors_count);
 
- for (size_t i = 0; i < neighbors_count; ++i)
+ for (std::size_t i = 0; i < neighbors_count; ++i)
  {
  Vector3<float> neighbor;
  load_from_vector(raw, offset, neighbor);