a

1 files changed, 64 insertions, 27 deletions

diff --git a/src/path-search-dijkstra.cpp b/src/path-search-dijkstra.cpp
index 844a2c0f..df8dd65b 100644
--- a/src/path-search-dijkstra.cpp
+++ b/src/path-search-dijkstra.cpp
@@ -117,7 +117,7 @@ path_search_result astar::Dijkstra(world& w, const point from_, const point to_,
 
     constexpr auto min_size_ = Vector2ui{Vector2(div_size) * 1.5f + Vector2{.5f}};
     const auto own_size = Math::max(Vector2ui{Vector2{own_size_}*1.5f + Vector2{.5f}}, min_size_);
-    const auto goal_distance = (uint32_t)(own_size * 2).length();
+    const auto goal_distance = (uint32_t)Math::max(Vector2ui(iTILE_SIZE2*11/10), own_size * 2).length();
 
     const auto [from, from_offset] = from_;
     const auto [to, to_offset] = to_;
@@ -168,6 +168,8 @@ path_search_result astar::Dijkstra(world& w, const point from_, const point to_,
     auto closest = distance(from_, to_);
     auto closest_pos = from_;
     uint32_t closest_path_len = 0;
+    auto goal_idx = (uint32_t)-1;
+    const auto goal_bb = bbox_from_pos(Vector2(to_.local()), to_.offset(), own_size);
 
     while (!Q.empty())
     {
@@ -194,6 +196,44 @@ path_search_result astar::Dijkstra(world& w, const point from_, const point to_,
 #endif
 
         const auto bb0 = bbox_from_pos(Vector2(cur_pt.local()), cur_pt.offset(), own_size);
+
+        if (auto dist_to_goal = distance(cur_pt, to_); dist_to_goal < goal_distance) [[unlikely]]
+        {
+            if (auto dist = cur_dist + dist_to_goal; dist < max_dist)
+            {
+                auto vec = Vector2(cur_pt.coord() - to_.coord()) * TILE_SIZE2 + Vector2(cur_pt.offset() - to_.offset());
+                auto bb1 = bbox<float>{goal_bb.min + vec, goal_bb.max + vec};
+                auto bb = bbox_union(goal_bb, bb1);
+                if (path_search::is_passable(w, to_.chunk3(), bb, own_id, p))
+                {
+                    if (goal_idx != (uint32_t)-1)
+                    {
+                        auto& n = nodes[goal_idx];
+                        if (dist >= n.dist)
+                            continue;
+                        n.dist = dist;
+                        n.prev = id;
+                    }
+                    else
+                    {
+                        auto chunk_idx = cache.get_chunk_index(Vector2i(to_.chunk()));
+                        auto tile_idx = cache.get_tile_index(Vector2i(to_.local()), to_offset);
+                        goal_idx = (uint32_t)nodes.size();
+                        cache.add_index(chunk_idx, tile_idx, goal_idx);
+                        auto new_node = visited {
+                            .dist = dist, .prev = id,
+                            .pt = to_,
+                        };
+                        nodes.push_back(new_node);
+                        auto idx = cache.lookup_index(chunk_idx, tile_idx);
+                        fm_assert(idx == goal_idx);
+                    }
+                    max_dist = dist;
+                    continue; // path can only get longer
+                }
+            }
+        }
+
         for (auto [vec, len] : directions)
         {
             const auto dist = cur_dist + len;
@@ -211,37 +251,18 @@ path_search_result astar::Dijkstra(world& w, const point from_, const point to_,
 
             if (new_idx != (uint32_t)-1)
             {
-                fresh = false;
-
                 if (nodes[new_idx].dist <= dist)
                     continue;
+                fresh = false;
+
             }
 
-#if 1
             {   auto vec_ = Vector2(vec);
                 auto bb1 = bbox<float>{ bb0.min + vec_, bb0.max + vec_ };
-                auto bb = bbox_union(bb1, bb0);
+                auto bb = bbox_union(bb0, bb1);
                 if (!path_search::is_passable(w, new_coord.chunk3(), bb, own_id, p))
                     continue;
             }
-#else
-            auto e = make_edge(cur_pt, new_pt);
-            if (auto [it, fresh] = edges.try_emplace(e, edge_status::unknown); fresh)
-            {
-                auto& status = it.value();
-                auto vec_ = Vector2(vec);
-                auto bb1 = bbox<float>{ bb0.min + vec_, bb0.max + vec_ };
-                auto bb = bbox_union(bb1, bb0);
-
-                if (path_search::is_passable(w, new_coord.chunk3(), bb, own_id, p))
-                    status = edge_status::good;
-                else
-                {
-                    status = edge_status::bad;
-                    continue;
-                }
-            }
-#endif
 
             if (fresh)
             {   const auto sz = nodes.size();
@@ -253,6 +274,12 @@ path_search_result astar::Dijkstra(world& w, const point from_, const point to_,
                 };
                 nodes.push_back(new_node);
             }
+            else
+            {
+                auto& n = nodes[new_idx];
+                n.dist = dist;
+                n.prev = id;
+            }
 
 #ifndef FM_NO_DEBUG
             if (debug >= 3) [[unlikely]]
@@ -269,10 +296,19 @@ path_search_result astar::Dijkstra(world& w, const point from_, const point to_,
     //fm_debug_assert(nodes.size() == indexes.size());
 #ifndef FM_NO_DEBUG
     if (debug >= 1)
-        DBG_nospace << "dijkstra: closest px:" << closest
-                    << " path:" << closest_path_len
-                    << " pos:" << closest_pos
-                    << " nodes:" << nodes.size();
+    {
+        auto dbg = DBG_nospace;
+        dbg << "dijkstra: ";
+        if (goal_idx != (uint32_t)-1)
+            dbg << "found len:" << nodes[goal_idx].dist;
+        else
+        {
+            dbg << "closest px:" << closest
+                << " path:" << closest_path_len
+                << " pos:" << closest_pos;
+        }
+         dbg << " nodes:" << nodes.size();
+    }
 #endif
 
     // todo...
@@ -366,6 +402,7 @@ size_t astar::cache::get_tile_index(Vector2i pos, Vector2b offset_)
 
 void astar::cache::add_index(size_t chunk_index, size_t tile_index, uint32_t index)
 {
+    fm_debug_assert(index != (uint32_t)-1);
     auto& c = array[chunk_index];
     fm_debug_assert(!c.exists[tile_index]);
     c.exists[tile_index] = true;