w

1 files changed, 111 insertions, 14 deletions

diff --git a/editor/tests/raycast-test.cpp b/editor/tests/raycast-test.cpp
index adabb0bf..2953ae69 100644
--- a/editor/tests/raycast-test.cpp
+++ b/editor/tests/raycast-test.cpp
@@ -1,5 +1,11 @@
 #include "../tests-private.hpp"
+#include "editor/app.hpp"
+#include "floormat/main.hpp"
+#include "compat/shared-ptr-wrapper.hpp"
+#include "src/critter.hpp"
+#include <memory>
 #include <array>
+#include <vector>
 #include <Magnum/Math/Functions.h>
 #include <Magnum/Math/Vector2.h>
 
@@ -13,7 +19,9 @@ struct aabb_result
     bool result;
 };
 
-std::array<uint8_t, 2> ray_aabb_signs(Vector2 ray_dir_inv_norm)
+template<typename T>
+requires std::is_arithmetic_v<T>
+std::array<uint8_t, 2> ray_aabb_signs(Math::Vector2<T> ray_dir_inv_norm)
 {
     bool signs[2];
     for (unsigned d = 0; d < 2; ++d)
@@ -21,19 +29,6 @@ std::array<uint8_t, 2> ray_aabb_signs(Vector2 ray_dir_inv_norm)
     return { signs[0], signs[1] };
 }
 
-Vector2 dir_to_inv_norm(Vector2 ray_dir)
-{
-    constexpr float eps = 1e-6f;
-    auto dir = ray_dir.normalized();
-    Vector2 inv_dir{NoInit};
-    for (unsigned i = 0; i < 2; i++)
-        if (Math::abs(dir[i]) < eps)
-            inv_dir[i] = 0;
-        else
-            inv_dir[i] = 1 / dir[i];
-    return inv_dir;
-}
-
 // https://tavianator.com/2022/ray_box_boundary.html
 // https://www.researchgate.net/figure/The-slab-method-for-ray-intersection-detection-15_fig3_283515372
 aabb_result ray_aabb_intersection(Vector2 ray_origin, Vector2 ray_dir_inv_norm,
@@ -61,10 +56,33 @@ aabb_result ray_aabb_intersection(Vector2 ray_origin, Vector2 ray_dir_inv_norm,
     return { {ts[0], ts[1] }, tmin < tmax };
 }
 
+struct bbox
+{
+    point center;
+    Vector2ub size;
+};
+
+struct result_s
+{
+    point from, to;
+    Vector3d vec;
+    std::vector<bbox> path;
+    bool has_result : 1 = false;
+};
+
+struct pending_s
+{
+    point from, to;
+    bool exists : 1 = false;
+};
+
 } // namespace
 
 struct raycast_test : base_test
 {
+    result_s result;
+    pending_s pending;
+
     ~raycast_test() noexcept override;
 
     bool handle_key(app& a, const key_event& e, bool is_down) override
@@ -74,6 +92,19 @@ struct raycast_test : base_test
 
     bool handle_mouse_click(app& a, const mouse_button_event& e, bool is_down) override
     {
+        if (e.button == mouse_button_left && is_down)
+        {
+            auto& M = a.main();
+            auto& w = M.world();
+            if (auto pt_ = a.cursor_state().point())
+            {
+                auto C = a.ensure_player_character(w).ptr;
+                auto pt0 = C->position();
+                pending = { .from = pt0, .to = *pt_, .exists = true, };
+                return true;
+            }
+        }
+
         return false;
     }
 
@@ -99,7 +130,73 @@ struct raycast_test : base_test
 
     void update_post(app& a) override
     {
+        if (pending.exists)
+        {
+            pending.exists = false;
+            if (pending.from.chunk3().z != pending.to.chunk3().z)
+            {
+                fm_warn("raycast: wrong Z value");
+                return;
+            }
+            if (pending.from == pending.to)
+            {
+                fm_warn("raycast: from == to");
+                return;
+            }
+
+            do_raycasting(pending.from, pending.to);
+        }
+    }
 
+    void do_raycasting(point from, point to)
+    {
+        constexpr auto inv_tile_size = 1. / Vector2d(iTILE_SIZE2);
+        constexpr auto chunk_size = Vector2d{TILE_MAX_DIM};
+        constexpr double eps = 1e-8;
+        constexpr double sqrt_2 = Math::sqrt(2.);
+        constexpr double inv_sqrt_2 = 1. / sqrt_2;
+
+        auto vec = Vector2d{};
+        vec += (Vector2d(to.chunk()) - Vector2d(from.chunk())) * chunk_size;
+        vec += Vector2d(to.local()) - Vector2d(from.local());
+        vec += (Vector2d(to.offset()) - Vector2d(from.offset())) * inv_tile_size;
+
+        auto dir = vec.normalized();
+
+        if (Math::abs(dir.x()) < eps && Math::abs(dir.y()) < eps)
+        {
+            fm_error("raycast: bad dir? {%f, %f}", dir.x(), dir.y());
+            return;
+        }
+
+        double step;
+        unsigned long_axis, short_axis;
+
+        if (Math::abs(dir.y()) > Math::abs(dir.x()))
+        {
+            long_axis = 1;
+            short_axis = 0;
+        }
+        else
+        {
+            long_axis = 0;
+            short_axis = 1;
+        }
+
+        if (Math::abs(dir[short_axis]) < eps)
+            step = chunk_size[short_axis] * .5;
+        else
+        {
+            step = Math::abs(inv_sqrt_2 / dir[short_axis]);
+            Debug{} << "step" << step;
+            step = Math::clamp(step, 1., chunk_size[short_axis] * .5);
+        }
+
+        auto dir_inv_norm = Vector2d{
+            Math::abs(dir.x()) < eps ? 0. : 1. / dir.x(),
+            Math::abs(dir.y()) < eps ? 0. : 1. / dir.y(),
+        };
+        auto signs = ray_aabb_signs(dir_inv_norm);
     }
 };