From c05a6a1ad267c00d5fe261d5f784a3bb2a34b04f Mon Sep 17 00:00:00 2001
From: Adrian Scripca <benishor@gmail.com>
Date: Sun, 14 May 2023 00:03:36 +0300
Subject: [PATCH] Added bi-linear interpolation for getting Z coordinate at any
 location in the grid.

---
 CMakeLists.txt     |  3 ++-
 heightmap.cpp      | 35 +++++++++++++++++++++++++++++++++++
 heightmap.h        |  2 ++
 heightmap_test.cpp | 26 ++++++++++++++++++++++++++
 main.cpp           |  1 -
 render.cpp         |  2 +-
 6 files changed, 66 insertions(+), 3 deletions(-)
 create mode 100644 heightmap_test.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9f816dc..e434680 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -24,6 +24,7 @@ set_property(TARGET glfw glfw_objects nanogui PROPERTY FOLDER "dependencies")
 set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_VISIBILITY_PRESET hidden)
 
+set(TESTS grbl_test.cpp heightmap_test.cpp)
 
-add_executable(sender main.cpp grbl.h grbl.cpp grbl_test.cpp grbl_communication.h grbl_communication.cpp grbl_machine.h grbl_machine.cpp string_utils.h render.h render.cpp heightmap.h heightmap.cpp)
+add_executable(sender main.cpp grbl.h grbl.cpp ${TESTS} grbl_communication.h grbl_communication.cpp grbl_machine.h grbl_machine.cpp string_utils.h render.h render.cpp heightmap.h heightmap.cpp)
 target_link_libraries(sender nanogui GL gtest gtest_main)
\ No newline at end of file
diff --git a/heightmap.cpp b/heightmap.cpp
index af0623f..b3f57eb 100644
--- a/heightmap.cpp
+++ b/heightmap.cpp
@@ -27,3 +27,38 @@ grbl::heightmap grbl::heightmap::from_params(float from_x, float from_y, float t
 
     return result;
 }
+
+size_t grbl::heightmap::index_from_coords(float x, float y) const {
+    size_t result = 0;
+    for (auto& v: vertices) {
+        if (v.x == x && v.y == y) {
+            return result;
+        }
+        result++;
+    }
+    return 0;
+}
+
+// bi-linear interpolation
+float grbl::heightmap::get_z_at(float x, float y) const {
+
+    // TODO: make faster by precalculating indices
+
+    float x1 = x - fmodf(x, resolution);
+    float x2 = x1 + resolution;
+    float y1 = y - fmodf(y, resolution);
+    float y2 = y1 + resolution;
+    float z11 = vertices[index_from_coords(x1, y1)].z;
+    float z12 = vertices[index_from_coords(x1, y2)].z;
+    float z21 = vertices[index_from_coords(x2, y1)].z;
+    float z22 = vertices[index_from_coords(x2, y2)].z;
+
+    float alpha_x = (x - x1) / resolution;
+    float a = z11 + (z21 - z11) * alpha_x;
+    float b = z12 + (z22 - z12) * alpha_x;
+
+    float alpha_y = (y - y1) / resolution;
+    float c = a + (b - a) * alpha_y;
+
+    return c;
+}
diff --git a/heightmap.h b/heightmap.h
index 1d2c29b..7e659c5 100644
--- a/heightmap.h
+++ b/heightmap.h
@@ -9,6 +9,8 @@ namespace grbl {
 
 struct heightmap {
     static heightmap from_params(float from_x, float from_y, float to_x, float to_y, float resolution);
+    float get_z_at(float x, float y) const;
+    size_t index_from_coords(float x, float y) const;
 
     float from_x, from_y;
     float to_x, to_y;
diff --git a/heightmap_test.cpp b/heightmap_test.cpp
new file mode 100644
index 0000000..be5bb56
--- /dev/null
+++ b/heightmap_test.cpp
@@ -0,0 +1,26 @@
+#include <gtest/gtest.h>
+
+#include "heightmap.h"
+
+TEST(heightmap, get_z_at) {
+    auto grid = grbl::heightmap::from_params(0, 0, 1, 1, 1);
+    grid.vertices[0].z = 0;
+    grid.vertices[1].z = 1;
+    grid.vertices[2].z = 2;
+    grid.vertices[3].z = 3;
+
+    auto z = grid.get_z_at(0.5f, 0.5f);
+    EXPECT_EQ(1.5, z);
+
+    z = grid.get_z_at(0.5f, 0);
+    EXPECT_EQ(0.5, z);
+
+    z = grid.get_z_at(0.5f, 1);
+    EXPECT_EQ(2.5, z);
+
+    z = grid.get_z_at(0, 0.5f);
+    EXPECT_EQ(1, z);
+
+    z = grid.get_z_at(1, 0.5f);
+    EXPECT_EQ(2, z);
+}
\ No newline at end of file
diff --git a/main.cpp b/main.cpp
index 7cc423c..9b0a933 100644
--- a/main.cpp
+++ b/main.cpp
@@ -1104,7 +1104,6 @@ private:
 
     using ImageHolder = std::unique_ptr<uint8_t[], void (*)(void *)>;
     std::vector<std::pair<ref<Texture>, ImageHolder>> m_images;
-    int m_current_image;
 };
 
 
diff --git a/render.cpp b/render.cpp
index 5619a92..44b9683 100644
--- a/render.cpp
+++ b/render.cpp
@@ -508,6 +508,7 @@ std::string get_shader_info_log(GLuint id) {
     char error_log[log_length]; // length includes the NULL character
     glGetShaderInfoLog(id, log_length, &log_length, &error_log[0]);
 
+
     return std::string(error_log, log_length);
 }
 
@@ -521,7 +522,6 @@ std::string get_program_info_log(GLuint id) {
     return {error_log, (size_t) log_length};
 }
 
-
 bool check_compile_error(GLuint shader_id) {
     GLint is_compiled = 0;
     glGetShaderiv(shader_id, GL_COMPILE_STATUS, &is_compiled);