Added bi-linear interpolation for getting Z coordinate at any location in the grid.

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;
+}