UBL - Hilbert space-filling curve probing sequence (#21387)

MarlinFirmware · Mar 23, 2021 · eee726e · eee726e
1 parent 22fc59a
commit eee726e
Show file tree

Hide file tree

Showing 4 changed files with 63 additions and 4 deletions.
diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h
@@ -1478,6 +1478,8 @@
   #define GRID_MAX_POINTS_X 10      // Don't use more than 15 points per axis, implementation limited.
   #define GRID_MAX_POINTS_Y GRID_MAX_POINTS_X
 
+  //#define UBL_HILBERT_CURVE       // Use Hilbert distribution for less travel when probing multiple points
+
   #define UBL_MESH_EDIT_MOVES_Z     // Sophisticated users prefer no movement of nozzle
   #define UBL_SAVE_ACTIVE_ON_M500   // Save the currently active mesh in the current slot on M500
 

diff --git a/Marlin/src/feature/bedlevel/ubl/ubl.h b/Marlin/src/feature/bedlevel/ubl/ubl.h
@@ -98,6 +98,11 @@ 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);

diff --git a/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp b/Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp
@@ -757,9 +757,11 @@ void unified_bed_leveling::shift_mesh_height() {
         }
       #endif
 
-      best = do_furthest
-        ? find_furthest_invalid_mesh_point()
-        : find_closest_mesh_point_of_type(INVALID, nearby, true);
+      best = do_furthest  ? find_furthest_invalid_mesh_point()
+                          : TERN(UBL_HILBERT_CURVE,
+                              next_point_in_grid(),
+                              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));
@@ -1298,6 +1300,56 @@ mesh_index_pair unified_bed_leveling::find_closest_mesh_point_of_type(const Mesh
   return closest;
 }
 
+#if ENABLED(UBL_HILBERT_CURVE)
+
+  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);
+    }
+  }
+
+  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))
+      ) {
+        pt.pos.set(x, y);
+        pt.distance = 1;
+      }
+   }
+
+   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
+
 /**
  * 'Smart Fill': Scan from the outward edges of the mesh towards the center.
  * If an invalid location is found, use the next two points (if valid) to

diff --git a/buildroot/tests/mega2560 b/buildroot/tests/mega2560
@@ -36,7 +36,7 @@ opt_set MOTHERBOARD BOARD_AZTEEG_X3_PRO LCD_LANGUAGE jp_kana \
 opt_enable REPRAP_DISCOUNT_FULL_GRAPHIC_SMART_CONTROLLER LIGHTWEIGHT_UI SHOW_CUSTOM_BOOTSCREEN BOOT_MARLIN_LOGO_SMALL \
            LCD_SET_PROGRESS_MANUALLY PRINT_PROGRESS_SHOW_DECIMALS SHOW_REMAINING_TIME STATUS_MESSAGE_SCROLLING SCROLL_LONG_FILENAMES \
            SDSUPPORT SDCARD_SORT_ALPHA NO_SD_AUTOSTART USB_FLASH_DRIVE_SUPPORT CANCEL_OBJECTS \
-           Z_PROBE_SLED AUTO_BED_LEVELING_UBL RESTORE_LEVELING_AFTER_G28 DEBUG_LEVELING_FEATURE G26_MESH_VALIDATION ENABLE_LEVELING_FADE_HEIGHT \
+           Z_PROBE_SLED AUTO_BED_LEVELING_UBL UBL_HILBERT_CURVE RESTORE_LEVELING_AFTER_G28 DEBUG_LEVELING_FEATURE G26_MESH_VALIDATION ENABLE_LEVELING_FADE_HEIGHT \
            EEPROM_SETTINGS EEPROM_CHITCHAT GCODE_MACROS CUSTOM_USER_MENUS \
            MULTI_NOZZLE_DUPLICATION CLASSIC_JERK LIN_ADVANCE QUICK_HOME \
            NANODLP_Z_SYNC I2C_POSITION_ENCODERS M114_DETAIL \