Refactor Hilbert curve. Enhance Touch UI Bed Level Screen. (MarlinFirmware#21453)

Author: marciot

Marlin/src/feature/bedlevel/hilbert_curve.cpp

@@ -0,0 +1,112 @@
+/*********************
+ * hilbert_curve.cpp *
+ *********************/
+
+/****************************************************************************
+ *   Written By Marcio Teixeira 2021 - SynDaver Labs, Inc.                  *
+ *                                                                          *
+ *   This program is free software: you can redistribute it and/or modify   *
+ *   it under the terms of the GNU General Public License as published by   *
+ *   the Free Software Foundation, either version 3 of the License, or      *
+ *   (at your option) any later version.                                    *
+ *                                                                          *
+ *   This program is distributed in the hope that it will be useful,        *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of         *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the          *
+ *   GNU General Public License for more details.                           *
+ *                                                                          *
+ *   To view a copy of the GNU General Public License, go to the following  *
+ *   location: <https://www.gnu.org/licenses/>.                             *
+ ****************************************************************************/
+
+#include "../../inc/MarlinConfig.h"
+
+#if ENABLED(UBL_HILBERT_CURVE)
+
+#include "bedlevel.h"
+#include "hilbert_curve.h"
+
+constexpr int8_t  to_fix(int8_t  v) { return v * 2; }
+constexpr int8_t  to_int(int8_t  v) { return v / 2; }
+constexpr uint8_t   log2(uint8_t n) { return (n > 1) ? 1 + log2(n >> 1) : 0; }
+constexpr uint8_t  order(uint8_t n) { return uint8_t(log2(n - 1)) + 1; }
+constexpr uint8_t ord = order(_MAX(GRID_MAX_POINTS_X, GRID_MAX_POINTS_Y));
+constexpr uint8_t dim = _BV(ord);
+
+static inline bool eval_candidate(int8_t x, int8_t y, hilbert_curve::callback_ptr func, void *data) {
+  // The print bed likely has fewer points than the full Hilbert
+  // curve, so cull unecessary points
+  return x < GRID_MAX_POINTS_X && y < GRID_MAX_POINTS_Y ? func(x, y, data) : false;
+}
+
+bool hilbert_curve::hilbert(int8_t x, int8_t y, int8_t xi, int8_t xj, int8_t yi, int8_t yj, uint8_t n, hilbert_curve::callback_ptr func, void *data) {
+  /**
+   * Hilbert space-filling curve implementation
+   *
+   * x  and y  : coordinates of the bottom left corner
+   * xi and xj : i and j components of the unit x vector of the frame
+   * yi and yj : i and j components of the unit y vector of the frame
+   *
+   * From: http://www.fundza.com/algorithmic/space_filling/hilbert/basics/index.html
+   */
+  if (n)
+    return hilbert(x,           y,           yi/2,  yj/2,  xi/2,  xj/2, n-1, func, data) ||
+           hilbert(x+xi/2,      y+xj/2,      xi/2,  xj/2,  yi/2,  yj/2, n-1, func, data) ||
+           hilbert(x+xi/2+yi/2, y+xj/2+yj/2, xi/2,  xj/2,  yi/2,  yj/2, n-1, func, data) ||
+           hilbert(x+xi/2+yi,   y+xj/2+yj,  -yi/2, -yj/2, -xi/2, -xj/2, n-1, func, data);
+  else
+    return eval_candidate(to_int(x+(xi+yi)/2), to_int(y+(xj+yj)/2), func, data);
+}
+
+/**
+ * Calls func(x, y, data) for all points in the Hilbert curve.
+ * If that function returns true, the search is terminated.
+ */
+bool hilbert_curve::search(hilbert_curve::callback_ptr func, void *data) {
+  return hilbert(to_fix(0), to_fix(0),to_fix(dim), to_fix(0), to_fix(0), to_fix(dim), ord, func, data);
+}
+
+/* Helper function for starting the search at a particular point */
+
+typedef struct {
+  uint8_t x, y;
+  bool found_1st;
+  hilbert_curve::callback_ptr func;
+  void *data;
+} search_from_t;
+
+static bool search_from_helper(uint8_t x, uint8_t y, void *data) {
+  search_from_t *d = (search_from_t *) data;
+  if (d->x == x && d->y == y)
+    d->found_1st = true;
+  return d->found_1st ? d->func(x, y, d->data) : false;
+}
+
+/**
+ * Same as search, except start at a specific grid intersection point.
+ */
+bool hilbert_curve::search_from(uint8_t x, uint8_t y, hilbert_curve::callback_ptr func, void *data) {
+  search_from_t d;
+  d.x = x;
+  d.y = y;
+  d.found_1st = false;
+  d.func = func;
+  d.data = data;
+  // Call twice to allow search to wrap back to the beginning and picked up points prior to the start.
+  return search(search_from_helper, &d) || search(search_from_helper, &d);
+}
+
+/**
+ * Like search_from, but takes a bed position and starts from the nearest
+ * point on the Hilbert curve.
+ */
+bool hilbert_curve::search_from_closest(const xy_pos_t &pos, hilbert_curve::callback_ptr func, void *data) {
+  // Find closest grid intersection
+  uint8_t grid_x = LROUND(float(pos.x - MESH_MIN_X) / MESH_X_DIST);
+  uint8_t grid_y = LROUND(float(pos.y - MESH_MIN_Y) / MESH_Y_DIST);
+  LIMIT(grid_x, 0, GRID_MAX_POINTS_X);
+  LIMIT(grid_y, 0, GRID_MAX_POINTS_Y);
+  return search_from(grid_x, grid_y, func, data);
+}
+
+#endif // UBL_HILBERT_CURVE

Marlin/src/feature/bedlevel/hilbert_curve.h

@@ -0,0 +1,32 @@
+/*******************
+ * hilbert_curve.h *
+ *******************/
+
+/****************************************************************************
+ *   Written By Marcio Teixeira 2021 - SynDaver Labs, Inc.                  *
+ *                                                                          *
+ *   This program is free software: you can redistribute it and/or modify   *
+ *   it under the terms of the GNU General Public License as published by   *
+ *   the Free Software Foundation, either version 3 of the License, or      *
+ *   (at your option) any later version.                                    *
+ *                                                                          *
+ *   This program is distributed in the hope that it will be useful,        *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of         *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the          *
+ *   GNU General Public License for more details.                           *
+ *                                                                          *
+ *   To view a copy of the GNU General Public License, go to the following  *
+ *   location: <https://www.gnu.org/licenses/>.                             *
+ ****************************************************************************/
+
+#pragma once
+
+class hilbert_curve {
+  public:
+    typedef bool (*callback_ptr)(uint8_t x, uint8_t y, void *data);
+    static bool search(callback_ptr func, void *data);
+    static bool search_from(uint8_t x, uint8_t y, callback_ptr func, void *data);
+    static bool search_from_closest(const xy_pos_t &pos, callback_ptr func, void *data);
+  private:
+    static bool hilbert(int8_t x, int8_t y, int8_t xi, int8_t xj, int8_t yi, int8_t yj, uint8_t n, callback_ptr func, void *data);
+};

Marlin/src/feature/bedlevel/ubl/ubl.h

@@ -101,11 +101,6 @@ class unified_bed_leveling {
   static void display_map(const int) _O0;
   static mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType, const xy_pos_t&, const bool=false, MeshFlags *done_flags=nullptr) _O0;
   static mesh_index_pair find_furthest_invalid_mesh_point() _O0;
-  #if ENABLED(UBL_HILBERT_CURVE)
-    static void check_if_missing(mesh_index_pair &pt, int x, int y);
-    static void hilbert(mesh_index_pair &pt, int8_t x, int8_t y, int8_t xi, int8_t xj, int8_t yi, int8_t yj, uint8_t n);
-    static mesh_index_pair find_next_mesh_point();
-  #endif
   static void reset();
   static void invalidate();
   static void set_all_mesh_points_to_value(const float value);

Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp

@@ -49,6 +49,10 @@
   #include "../../../lcd/extui/ui_api.h"
 #endif
 
+#if ENABLED(UBL_HILBERT_CURVE)
+  #include "../hilbert_curve.h"
+#endif
+
 #include <math.h>
 
 #define UBL_G29_P31
@@ -747,11 +751,9 @@ void unified_bed_leveling::shift_mesh_height() {
         }
       #endif
 
-      best = do_furthest  ? find_furthest_invalid_mesh_point()
-                          : TERN(UBL_HILBERT_CURVE,
-                              find_next_mesh_point(),
-                              find_closest_mesh_point_of_type(INVALID, nearby, true)
-                            );
+      best = do_furthest
+        ? find_furthest_invalid_mesh_point()
+        : find_closest_mesh_point_of_type(INVALID, nearby, true);
 
       if (best.pos.x >= 0) {    // mesh point found and is reachable by probe
         TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(best.pos, ExtUI::PROBE_START));
@@ -1269,97 +1271,93 @@ mesh_index_pair unified_bed_leveling::find_furthest_invalid_mesh_point() {
   return farthest;
 }
 
-mesh_index_pair unified_bed_leveling::find_closest_mesh_point_of_type(const MeshPointType type, const xy_pos_t &pos, const bool probe_relative/*=false*/, MeshFlags *done_flags/*=nullptr*/) {
-  mesh_index_pair closest;
-  closest.invalidate();
-  closest.distance = -99999.9f;
-
-  // Get the reference position, either nozzle or probe
-  const xy_pos_t ref = probe_relative ? pos + probe.offset_xy : pos;
-
-  float best_so_far = 99999.99f;
+#if ENABLED(UBL_HILBERT_CURVE)
 
-  GRID_LOOP(i, j) {
-    if ( (type == (isnan(z_values[i][j]) ? INVALID : REAL))
-      || (type == SET_IN_BITMAP && !done_flags->marked(i, j))
+  typedef struct {
+    MeshPointType   type;
+    MeshFlags       *done_flags;
+    bool            probe_relative;
+    mesh_index_pair closest;
+  } find_closest_t;
+
+  static bool test_func(uint8_t i, uint8_t j, void *data) {
+    find_closest_t *d = (find_closest_t*)data;
+    if ( (d->type == (isnan(ubl.z_values[i][j]) ? INVALID : REAL))
+      || (d->type == SET_IN_BITMAP && !d->done_flags->marked(i, j))
     ) {
       // Found a Mesh Point of the specified type!
-      const xy_pos_t mpos = { mesh_index_to_xpos(i), mesh_index_to_ypos(j) };
+      const xy_pos_t mpos = { ubl.mesh_index_to_xpos(i), ubl.mesh_index_to_ypos(j) };
 
       // If using the probe as the reference there are some unreachable locations.
       // Also for round beds, there are grid points outside the bed the nozzle can't reach.
       // Prune them from the list and ignore them till the next Phase (manual nozzle probing).
 
-      if (!(probe_relative ? probe.can_reach(mpos) : position_is_reachable(mpos)))
-        continue;
+      if (!(d->probe_relative ? probe.can_reach(mpos) : position_is_reachable(mpos)))
+        return false;
+      d->closest.pos.set(i, j);
+      return true;
+    }
+    return false;
+  }
 
-      // Reachable. Check if it's the best_so_far location to the nozzle.
+#endif
 
-      const xy_pos_t diff = current_position - mpos;
-      const float distance = (ref - mpos).magnitude() + diff.magnitude() * 0.1f;
+mesh_index_pair unified_bed_leveling::find_closest_mesh_point_of_type(const MeshPointType type, const xy_pos_t &pos, const bool probe_relative/*=false*/, MeshFlags *done_flags/*=nullptr*/) {
 
-      // factor in the distance from the current location for the normal case
-      // so the nozzle isn't running all over the bed.
-      if (distance < best_so_far) {
-        best_so_far = distance;   // Found a closer location with the desired value type.
-        closest.pos.set(i, j);
-        closest.distance = best_so_far;
-      }
-    }
-  } // GRID_LOOP
+  #if ENABLED(UBL_HILBERT_CURVE)
 
-  return closest;
-}
+    find_closest_t d;
+    d.type           = type;
+    d.done_flags     = done_flags;
+    d.probe_relative = probe_relative;
+    d.closest.invalidate();
+    hilbert_curve::search_from_closest(pos, test_func, &d);
+    return d.closest;
 
-#if ENABLED(UBL_HILBERT_CURVE)
+  #else
 
-  constexpr int8_t  to_fix(int8_t  v) { return v << 1; }
-  constexpr int8_t  to_int(int8_t  v) { return v >> 1; }
-  constexpr uint8_t   log2(uint8_t n) { return (n > 1) ? 1 + log2(n >> 1) : 0; }
-  constexpr uint8_t  order(uint8_t n) { return uint8_t(log2(n - 1)) + 1; }
-
-  void unified_bed_leveling::hilbert(mesh_index_pair &pt, int8_t x, int8_t y, int8_t xi, int8_t xj, int8_t yi, int8_t yj, uint8_t n) {
-    /* Hilbert space filling curve implementation
-     *
-     * x and y are the coordinates of the bottom left corner
-     * xi & xj are the i & j components of the unit x vector of the frame
-     * similarly yi and yj
-     *
-     * From: http://www.fundza.com/algorithmic/space_filling/hilbert/basics/index.html
-     */
-    if (n <= 0)
-      check_if_missing(pt, to_int(x+(xi+yi)/2),to_int(y+(xj+yj)/2));
-    else {
-      hilbert(pt, x,           y,           yi/2,  yj/2,  xi/2,  xj/2, n-1);
-      hilbert(pt, x+xi/2,      y+xj/2,      xi/2,  xj/2,  yi/2,  yj/2, n-1);
-      hilbert(pt, x+xi/2+yi/2, y+xj/2+yj/2, xi/2,  xj/2,  yi/2,  yj/2, n-1);
-      hilbert(pt, x+xi/2+yi,   y+xj/2+yj,  -yi/2, -yj/2, -xi/2, -xj/2, n-1);
-    }
-  }
+    mesh_index_pair closest;
+    closest.invalidate();
+    closest.distance = -99999.9f;
+
+    // Get the reference position, either nozzle or probe
+    const xy_pos_t ref = probe_relative ? pos + probe.offset_xy : pos;
 
-  void unified_bed_leveling::check_if_missing(mesh_index_pair &pt, int x, int y) {
-      if (   pt.distance < 0
-          && x < GRID_MAX_POINTS_X
-          && y < GRID_MAX_POINTS_Y
-          && isnan(z_values[x][y])
-          && probe.can_reach(mesh_index_to_xpos(x), mesh_index_to_ypos(y))
+    float best_so_far = 99999.99f;
+
+    GRID_LOOP(i, j) {
+      if ( (type == (isnan(z_values[i][j]) ? INVALID : REAL))
+        || (type == SET_IN_BITMAP && !done_flags->marked(i, j))
       ) {
-        pt.pos.set(x, y);
-        pt.distance = 1;
+        // Found a Mesh Point of the specified type!
+        const xy_pos_t mpos = { mesh_index_to_xpos(i), mesh_index_to_ypos(j) };
+
+        // If using the probe as the reference there are some unreachable locations.
+        // Also for round beds, there are grid points outside the bed the nozzle can't reach.
+        // Prune them from the list and ignore them till the next Phase (manual nozzle probing).
+
+        if (!(probe_relative ? probe.can_reach(mpos) : position_is_reachable(mpos)))
+          continue;
+
+        // Reachable. Check if it's the best_so_far location to the nozzle.
+
+        const xy_pos_t diff = current_position - mpos;
+        const float distance = (ref - mpos).magnitude() + diff.magnitude() * 0.1f;
+
+        // factor in the distance from the current location for the normal case
+        // so the nozzle isn't running all over the bed.
+        if (distance < best_so_far) {
+          best_so_far = distance;   // Found a closer location with the desired value type.
+          closest.pos.set(i, j);
+          closest.distance = best_so_far;
+        }
       }
-   }
-
-   mesh_index_pair unified_bed_leveling::find_next_mesh_point() {
-     mesh_index_pair pt;
-     pt.invalidate();
-     pt.distance = -99999.9f;
-     constexpr uint8_t ord = order(_MAX(GRID_MAX_POINTS_X, GRID_MAX_POINTS_Y));
-     constexpr uint8_t dim = _BV(ord);
-     hilbert(pt, to_fix(0), to_fix(0), to_fix(dim), to_fix(0), to_fix(0), to_fix(dim), ord);
-     return pt;
-   }
-
-#endif // UBL_HILBERT_CURVE
+    } // GRID_LOOP
+
+    return closest;
+
+  #endif
+}
 
 /**
  * 'Smart Fill': Scan from the outward edges of the mesh towards the center.