wip astar neighbor bitmask seems to work now

3 files changed, 104 insertions, 64 deletions

diff --git a/src/path-search.cpp b/src/path-search.cpp
index 915a41ad..dfcf0c3c 100644
--- a/src/path-search.cpp
+++ b/src/path-search.cpp
@@ -18,7 +18,7 @@ constexpr auto div = path_search::subdivide_factor;
 constexpr int div_BITS = 2;
 static_assert(1 << div_BITS == div);
 
-using bbox = path_search::bbox;
+static_assert(path_search::min_size.x() % 2 == 0 && path_search::min_size.x() * div * 2 == iTILE_SIZE2.x());
 
 constexpr auto never_continue_1 = [](collision_data) constexpr { return path_search_continue::blocked; };
 constexpr auto never_continue_ = path_search::pred{never_continue_1};
@@ -50,8 +50,8 @@ constexpr Pair<Vector2i, Vector2i> get_value(Vector2i sz, Vector2ub div, rotatio
         return {min_W, max_W};
     }
     case (uint8_t)rotation::E: {
-        auto min_E = Vector2i(sz.x()/2,                         -sz.y()/2                       );
-        auto max_E = Vector2i(offset_W + sz.x() - sz.x()/2,     min_E.y() + sz.y() - sz.y()/2   );
+        auto min_E = Vector2i(-sz.x()/2,                        -sz.y()/2                       );
+        auto max_E = Vector2i(offset_W + sz.x() - sz.x()/2,     min_E.y() + sz.y()              );
         return {min_E, max_E};
     }
     case (uint8_t)rotation_COUNT: {
@@ -66,10 +66,7 @@ constexpr Pair<Vector2i, Vector2i> get_value(Vector2i sz, Vector2ub div, rotatio
 
 [[maybe_unused]] constexpr bool test_offsets()
 {
-    constexpr auto min_size = iTILE_SIZE2/2;
-    static_assert(min_size.x() % 2 == 0);
-    static_assert(min_size.x() >= div && min_size.y() >= div);
-    constexpr auto sz_  = min_size;
+    constexpr auto sz_  = path_search::min_size;
     constexpr Vector2i shift = Vector2i(0, 0) * iTILE_SIZE2 + Vector2i(0, 0);
 
     [[maybe_unused]] constexpr auto N = get_value(sz_, {1,1}, rotation::N);
@@ -125,15 +122,6 @@ static_assert(test_offsets());
 
 static_assert(test_offsets2());
 
-constexpr Vector2i tile_subdiv_at(Vector2i subdiv)
-{
-    constexpr auto tile_start_offset = iTILE_SIZE2/-2;
-    constexpr auto shift = iTILE_SIZE2/div;
-    static_assert(shift * div == iTILE_SIZE2);
-    fm_debug_assert(Vector2ui(subdiv) < Vector2ui{(unsigned)div});
-    return tile_start_offset + shift * subdiv;
-};
-
 struct chunk_subdiv_array { Vector2i min[div], max[div]; };
 
 constexpr chunk_subdiv_array make_chunk_subdiv_array()
@@ -149,7 +137,7 @@ auto path_search::always_continue() noexcept -> const pred& { return always_cont
 
 void path_search::ensure_allocated(chunk_coords a, chunk_coords b)
 {
-    auto new_size = Math::abs(a - b) + Vector2i(3);
+    auto new_size = (Math::abs(a - b) + Vector2i(3));
     auto new_start = Vector2i(std::min(a.x, b.x), std::min(a.y, b.y)) - Vector2i(1);
     auto size1 = new_size.product();
     fm_debug_assert(size1 > 0);
@@ -241,12 +229,8 @@ bool path_search::is_passable(world& w, global_coords coord, Vector2b offset, Ve
     return is_passable(w, coord, min, max, own_id, p);
 }
 
-auto path_search::make_neighbor_tile_bbox(Vector2i coord, Vector2ub own_size, Vector2ub div, rotation r) -> bbox
+auto path_search::make_neighbor_tile_bbox(Vector2i coord, Vector2ub own_size, Vector2ub div, rotation r) -> bbox<float>
 {
-    constexpr auto min_size = iTILE_SIZE2*1/4;
-    static_assert(min_size.x() % 2 == 0);
-    static_assert(min_size.x() >= subdivide_factor && min_size.y() >= subdivide_factor);
-
     const auto shift = coord * iTILE_SIZE2;
     auto sz = Math::max(Vector2i(own_size), min_size);
     auto [min, max] = get_value(sz, div, r);
@@ -287,7 +271,7 @@ auto path_search::get_walkable_neighbor_tiles(world& w, global_coords coord, Vec
     return ns;
 }
 
-auto path_search::bbox_union(bbox bb, Vector2i coord, Vector2b offset, Vector2ub size) -> bbox
+auto path_search::bbox_union(bbox<float> bb, Vector2i coord, Vector2b offset, Vector2ub size) -> bbox<float>
 {
     auto center = coord * iTILE_SIZE2 + Vector2i(offset);
     auto min = center - Vector2i(size / 2u);
@@ -298,13 +282,16 @@ auto path_search::bbox_union(bbox bb, Vector2i coord, Vector2b offset, Vector2ub
     };
 }
 
-void path_search::fill_cache_(world& w, chunk_coords_ coord, Vector2ub own_size, object_id own_id, const pred& p)
+void path_search::fill_cache_(world& w, chunk_coords_ coord, Vector2ub own_size_, object_id own_id, const pred& p)
 {
+    auto own_size = Math::max(Vector2i(own_size_), path_search::min_size);
+
     int32_t x = coord.x, y = coord.y;
     int8_t z = coord.z;
 
     auto off = Vector2i(x - cache.start.x(), y - cache.start.y());
     fm_debug_assert(off >= Vector2i{} && off < cache.size);
+    static_assert(iTILE_SIZE2 / div * div == iTILE_SIZE2);
     auto ch = chunk_coords_{(int16_t)x, (int16_t)y, z};
     auto* c = w.at(ch);
     auto nb = w.neighbors(ch);
@@ -312,16 +299,43 @@ void path_search::fill_cache_(world& w, chunk_coords_ coord, Vector2ub own_size,
     if (!c && std::all_of(nb.begin(), nb.end(), [](const auto& c) { return c.c == nullptr; }))
         return;
 
+    const auto [min_N_, max_N_] = get_value(Vector2i(own_size), Vector2ub(div), rotation::N);
+    const auto [min_W_, max_W_] = get_value(Vector2i(own_size), Vector2ub(div), rotation::W);
+
+    const auto min_N = Vector2(min_N_), max_N = Vector2(max_N_),
+               min_W = Vector2(min_W_), max_W = Vector2(max_W_);
+
+    enum : unsigned { N, E, S, W };
+
+    constexpr auto tile_start = TILE_SIZE2/-2;
+    constexpr auto part_size = Vector2(iTILE_SIZE2/Vector2i(div));
+
     auto& bits = cache.array[off.y()*cache.size.x()+off.x()];
-    for (auto i = 0uz; i < TILE_COUNT; i++)
+    constexpr auto stride = TILE_MAX_DIM*(size_t)div;
+
+    for (auto j = 0uz; j < TILE_MAX_DIM; j++)
     {
-        auto pos = Vector2i(local_coords{i});
-        auto bb_N = make_neighbor_tile_bbox(pos, own_size, {1,1}, rotation::N),
-             bb_W = make_neighbor_tile_bbox(pos, own_size, {1,1}, rotation::W);
-        bool b_N = is_passable_(c, nb, bb_N.min, bb_N.max, own_id, p),
-             b_W = is_passable_(c, nb, bb_W.min, bb_W.max, own_id, p);
-        bits.can_go_north[i] = b_N;
-        bits.can_go_west[i] = b_W;
+        for (auto i = 0uz; i < TILE_MAX_DIM; i++)
+        {
+            const auto pos_ = tile_start + Vector2(i, j) * TILE_SIZE2;
+
+            for (auto ky = 0uz; ky < div; ky++)
+            {
+                for (auto kx = 0uz; kx < div; kx++)
+                {
+                    auto pos = pos_ + part_size*Vector2(kx, ky);
+                    auto bb_N = bbox<float> { pos + min_N, pos + max_N };
+                    auto bb_W = bbox<float> { pos + min_W, pos + max_W };
+                    bool b_N = is_passable_(c, nb, bb_N.min, bb_N.max, own_id, p),
+                         b_W = is_passable_(c, nb, bb_W.min, bb_W.max, own_id, p);
+                    auto jj = j * div + ky, ii = i * div + kx;
+                    auto index = jj * stride + ii;
+                    fm_debug_assert(index < bits.can_go_north.size());
+                    bits.can_go_north[index] = b_N;
+                    bits.can_go_west[index] = b_W;
+                }
+            }
+        }
     }
 }
 
diff --git a/src/path-search.hpp b/src/path-search.hpp
index 92d113cf..775cb618 100644
--- a/src/path-search.hpp
+++ b/src/path-search.hpp
@@ -12,6 +12,8 @@
 #include <Corrade/Containers/BitArray.h>
 #include <Corrade/Containers/StridedDimensions.h>
 #include <Magnum/Math/Vector2.h>
+#include <Magnum/DimensionTraits.h>
+
 
 namespace Corrade::Containers {
 template<typename T> class Optional;
@@ -57,6 +59,7 @@ class path_search final
 
 public:
     static constexpr int subdivide_factor = 4;
+    static constexpr auto min_size = iTILE_SIZE2 / subdivide_factor / 2;
     static constexpr size_t tile_count = Vector2i(subdivide_factor * TILE_MAX_DIM).product();
 
     struct neighbors final
@@ -84,7 +87,7 @@ public:
     chunk_cache cache;
     Array<global_coords> output;
 
-    struct bbox { Vector2 min, max; };
+    template<typename T> struct bbox { VectorTypeFor<2, T> min, max; };
 
     using pred = fu2::function_view<path_search_continue(collision_data) const>;
     static const pred& never_continue() noexcept;
@@ -105,8 +108,8 @@ public:
     static bool is_passable(world& w, chunk_coords_ ch0, Vector2 min, Vector2 max, object_id own_id, const pred& p = never_continue());
     static bool is_passable(world& w, global_coords coord, Vector2b offset, Vector2ub size, object_id own_id, const pred& p = never_continue());
 
-    static bbox make_neighbor_tile_bbox(Vector2i coord, Vector2ub own_size, Vector2ub div, rotation r);
-    static bbox bbox_union(bbox bb, Vector2i coord, Vector2b offset, Vector2ub size);
+    static bbox<float> make_neighbor_tile_bbox(Vector2i coord, Vector2ub own_size, Vector2ub div, rotation r);
+    static bbox<float> bbox_union(bbox<float> bb, Vector2i coord, Vector2b offset, Vector2ub size);
     static neighbors get_walkable_neighbor_tiles(world& w, global_coords coord, Vector2ub size, object_id own_id, const pred& p = never_continue());
 };
 
diff --git a/test/path-search.cpp b/test/path-search.cpp
index e54474cf..ecd631e4 100644
--- a/test/path-search.cpp
+++ b/test/path-search.cpp
@@ -10,13 +10,16 @@ namespace floormat {
 
 namespace {
 
+constexpr auto div = path_search::subdivide_factor;
+template<typename T> using bbox = path_search::bbox<T>;
+
 void test_bbox()
 {
-    static constexpr auto is_passable_1 = [](chunk& c, path_search::bbox bb) {
+    static constexpr auto is_passable_1 = [](chunk& c, bbox<float> bb) {
         return path_search::is_passable_1(c, bb.min, bb.max, (object_id)-1);
     };
 
-    static constexpr auto is_passable = [](world& w, chunk_coords_ ch, path_search::bbox bb) {
+    static constexpr auto is_passable = [](world& w, chunk_coords_ ch, bbox<float> bb) {
         return path_search::is_passable(w, ch, bb.min, bb.max, (object_id)-1);
     };
 
@@ -132,37 +135,57 @@ void test_bbox()
             fm_assert(ch_ >= Vector2i{} && ch_ < search.cache.size);
             return ch_.y()*search.cache.size.x() + ch_.x();
         };
-        static constexpr auto t_idx = [](local_coords tile) constexpr {
-            return (size_t)tile.y * TILE_MAX_DIM + (size_t)tile.x;
+        static constexpr auto t_idx = [](local_coords tile, Vector2i subdiv) constexpr {
+            constexpr auto stride = TILE_MAX_DIM * (size_t)div;
+            auto jj = tile.y * (size_t)div + (size_t)subdiv.y();
+            auto ii = tile.x * (size_t)div + (size_t)subdiv.x();
+            return jj * stride + ii;
         };
-        constexpr auto check_N = [&](path_search& search, chunk_coords ch, local_coords tile) {
-            return search.cache.array[c_idx(search, ch)].can_go_north[t_idx(tile)];
+        constexpr auto check_N = [&](path_search& search, chunk_coords ch, local_coords tile, Vector2i subdiv) {
+            auto c = c_idx(search, ch);
+            auto t = t_idx(tile, subdiv);
+            return search.cache.array[c].can_go_north[t];
         };
-        constexpr auto check_W = [&](path_search& search, chunk_coords ch, local_coords tile) {
-            return search.cache.array[c_idx(search, ch)].can_go_west[t_idx(tile)];
+        constexpr auto check_W = [&](path_search& search, chunk_coords ch, local_coords tile, Vector2i subdiv) {
+            auto c = c_idx(search, ch);
+            auto t = t_idx(tile, subdiv);
+            return search.cache.array[c].can_go_west[t];
         };
 
-        fm_assert( !check_N(search, {}, {  0,   0} ));
-        fm_assert( !check_W(search, {}, {  0,   0} ));
-        fm_assert(  check_N(search, {}, {  0,   1} ));
-        fm_assert(  check_W(search, {}, {  1,   0} ));
-
-        fm_assert(  check_W(search, {}, {K-1, K  } ));
-        fm_assert(  check_N(search, {}, {K-1, K  } ));
-        fm_assert(  check_W(search, {}, {K,   K-1} ));
-        fm_assert(  check_N(search, {}, {K,   K-1} ));
-
-        fm_assert( !check_N(search, {}, {K,   K  } ));
-        fm_assert( !check_W(search, {}, {K,   K  } ));
-        fm_assert( !check_W(search, {}, {K+1, K  } ));
-        fm_assert(  check_N(search, {}, {K+1, K  } ));
-        fm_assert( !check_N(search, {}, {K,   K+1} ));
-        fm_assert(  check_W(search, {}, {K,   K+1} ));
-
-        fm_assert(  check_N(search, {}, {K+2, K  } ));
-        fm_assert(  check_W(search, {}, {K+2, K  } ));
-        fm_assert(  check_N(search, {}, {K,   K+2} ));
-        fm_assert(  check_W(search, {}, {K,   K+2} ));
+        static_assert(div == 4);
+        constexpr Vector2i s00 = {0,0}, s01 = {0,2}, s10 = {2,0}, s11 = {2,2}, s22 = {3,3};
+
+        fm_assert( !check_N(search, {}, {  0,   0}, s10 ));
+        fm_assert( !check_W(search, {}, {  0,   0}, s01 ));
+        fm_assert(  check_N(search, {}, {  0,   1}, s10 ));
+        fm_assert(  check_W(search, {}, {  1,   0}, s01 ));
+
+        fm_assert( !check_N(search, {}, {  0,   1}, s00 ));
+        fm_assert( !check_W(search, {}, {  1,   0}, s00 ));
+
+        fm_assert(  check_W(search, {}, {K-1, K  }, s01 ));
+        fm_assert(  check_N(search, {}, {K-1, K  }, s10 ));
+        fm_assert(  check_W(search, {}, {K,   K-1}, s01 ));
+        fm_assert(  check_N(search, {}, {K,   K-1}, s10 ));
+
+        fm_assert(  check_W(search, {}, {K-1, K  }, s22 ));
+        fm_assert(  check_N(search, {}, {K-1, K  }, s22 ));
+        fm_assert(  check_W(search, {}, {K,   K-1}, s22 ));
+        fm_assert(  check_N(search, {}, {K,   K-1}, s22 ));
+
+        fm_assert( !check_N(search, {}, {K,   K  }, s10 ));
+        fm_assert( !check_W(search, {}, {K,   K  }, s01 ));
+        fm_assert(  check_N(search, {}, {K,   K  }, s11 ));
+        fm_assert(  check_W(search, {}, {K,   K  }, s11 ));
+
+        fm_assert( !check_W(search, {}, {K+1, K  }, s01 ));
+        fm_assert(  check_N(search, {}, {K+1, K  }, s10 ));
+        fm_assert( !check_N(search, {}, {K,   K+1}, s10 ));
+        fm_assert(  check_W(search, {}, {K,   K+1}, s01 ));
+        fm_assert(  check_N(search, {}, {K+2, K  }, s10 ));
+        fm_assert(  check_W(search, {}, {K+2, K  }, s01 ));
+        fm_assert(  check_N(search, {}, {K,   K+2}, s10 ));
+        fm_assert(  check_W(search, {}, {K,   K+2}, s01 ));
     }
 }