One or the other?

Marlin/ubl.h

@@ -326,21 +326,24 @@
         return i < GRID_MAX_POINTS_Y ? pgm_read_float(&_mesh_index_to_ypos[i]) : MESH_MIN_Y + i * (MESH_Y_DIST);
       }
 
-      static bool prepare_segmented_line_to(const float (&rtarget)[XYZE], const float &feedrate);
-      static void line_to_destination_cartesian(const float &fr, uint8_t e);
-
-    #define _CMPZ(a,b) (z_values[a][b] == z_values[a][b+1])
-    #define CMPZ(a) (_CMPZ(a, 0) && _CMPZ(a, 1))
-    #define ZZER(a) (z_values[a][0] == 0)
-
-    FORCE_INLINE bool mesh_is_valid() {
-      return !(
-        (    CMPZ(0) && CMPZ(1) && CMPZ(2) // adjacent z values all equal?
-          && ZZER(0) && ZZER(1) && ZZER(2) // all zero at the edge?
-        )
-        || isnan(z_values[0][0])
-      );
-    }
+      #if UBL_SEGMENTED
+        static bool prepare_segmented_line_to(const float (&rtarget)[XYZE], const float &feedrate);
+      #else
+        static void line_to_destination_cartesian(const float &fr, const uint8_t e);
+      #endif
+
+      #define _CMPZ(a,b) (z_values[a][b] == z_values[a][b+1])
+      #define CMPZ(a) (_CMPZ(a, 0) && _CMPZ(a, 1))
+      #define ZZER(a) (z_values[a][0] == 0)
+
+      FORCE_INLINE bool mesh_is_valid() {
+        return !(
+          (    CMPZ(0) && CMPZ(1) && CMPZ(2) // adjacent z values all equal?
+            && ZZER(0) && ZZER(1) && ZZER(2) // all zero at the edge?
+          )
+          || isnan(z_values[0][0])
+        );
+      }
   }; // class unified_bed_leveling
 
   extern unified_bed_leveling ubl;

Marlin/ubl_motion.cpp

@@ -36,193 +36,322 @@
     extern void set_current_from_destination();
   #endif
 
