Fix UBL mesh inset Z position (#20538)

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

@@ -122,20 +122,29 @@ class unified_bed_leveling {
 
     FORCE_INLINE static void set_z(const int8_t px, const int8_t py, const float &z) { z_values[px][py] = z; }
 
+    static int8_t cell_index_x_raw(const float &x) {
+      return FLOOR((x - (MESH_MIN_X)) * RECIPROCAL(MESH_X_DIST));
+    }
+
+    static int8_t cell_index_y_raw(const float &y) {
+      return FLOOR((y - (MESH_MIN_Y)) * RECIPROCAL(MESH_Y_DIST));
+    }
+
+    static int8_t cell_index_x_valid(const float &x) {
+      return WITHIN(cell_index_x_raw(x), 0, (GRID_MAX_POINTS_X - 2));
+    }
+
+    static int8_t cell_index_y_valid(const float &y) {
+      return WITHIN(cell_index_y_raw(y), 0, (GRID_MAX_POINTS_Y - 2));
+    }
+
     static int8_t cell_index_x(const float &x) {
-      const int8_t cx = (x - (MESH_MIN_X)) * RECIPROCAL(MESH_X_DIST);
-      return constrain(cx, 0, (GRID_MAX_POINTS_X) - 1);   // -1 is appropriate if we want all movement to the X_MAX
-    }                                                     // position. But with this defined this way, it is possible
-                                                          // to extrapolate off of this point even further out. Probably
-                                                          // that is OK because something else should be keeping that from
-                                                          // happening and should not be worried about at this level.
+      return constrain(cell_index_x_raw(x), 0, (GRID_MAX_POINTS_X) - 2);
+    }
+
     static int8_t cell_index_y(const float &y) {
-      const int8_t cy = (y - (MESH_MIN_Y)) * RECIPROCAL(MESH_Y_DIST);
-      return constrain(cy, 0, (GRID_MAX_POINTS_Y) - 1);   // -1 is appropriate if we want all movement to the Y_MAX
-    }                                                     // position. But with this defined this way, it is possible
-                                                          // to extrapolate off of this point even further out. Probably
-                                                          // that is OK because something else should be keeping that from
-                                                          // happening and should not be worried about at this level.
+      return constrain(cell_index_y_raw(y), 0, (GRID_MAX_POINTS_Y) - 2);
+    }
 
     static inline xy_int8_t cell_indexes(const float &x, const float &y) {
       return { cell_index_x(x), cell_index_y(y) };

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

@@ -56,39 +56,32 @@
     // A move within the same cell needs no splitting
     if (istart == iend) {
 
-      // For a move off the bed, use a constant Z raise
-      if (!WITHIN(iend.x, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(iend.y, 0, GRID_MAX_POINTS_Y - 1)) {
+      FINAL_MOVE:
 
-        // Note: There is no Z Correction in this case. We are off the grid and don't know what
-        // a reasonable correction would be.  If the user has specified a UBL_Z_RAISE_WHEN_OFF_MESH
-        // value, that will be used instead of a calculated (Bi-Linear interpolation) correction.
+      // When UBL_Z_RAISE_WHEN_OFF_MESH is disabled Z correction is extrapolated from the edge of the mesh
+      #ifdef UBL_Z_RAISE_WHEN_OFF_MESH
+        // For a move off the UBL mesh, use a constant Z raise
+        if (!cell_index_x_valid(end.x) || !cell_index_y_valid(end.y)) {
 
-        #ifdef UBL_Z_RAISE_WHEN_OFF_MESH
-          end.z += UBL_Z_RAISE_WHEN_OFF_MESH;
-        #endif
-        planner.buffer_segment(end, scaled_fr_mm_s, extruder);
-        current_position = destination;
-        return;
-      }
+          // Note: There is no Z Correction in this case. We are off the mesh and don't know what
+          // a reasonable correction would be, UBL_Z_RAISE_WHEN_OFF_MESH will be used instead of
+          // a calculated (Bi-Linear interpolation) correction.
 
-      FINAL_MOVE:
+          end.z += UBL_Z_RAISE_WHEN_OFF_MESH;
+          planner.buffer_segment(end, scaled_fr_mm_s, extruder);
+          current_position = destination;
+          return;
+        }
+      #endif
 
       // The distance is always MESH_X_DIST so multiply by the constant reciprocal.
-      const float xratio = (end.x - mesh_index_to_xpos(iend.x)) * RECIPROCAL(MESH_X_DIST);
-
-      float z1, z2;
-      if (iend.x >= GRID_MAX_POINTS_X - 1)
-        z1 = z2 = 0.0;
-      else {
-        z1 = z_values[iend.x    ][iend.y    ] + xratio *
-            (z_values[iend.x + 1][iend.y    ] - z_values[iend.x][iend.y    ]),
-        z2 = z_values[iend.x    ][iend.y + 1] + xratio *
-            (z_values[iend.x + 1][iend.y + 1] - z_values[iend.x][iend.y + 1]);
-      }
+      const float xratio = (end.x - mesh_index_to_xpos(iend.x)) * RECIPROCAL(MESH_X_DIST),
+                  yratio = (end.y - mesh_index_to_ypos(iend.y)) * RECIPROCAL(MESH_Y_DIST),
+                  z1 = z_values[iend.x][iend.y    ] + xratio * (z_values[iend.x + 1][iend.y    ] - z_values[iend.x][iend.y    ]),
+                  z2 = z_values[iend.x][iend.y + 1] + xratio * (z_values[iend.x + 1][iend.y + 1] - z_values[iend.x][iend.y + 1]);
 
       // X cell-fraction done. Interpolate the two Z offsets with the Y fraction for the final Z offset.
-      const float yratio = (end.y - mesh_index_to_ypos(iend.y)) * RECIPROCAL(MESH_Y_DIST),
-                  z0 = iend.y < GRID_MAX_POINTS_Y - 1 ? (z1 + (z2 - z1) * yratio) * planner.fade_scaling_factor_for_z(end.z) : 0.0;
+      const float z0 = (z1 + (z2 - z1) * yratio) * planner.fade_scaling_factor_for_z(end.z);
 
       // Undefined parts of the Mesh in z_values[][] are NAN.
       // Replace NAN corrections with 0.0 to prevent NAN propagation.