fix AStar and BinaryHeap

Author: jube

include/gf2/core/BinaryHeap.h

@@ -76,7 +76,7 @@ namespace gf {
 
       if (has_available_index()) {
         index = available_index();
-        ElementIndex& ei = element_index(index);
+        ElementIndex& ei = m_elements[index];
         ei.element = value;
         ei.heap_index = m_size;
       } else {
@@ -97,7 +97,7 @@ namespace gf {
 
       if (has_available_index()) {
         index = available_index();
-        ElementIndex& ei = element_index(index);
+        ElementIndex& ei = m_elements[index];
         ei.element = std::move(value);
         ei.heap_index = m_size;
       } else {

include/gf2/core/GridMap.h

@@ -25,7 +25,7 @@ namespace gf {
     float blocked = 5.0f;
   };
 
-  using RouteCostFunction = std::function<float(Vec2I position, Vec2I neighbor, Flags<CellProperty> neighbor_properties, const AnyGrid& grid)>;
+  using RouteCostFunction = std::function<float(Vec2I position, Vec2I neighbor)>;
 
   class GF_CORE_API GridMap {
   public:

library/core/GridMap.cc

@@ -338,7 +338,7 @@ namespace gf {
             continue;
           }
 
-          const float updated_distance = m_data(heap_data.position).distance + cost_function(heap_data.position, position, cell, m_grid);
+          const float updated_distance = m_data(heap_data.position).distance + cost_function(heap_data.position, position);
 
           if (updated_distance < m_data(position).distance) {
             auto& data = m_data(position);
@@ -443,19 +443,7 @@ namespace gf {
         m_data(origin).distance = 0.0f;
         m_data(origin).state = AStarState::Open;
 
-        // for (auto position : m_cells.position_range()) {
-        //   const auto cell = m_cells(position);
-        //
-        //   if (!cell.test(CellProperty::Walkable)) {
-        //     continue;
-        //   }
-        //
-        //   AStarHeapData data = {};
-        //   data.position = position;
-        //   data.distance = m_data(position).distance;
-        //
-        //   m_data(position).handle = m_heap.push(data);
-        // }
+        m_heap.push({ origin, 0.0f });
       }
 
       void compute_node(AStarHeapData heap_data, RouteCostFunction& cost_function, Flags<CellNeighborQuery> flags)
@@ -477,7 +465,7 @@ namespace gf {
             continue;
           }
 
-          const float updated_distance = m_data(heap_data.position).distance + cost_function(heap_data.position, position, cell, m_grid);
+          const float updated_distance = m_data(heap_data.position).distance + cost_function(heap_data.position, position);
 
           if (updated_distance < m_data(position).distance) {
             auto& data = m_data(position);
@@ -550,14 +538,14 @@ namespace gf {
       flags |= CellNeighborQuery::Diagonal;
     }
 
-    auto cost_function = [cost](Vec2I position, Vec2I neighbor, Flags<CellProperty> neighbor_properties, const AnyGrid& grid)
+    auto cost_function = [cost,this](Vec2I position, Vec2I neighbor)
     {
-      const bool is_diagonal = grid.are_diagonal_neighbors(position, neighbor);
+      const bool is_diagonal = m_grid.are_diagonal_neighbors(position, neighbor);
       assert(cost.diagonal > 0 || !is_diagonal);
 
       float neighbor_cost = is_diagonal ? cost.diagonal : cost.cardinal;
 
-      if (neighbor_properties.test(CellProperty::Blocked)) {
+      if (m_cells(neighbor).test(CellProperty::Blocked)) {
         neighbor_cost += cost.blocked;
       }
 

tests/tests_core_Grid.cc

@@ -0,0 +1,37 @@
+#include <gf2/core/GridMap.h>
+
+#include "gtest/gtest.h"
+
+TEST(GridTest, DefaultConstructor) {
+  gf::GridMap map;
+
+  EXPECT_EQ(map.size(), gf::vec(0, 0));
+}
+
+TEST(GridTest, Dijkstra) {
+  constexpr int Size = 10;
+  gf::GridMap map = gf::GridMap::make_orthogonal({ Size, Size });
+
+  gf::RouteCost cost;
+  cost.cardinal = 1.0f;
+  cost.diagonal = 0.0f;
+  cost.blocked = 1.0f;
+
+  auto path = map.compute_route({ 0, 0 }, { Size - 1, Size - 1}, cost, gf::Route::Dijkstra);
+
+  EXPECT_EQ(path.size(), 2 * Size - 1);
+}
+
+TEST(GridTest, AStar) {
+  constexpr int Size = 10;
+  gf::GridMap map = gf::GridMap::make_orthogonal({ Size, Size });
+
+  gf::RouteCost cost;
+  cost.cardinal = 1.0f;
+  cost.diagonal = 0.0f;
+  cost.blocked = 1.0f;
+
+  auto path = map.compute_route({ 0, 0 }, { Size - 1, Size - 1}, cost, gf::Route::AStar);
+
+  EXPECT_EQ(path.size(), 2 * Size - 1);
+}