-  void unified_bed_leveling::line_to_destination_cartesian(const float &feed_rate, uint8_t extruder) {
-    /**
-     * Much of the nozzle movement will be within the same cell. So we will do as little computation
-     * as possible to determine if this is the case. If this move is within the same cell, we will
-     * just do the required Z-Height correction, call the Planner's buffer_line() routine, and leave
-     */
-    const float start[XYZE] = {
-                  current_position[X_AXIS],
-                  current_position[Y_AXIS],
-                  current_position[Z_AXIS],
-                  current_position[E_AXIS]
-                },
-                end[XYZE] = {
-                  destination[X_AXIS],
-                  destination[Y_AXIS],
-                  destination[Z_AXIS],
-                  destination[E_AXIS]
-                };
-
-    const int cell_start_xi = get_cell_index_x(start[X_AXIS]),
-              cell_start_yi = get_cell_index_y(start[Y_AXIS]),
-              cell_dest_xi  = get_cell_index_x(end[X_AXIS]),
-              cell_dest_yi  = get_cell_index_y(end[Y_AXIS]);
-
-    if (g26_debug_flag) {
-      SERIAL_ECHOPAIR(" ubl.line_to_destination(xe=", end[X_AXIS]);
-      SERIAL_ECHOPAIR(", ye=", end[Y_AXIS]);
-      SERIAL_ECHOPAIR(", ze=", end[Z_AXIS]);
-      SERIAL_ECHOPAIR(", ee=", end[E_AXIS]);
-      SERIAL_CHAR(')');
-      SERIAL_EOL();
-      debug_current_and_destination(PSTR("Start of ubl.line_to_destination()"));
-    }
+  #if !UBL_SEGMENTED
 
-    if (cell_start_xi == cell_dest_xi && cell_start_yi == cell_dest_yi) { // if the whole move is within the same cell,
+    void unified_bed_leveling::line_to_destination_cartesian(const float &feed_rate, const uint8_t extruder) {
       /**
-       * we don't need to break up the move
-       *
-       * If we are moving off the print bed, we are going to allow the move at this level.
-       * But we detect it and isolate it. For now, we just pass along the request.
+       * Much of the nozzle movement will be within the same cell. So we will do as little computation
+       * as possible to determine if this is the case. If this move is within the same cell, we will
+       * just do the required Z-Height correction, call the Planner's buffer_line() routine, and leave
        */
+      const float start[XYZE] = {
+                    current_position[X_AXIS],
+                    current_position[Y_AXIS],
+                    current_position[Z_AXIS],
+                    current_position[E_AXIS]
+                  },
+                  end[XYZE] = {
+                    destination[X_AXIS],
+                    destination[Y_AXIS],
+                    destination[Z_AXIS],
+                    destination[E_AXIS]
+                  };
+
+      const int cell_start_xi = get_cell_index_x(start[X_AXIS]),
+                cell_start_yi = get_cell_index_y(start[Y_AXIS]),
+                cell_dest_xi  = get_cell_index_x(end[X_AXIS]),
+                cell_dest_yi  = get_cell_index_y(end[Y_AXIS]);
+
+      if (g26_debug_flag) {
+        SERIAL_ECHOPAIR(" ubl.line_to_destination(xe=", end[X_AXIS]);
+        SERIAL_ECHOPAIR(", ye=", end[Y_AXIS]);
+        SERIAL_ECHOPAIR(", ze=", end[Z_AXIS]);
+        SERIAL_ECHOPAIR(", ee=", end[E_AXIS]);
+        SERIAL_CHAR(')');
+        SERIAL_EOL();
+        debug_current_and_destination(PSTR("Start of ubl.line_to_destination()"));
+      }
 
-      if (!WITHIN(cell_dest_xi, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(cell_dest_yi, 0, GRID_MAX_POINTS_Y - 1)) {
+      if (cell_start_xi == cell_dest_xi && cell_start_yi == cell_dest_yi) { // if the whole move is within the same cell,
+        /**
+         * we don't need to break up the move
+         *
+         * If we are moving off the print bed, we are going to allow the move at this level.
+         * But we detect it and isolate it. For now, we just pass along the request.
+         */
 
-        // 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 (!WITHIN(cell_dest_xi, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(cell_dest_yi, 0, GRID_MAX_POINTS_Y - 1)) {
 
-        planner.buffer_segment(end[X_AXIS], end[Y_AXIS], end[Z_AXIS], end[E_AXIS], feed_rate, extruder);
-        set_current_from_destination();
+          // Note: There is no Z Correction in this case. We are off the grid and don't know what
+          // a reasonable correction would be.
+
+          planner.buffer_segment(end[X_AXIS], end[Y_AXIS], end[Z_AXIS], end[E_AXIS], feed_rate, extruder);
+          set_current_from_destination();
+
+          if (g26_debug_flag)
+            debug_current_and_destination(PSTR("out of bounds in ubl.line_to_destination()"));
+
+          return;
+        }
+
+        FINAL_MOVE:
+
+        /**
+         * Optimize some floating point operations here. We could call float get_z_correction(float x0, float y0) to
+         * generate the correction for us. But we can lighten the load on the CPU by doing a modified version of the function.
+         * We are going to only calculate the amount we are from the first mesh line towards the second mesh line once.
+         * We will use this fraction in both of the original two Z Height calculations for the bi-linear interpolation. And,
+         * instead of doing a generic divide of the distance, we know the distance is MESH_X_DIST so we can use the preprocessor
+         * to create a 1-over number for us. That will allow us to do a floating point multiply instead of a floating point divide.
+         */
+
+        const float xratio = (end[X_AXIS] - mesh_index_to_xpos(cell_dest_xi)) * (1.0 / (MESH_X_DIST));
+
+        float z1 = z_values[cell_dest_xi    ][cell_dest_yi    ] + xratio *
+                  (z_values[cell_dest_xi + 1][cell_dest_yi    ] - z_values[cell_dest_xi][cell_dest_yi    ]),
+              z2 = z_values[cell_dest_xi    ][cell_dest_yi + 1] + xratio *
+                  (z_values[cell_dest_xi + 1][cell_dest_yi + 1] - z_values[cell_dest_xi][cell_dest_yi + 1]);
+
+        if (cell_dest_xi >= GRID_MAX_POINTS_X - 1) z1 = z2 = 0.0;
+
+        // we are done with the fractional X distance into the cell. Now with the two Z-Heights we have calculated, we
+        // are going to apply the Y-Distance into the cell to interpolate the final Z correction.
+
+        const float yratio = (end[Y_AXIS] - mesh_index_to_ypos(cell_dest_yi)) * (1.0 / (MESH_Y_DIST));
+        float z0 = cell_dest_yi < GRID_MAX_POINTS_Y - 1 ? (z1 + (z2 - z1) * yratio) * planner.fade_scaling_factor_for_z(end[Z_AXIS]) : 0.0;
+
+        /**
+         * If part of the Mesh is undefined, it will show up as NAN
+         * in z_values[][] and propagate through the
+         * calculations. If our correction is NAN, we throw it out
+         * because part of the Mesh is undefined and we don't have the
+         * information we need to complete the height correction.
+         */
+        if (isnan(z0)) z0 = 0.0;
+
+        planner.buffer_segment(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + z0, end[E_AXIS], feed_rate, extruder);
 
         if (g26_debug_flag)
-          debug_current_and_destination(PSTR("out of bounds in ubl.line_to_destination()"));
+          debug_current_and_destination(PSTR("FINAL_MOVE in ubl.line_to_destination()"));
 
+        set_current_from_destination();
         return;
       }
 
-      FINAL_MOVE:
+      /**
+       * If we get here, we are processing a move that crosses at least one Mesh Line. We will check
+       * for the simple case of just crossing X or just crossing Y Mesh Lines after we get all the details
+       * of the move figured out. We can process the easy case of just crossing an X or Y Mesh Line with less
+       * computation and in fact most lines are of this nature. We will check for that in the following
+       * blocks of code:
+       */
+
+      const float dx = end[X_AXIS] - start[X_AXIS],
+                  dy = end[Y_AXIS] - start[Y_AXIS];
+
+      const int left_flag = dx < 0.0 ? 1 : 0,
+                down_flag = dy < 0.0 ? 1 : 0;
+
+      const float adx = left_flag ? -dx : dx,
+                  ady = down_flag ? -dy : dy;
+
+      const int dxi = cell_start_xi == cell_dest_xi ? 0 : left_flag ? -1 : 1,
+                dyi = cell_start_yi == cell_dest_yi ? 0 : down_flag ? -1 : 1;
 
       /**
-       * Optimize some floating point operations here. We could call float get_z_correction(float x0, float y0) to
-       * generate the correction for us. But we can lighten the load on the CPU by doing a modified version of the function.
-       * We are going to only calculate the amount we are from the first mesh line towards the second mesh line once.
-       * We will use this fraction in both of the original two Z Height calculations for the bi-linear interpolation. And,
-       * instead of doing a generic divide of the distance, we know the distance is MESH_X_DIST so we can use the preprocessor
-       * to create a 1-over number for us. That will allow us to do a floating point multiply instead of a floating point divide.
+       * Compute the scaling factor for the extruder for each partial move.
+       * We need to watch out for zero length moves because it will cause us to
+       * have an infinate scaling factor. We are stuck doing a floating point
+       * divide to get our scaling factor, but after that, we just multiply by this
+       * number. We also pick our scaling factor based on whether the X or Y
+       * component is larger. We use the biggest of the two to preserve precision.
        */
 
-      const float xratio = (end[X_AXIS] - mesh_index_to_xpos(cell_dest_xi)) * (1.0 / (MESH_X_DIST));
+      const bool use_x_dist = adx > ady;
 
-      float z1 = z_values[cell_dest_xi    ][cell_dest_yi    ] + xratio *
-                (z_values[cell_dest_xi + 1][cell_dest_yi    ] - z_values[cell_dest_xi][cell_dest_yi    ]),
-            z2 = z_values[cell_dest_xi    ][cell_dest_yi + 1] + xratio *
-                (z_values[cell_dest_xi + 1][cell_dest_yi + 1] - z_values[cell_dest_xi][cell_dest_yi + 1]);
+      float on_axis_distance = use_x_dist ? dx : dy,
+            e_position = end[E_AXIS] - start[E_AXIS],
+            z_position = end[Z_AXIS] - start[Z_AXIS];
 
-      if (cell_dest_xi >= GRID_MAX_POINTS_X - 1) z1 = z2 = 0.0;
+      const float e_normalized_dist = e_position / on_axis_distance,
+                  z_normalized_dist = z_position / on_axis_distance;
 
-      // we are done with the fractional X distance into the cell. Now with the two Z-Heights we have calculated, we
-      // are going to apply the Y-Distance into the cell to interpolate the final Z correction.
+      int current_xi = cell_start_xi,
+          current_yi = cell_start_yi;
 
-      const float yratio = (end[Y_AXIS] - mesh_index_to_ypos(cell_dest_yi)) * (1.0 / (MESH_Y_DIST));
-      float z0 = cell_dest_yi < GRID_MAX_POINTS_Y - 1 ? (z1 + (z2 - z1) * yratio) * planner.fade_scaling_factor_for_z(end[Z_AXIS]) : 0.0;
+      const float m = dy / dx,
+                  c = start[Y_AXIS] - m * start[X_AXIS];
 
+      const bool inf_normalized_flag = (isinf(e_normalized_dist) != 0),
+                 inf_m_flag = (isinf(m) != 0);
       /**
-       * If part of the Mesh is undefined, it will show up as NAN
-       * in z_values[][] and propagate through the
-       * calculations. If our correction is NAN, we throw it out
-       * because part of the Mesh is undefined and we don't have the
-       * information we need to complete the height correction.
+       * This block handles vertical lines. These are lines that stay within the same
+       * X Cell column. They do not need to be perfectly vertical. They just can
+       * not cross into another X Cell column.
        */
-      if (isnan(z0)) z0 = 0.0;
-
-      planner.buffer_segment(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + z0, end[E_AXIS], feed_rate, extruder);
+      if (dxi == 0) {       // Check for a vertical line
+        current_yi += down_flag;  // Line is heading down, we just want to go to the bottom
+        while (current_yi != cell_dest_yi + down_flag) {
+          current_yi += dyi;
+          const float next_mesh_line_y = mesh_index_to_ypos(current_yi);
+
+          /**
+           * if the slope of the line is infinite, we won't do the calculations
+           * else, we know the next X is the same so we can recover and continue!
+           * Calculate X at the next Y mesh line
+           */
+          const float rx = inf_m_flag ? start[X_AXIS] : (next_mesh_line_y - c) / m;
+
+          float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, current_xi, current_yi)
+                     * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
+
+          /**
+           * If part of the Mesh is undefined, it will show up as NAN
+           * in z_values[][] and propagate through the
+           * calculations. If our correction is NAN, we throw it out
+           * because part of the Mesh is undefined and we don't have the
+           * information we need to complete the height correction.
+           */
+          if (isnan(z0)) z0 = 0.0;
+
+          const float ry = mesh_index_to_ypos(current_yi);
+
+          /**
+           * Without this check, it is possible for the algorithm to generate a zero length move in the case
+           * where the line is heading down and it is starting right on a Mesh Line boundary. For how often that
+           * happens, it might be best to remove the check and always 'schedule' the move because
+           * the planner.buffer_segment() routine will filter it if that happens.
+           */
+          if (ry != start[Y_AXIS]) {
+            if (!inf_normalized_flag) {
+              on_axis_distance = use_x_dist ? rx - start[X_AXIS] : ry - start[Y_AXIS];
+              e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
+              z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
+            }
+            else {
+              e_position = end[E_AXIS];
+              z_position = end[Z_AXIS];
+            }
 
-      if (g26_debug_flag)
-        debug_current_and_destination(PSTR("FINAL_MOVE in ubl.line_to_destination()"));
+            planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder);
+          } //else printf("FIRST MOVE PRUNED  ");
+        }
 
-      set_current_from_destination();
-      return;
-    }
+        if (g26_debug_flag)
+          debug_current_and_destination(PSTR("vertical move done in ubl.line_to_destination()"));
 
-    /**
-     * If we get here, we are processing a move that crosses at least one Mesh Line. We will check
-     * for the simple case of just crossing X or just crossing Y Mesh Lines after we get all the details
-     * of the move figured out. We can process the easy case of just crossing an X or Y Mesh Line with less
-     * computation and in fact most lines are of this nature. We will check for that in the following
-     * blocks of code:
-     */
+        //
+        // Check if we are at the final destination. Usually, we won't be, but if it is on a Y Mesh Line, we are done.
+        //
+        if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
+          goto FINAL_MOVE;
 
-    const float dx = end[X_AXIS] - start[X_AXIS],
-                dy = end[Y_AXIS] - start[Y_AXIS];
+        set_current_from_destination();
+        return;
+      }
 
-    const int left_flag = dx < 0.0 ? 1 : 0,
-              down_flag = dy < 0.0 ? 1 : 0;
+      /**
+       *
+       * This block handles horizontal lines. These are lines that stay within the same
+       * Y Cell row. They do not need to be perfectly horizontal. They just can
+       * not cross into another Y Cell row.
+       *
+       */
 
-    const float adx = left_flag ? -dx : dx,
-                ady = down_flag ? -dy : dy;
+      if (dyi == 0) {             // Check for a horizontal line
+        current_xi += left_flag;  // Line is heading left, we just want to go to the left
+                                  // edge of this cell for the first move.
+        while (current_xi != cell_dest_xi + left_flag) {
+          current_xi += dxi;
+          const float next_mesh_line_x = mesh_index_to_xpos(current_xi),
+                      ry = m * next_mesh_line_x + c;   // Calculate Y at the next X mesh line
+
+          float z0 = z_correction_for_y_on_vertical_mesh_line(ry, current_xi, current_yi)
+                     * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
+
+          /**
+           * If part of the Mesh is undefined, it will show up as NAN
+           * in z_values[][] and propagate through the
+           * calculations. If our correction is NAN, we throw it out
+           * because part of the Mesh is undefined and we don't have the
+           * information we need to complete the height correction.
+           */
+          if (isnan(z0)) z0 = 0.0;
+
+          const float rx = mesh_index_to_xpos(current_xi);
+
+          /**
+           * Without this check, it is possible for the algorithm to generate a zero length move in the case
+           * where the line is heading left and it is starting right on a Mesh Line boundary. For how often
+           * that happens, it might be best to remove the check and always 'schedule' the move because
+           * the planner.buffer_segment() routine will filter it if that happens.
+           */
+          if (rx != start[X_AXIS]) {
+            if (!inf_normalized_flag) {
+              on_axis_distance = use_x_dist ? rx - start[X_AXIS] : ry - start[Y_AXIS];
+              e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;  // is based on X or Y because this is a horizontal move
+              z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
+            }
+            else {
+              e_position = end[E_AXIS];
+              z_position = end[Z_AXIS];
+            }
 
-    const int dxi = cell_start_xi == cell_dest_xi ? 0 : left_flag ? -1 : 1,
-              dyi = cell_start_yi == cell_dest_yi ? 0 : down_flag ? -1 : 1;
+            planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder);
+          } //else printf("FIRST MOVE PRUNED  ");
+        }
 
-    /**
-     * Compute the scaling factor for the extruder for each partial move.
-     * We need to watch out for zero length moves because it will cause us to
-     * have an infinate scaling factor. We are stuck doing a floating point
-     * divide to get our scaling factor, but after that, we just multiply by this
-     * number. We also pick our scaling factor based on whether the X or Y
-     * component is larger. We use the biggest of the two to preserve precision.
-     */
+        if (g26_debug_flag)
+          debug_current_and_destination(PSTR("horizontal move done in ubl.line_to_destination()"));
 
-    const bool use_x_dist = adx > ady;
+        if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
+          goto FINAL_MOVE;
 
-    float on_axis_distance = use_x_dist ? dx : dy,
-          e_position = end[E_AXIS] - start[E_AXIS],
-          z_position = end[Z_AXIS] - start[Z_AXIS];
+        set_current_from_destination();
+        return;
+      }
 
-    const float e_normalized_dist = e_position / on_axis_distance,
-                z_normalized_dist = z_position / on_axis_distance;
+      /**
+       *
+       * This block handles the generic case of a line crossing both X and Y Mesh lines.
+       *
+       */
 
-    int current_xi = cell_start_xi,
-        current_yi = cell_start_yi;
+      int xi_cnt = cell_start_xi - cell_dest_xi,
+          yi_cnt = cell_start_yi - cell_dest_yi;
 
-    const float m = dy / dx,
-                c = start[Y_AXIS] - m * start[X_AXIS];
+      if (xi_cnt < 0) xi_cnt = -xi_cnt;
+      if (yi_cnt < 0) yi_cnt = -yi_cnt;
 
-    const bool inf_normalized_flag = (isinf(e_normalized_dist) != 0),
-               inf_m_flag = (isinf(m) != 0);
-    /**
-     * This block handles vertical lines. These are lines that stay within the same
-     * X Cell column. They do not need to be perfectly vertical. They just can
-     * not cross into another X Cell column.
-     */
-    if (dxi == 0) {       // Check for a vertical line
-      current_yi += down_flag;  // Line is heading down, we just want to go to the bottom
-      while (current_yi != cell_dest_yi + down_flag) {
-        current_yi += dyi;
-        const float next_mesh_line_y = mesh_index_to_ypos(current_yi);
+      current_xi += left_flag;
+      current_yi += down_flag;
 
-        /**
-         * if the slope of the line is infinite, we won't do the calculations
-         * else, we know the next X is the same so we can recover and continue!
-         * Calculate X at the next Y mesh line
-         */
-        const float rx = inf_m_flag ? start[X_AXIS] : (next_mesh_line_y - c) / m;
+      while (xi_cnt > 0 || yi_cnt > 0) {
 
-        float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, current_xi, current_yi)
-                   * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
+        const float next_mesh_line_x = mesh_index_to_xpos(current_xi + dxi),
+                    next_mesh_line_y = mesh_index_to_ypos(current_yi + dyi),
+                    ry = m * next_mesh_line_x + c,   // Calculate Y at the next X mesh line
+                    rx = (next_mesh_line_y - c) / m; // Calculate X at the next Y mesh line
+                                                     // (No need to worry about m being zero.
+                                                     //  If that was the case, it was already detected
+                                                     //  as a vertical line move above.)
 
-        /**
-         * If part of the Mesh is undefined, it will show up as NAN
-         * in z_values[][] and propagate through the
-         * calculations. If our correction is NAN, we throw it out
-         * because part of the Mesh is undefined and we don't have the
-         * information we need to complete the height correction.
-         */
-        if (isnan(z0)) z0 = 0.0;
+        if (left_flag == (rx > next_mesh_line_x)) { // Check if we hit the Y line first
+          // Yes!  Crossing a Y Mesh Line next
+          float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, current_xi - left_flag, current_yi + dyi)
+                     * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
 
-        const float ry = mesh_index_to_ypos(current_yi);
+          /**
+           * If part of the Mesh is undefined, it will show up as NAN
+           * in z_values[][] and propagate through the
+           * calculations. If our correction is NAN, we throw it out
+           * because part of the Mesh is undefined and we don't have the
+           * information we need to complete the height correction.
+           */
+          if (isnan(z0)) z0 = 0.0;
 
-        /**
-         * Without this check, it is possible for the algorithm to generate a zero length move in the case
-         * where the line is heading down and it is starting right on a Mesh Line boundary. For how often that
-         * happens, it might be best to remove the check and always 'schedule' the move because
-         * the planner.buffer_segment() routine will filter it if that happens.
-         */
-        if (ry != start[Y_AXIS]) {
           if (!inf_normalized_flag) {
-            on_axis_distance = use_x_dist ? rx - start[X_AXIS] : ry - start[Y_AXIS];
+            on_axis_distance = use_x_dist ? rx - start[X_AXIS] : next_mesh_line_y - start[Y_AXIS];
             e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
             z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
           }
@@ -230,64 +359,27 @@
             e_position = end[E_AXIS];
             z_position = end[Z_AXIS];
           }
+          planner.buffer_segment(rx, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder);
+          current_yi += dyi;
+          yi_cnt--;
+        }
+        else {
+          // Yes!  Crossing a X Mesh Line next
+          float z0 = z_correction_for_y_on_vertical_mesh_line(ry, current_xi + dxi, current_yi - down_flag)
+                     * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
+
+          /**
+           * If part of the Mesh is undefined, it will show up as NAN
+           * in z_values[][] and propagate through the
+           * calculations. If our correction is NAN, we throw it out
+           * because part of the Mesh is undefined and we don't have the
+           * information we need to complete the height correction.
+           */
+          if (isnan(z0)) z0 = 0.0;
 
-          planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder);
-        } //else printf("FIRST MOVE PRUNED  ");
-      }
-
-      if (g26_debug_flag)
-        debug_current_and_destination(PSTR("vertical move done in ubl.line_to_destination()"));
-
-      //
-      // Check if we are at the final destination. Usually, we won't be, but if it is on a Y Mesh Line, we are done.
-      //
-      if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
-        goto FINAL_MOVE;
-
-      set_current_from_destination();
-      return;
-    }
-
-    /**
-     *
-     * This block handles horizontal lines. These are lines that stay within the same
-     * Y Cell row. They do not need to be perfectly horizontal. They just can
-     * not cross into another Y Cell row.
-     *
-     */
-
-    if (dyi == 0) {             // Check for a horizontal line
-      current_xi += left_flag;  // Line is heading left, we just want to go to the left
-                                // edge of this cell for the first move.
-      while (current_xi != cell_dest_xi + left_flag) {
-        current_xi += dxi;
-        const float next_mesh_line_x = mesh_index_to_xpos(current_xi),
-                    ry = m * next_mesh_line_x + c;   // Calculate Y at the next X mesh line
-
-        float z0 = z_correction_for_y_on_vertical_mesh_line(ry, current_xi, current_yi)
-                   * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
-
-        /**
-         * If part of the Mesh is undefined, it will show up as NAN
-         * in z_values[][] and propagate through the
-         * calculations. If our correction is NAN, we throw it out
-         * because part of the Mesh is undefined and we don't have the
-         * information we need to complete the height correction.
-         */
-        if (isnan(z0)) z0 = 0.0;
-
-        const float rx = mesh_index_to_xpos(current_xi);
-
-        /**
-         * Without this check, it is possible for the algorithm to generate a zero length move in the case
-         * where the line is heading left and it is starting right on a Mesh Line boundary. For how often
-         * that happens, it might be best to remove the check and always 'schedule' the move because
-         * the planner.buffer_segment() routine will filter it if that happens.
-         */
-        if (rx != start[X_AXIS]) {
           if (!inf_normalized_flag) {
-            on_axis_distance = use_x_dist ? rx - start[X_AXIS] : ry - start[Y_AXIS];
-            e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;  // is based on X or Y because this is a horizontal move
+            on_axis_distance = use_x_dist ? next_mesh_line_x - start[X_AXIS] : ry - start[Y_AXIS];
+            e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
             z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
           }
           else {
@@ -295,122 +387,24 @@
             z_position = end[Z_AXIS];
           }
 
-          planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder);
-        } //else printf("FIRST MOVE PRUNED  ");
+          planner.buffer_segment(next_mesh_line_x, ry, z_position + z0, e_position, feed_rate, extruder);
+          current_xi += dxi;
+          xi_cnt--;
+        }
+
+        if (xi_cnt < 0 || yi_cnt < 0) break; // we've gone too far, so exit the loop and move on to FINAL_MOVE
       }
 
       if (g26_debug_flag)
-        debug_current_and_destination(PSTR("horizontal move done in ubl.line_to_destination()"));
+        debug_current_and_destination(PSTR("generic move done in ubl.line_to_destination()"));
 
       if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
         goto FINAL_MOVE;
 
       set_current_from_destination();
-      return;
     }
 
-    /**
-     *
-     * This block handles the generic case of a line crossing both X and Y Mesh lines.
-     *
-     */
-
-    int xi_cnt = cell_start_xi - cell_dest_xi,
-        yi_cnt = cell_start_yi - cell_dest_yi;
-
-    if (xi_cnt < 0) xi_cnt = -xi_cnt;
-    if (yi_cnt < 0) yi_cnt = -yi_cnt;
-
-    current_xi += left_flag;
-    current_yi += down_flag;
-
-    while (xi_cnt > 0 || yi_cnt > 0) {
-
-      const float next_mesh_line_x = mesh_index_to_xpos(current_xi + dxi),
-                  next_mesh_line_y = mesh_index_to_ypos(current_yi + dyi),
-                  ry = m * next_mesh_line_x + c,   // Calculate Y at the next X mesh line
-                  rx = (next_mesh_line_y - c) / m; // Calculate X at the next Y mesh line
-                                                   // (No need to worry about m being zero.
-                                                   //  If that was the case, it was already detected
-                                                   //  as a vertical line move above.)
-
-      if (left_flag == (rx > next_mesh_line_x)) { // Check if we hit the Y line first
-        // Yes!  Crossing a Y Mesh Line next
-        float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, current_xi - left_flag, current_yi + dyi)
-                   * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
-
-        /**
-         * If part of the Mesh is undefined, it will show up as NAN
-         * in z_values[][] and propagate through the
-         * calculations. If our correction is NAN, we throw it out
-         * because part of the Mesh is undefined and we don't have the
-         * information we need to complete the height correction.
-         */
-        if (isnan(z0)) z0 = 0.0;
-
-        if (!inf_normalized_flag) {
-          on_axis_distance = use_x_dist ? rx - start[X_AXIS] : next_mesh_line_y - start[Y_AXIS];
-          e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
-          z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
-        }
-        else {
-          e_position = end[E_AXIS];
-          z_position = end[Z_AXIS];
-        }
-        planner.buffer_segment(rx, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder);
-        current_yi += dyi;
-        yi_cnt--;
-      }
-      else {
-        // Yes!  Crossing a X Mesh Line next
-        float z0 = z_correction_for_y_on_vertical_mesh_line(ry, current_xi + dxi, current_yi - down_flag)
-                   * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
-
-        /**
-         * If part of the Mesh is undefined, it will show up as NAN
-         * in z_values[][] and propagate through the
-         * calculations. If our correction is NAN, we throw it out
-         * because part of the Mesh is undefined and we don't have the
-         * information we need to complete the height correction.
-         */
-        if (isnan(z0)) z0 = 0.0;
-
-        if (!inf_normalized_flag) {
-          on_axis_distance = use_x_dist ? next_mesh_line_x - start[X_AXIS] : ry - start[Y_AXIS];
-          e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
-          z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
-        }
-        else {
-          e_position = end[E_AXIS];
-          z_position = end[Z_AXIS];
-        }
-
-        planner.buffer_segment(next_mesh_line_x, ry, z_position + z0, e_position, feed_rate, extruder);
-        current_xi += dxi;
-        xi_cnt--;
-      }
-
-      if (xi_cnt < 0 || yi_cnt < 0) break; // we've gone too far, so exit the loop and move on to FINAL_MOVE
-    }
-
-    if (g26_debug_flag)
-      debug_current_and_destination(PSTR("generic move done in ubl.line_to_destination()"));
-
-    if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
-      goto FINAL_MOVE;
-
-    set_current_from_destination();
-  }
-
-  #if UBL_SEGMENTED
-
-    // macro to inline copy exactly 4 floats, don't rely on sizeof operator
-    #define COPY_XYZE( target, source ) { \
-                target[X_AXIS] = source[X_AXIS]; \
-                target[Y_AXIS] = source[Y_AXIS]; \
-                target[Z_AXIS] = source[Z_AXIS]; \
-                target[E_AXIS] = source[E_AXIS]; \
-            }
+  #else // UBL_SEGMENTED
 
     #if IS_SCARA // scale the feed rate from mm/s to degrees/s
       static float scara_feed_factor, scara_oldA, scara_oldB;