ENH: fix the floating layer issue

Fix for floating layer under multi-color
printing while z_hop is zero.

Signed-off-by: salt.wei <salt.wei@bambulab.com>
Change-Id: I8cc96bd18020cac8424fe4c3e62fb87da118b826

src/libslic3r/GCode.cpp

@@ -520,9 +520,20 @@ static std::vector<Vec2d> get_path_of_change_filament(const Print& print)
 
             // retract before toolchange
             toolchange_gcode_str = toolchange_retract_str + toolchange_gcode_str;
-            //BBS: current position and fan_speed is unclear after interting change_filament_gcode
-            toolchange_gcode_str += ";_FORCE_RESUME_FAN_SPEED\n";
-            gcodegen.writer().set_current_position_clear(false);
+            //BBS
+            {
+                //BBS: current position and fan_speed is unclear after interting change_filament_gcode
+                check_add_eol(toolchange_gcode_str);
+                toolchange_gcode_str += ";_FORCE_RESUME_FAN_SPEED\n";
+                gcodegen.writer().set_current_position_clear(false);
+                //BBS: check whether custom gcode changes the z position. Update if changed
+                double temp_z_after_tool_change;
+                if (GCodeProcessor::get_last_z_from_gcode(toolchange_gcode_str, temp_z_after_tool_change)) {
+                    Vec3d pos = gcodegen.writer().get_position();
+                    pos(2) = temp_z_after_tool_change;
+                    gcodegen.writer().set_position(pos);
+                }
+            }
 
             // move to start_pos for wiping after toolchange
             std::string start_pos_str;
@@ -4239,12 +4250,23 @@ std::string GCode::set_extruder(unsigned int extruder_id, double print_z)
     std::string toolchange_gcode_parsed;
     if (!change_filament_gcode.empty()) {
         toolchange_gcode_parsed = placeholder_parser_process("change_filament_gcode", change_filament_gcode, extruder_id, &dyn_config);
+        check_add_eol(toolchange_gcode_parsed);
         gcode += toolchange_gcode_parsed;
-        check_add_eol(gcode);
-        //BBS: gcode writer doesn't know where the extruder is and whether fan speed is changed after inserting tool change gcode
-        //Set this flag so that normal lift will be used the first time after tool change.
-        gcode += ";_FORCE_RESUME_FAN_SPEED\n";
-        m_writer.set_current_position_clear(false);
+
+        //BBS
+        {
+            //BBS: gcode writer doesn't know where the extruder is and whether fan speed is changed after inserting tool change gcode
+            //Set this flag so that normal lift will be used the first time after tool change.
+            gcode += ";_FORCE_RESUME_FAN_SPEED\n";
+            m_writer.set_current_position_clear(false);
+            //BBS: check whether custom gcode changes the z position. Update if changed
+            double temp_z_after_tool_change;
+            if (GCodeProcessor::get_last_z_from_gcode(toolchange_gcode_parsed, temp_z_after_tool_change)) {
+                Vec3d pos = m_writer.get_position();
+                pos(2) = temp_z_after_tool_change;
+                m_writer.set_position(pos);
+            }
+        }
     }
 
     // BBS. Reset old extruder E-value.

src/libslic3r/GCode/GCodeProcessor.cpp

@@ -12,6 +12,8 @@
 #include <boost/nowide/cstdio.hpp>
 #include <boost/filesystem/path.hpp>
 
+#include <fast_float/fast_float.h>
+
 #include <float.h>
 #include <assert.h>
 
@@ -1921,6 +1923,61 @@ int GCodeProcessor::get_gcode_last_filament(const std::string& gcode_str)
     return out_filament;
 }
 
+//BBS: get last z position from gcode
+bool GCodeProcessor::get_last_z_from_gcode(const std::string& gcode_str, double& z)
+{
+    int str_size = gcode_str.size();
+    int start_index = 0;
+    int end_index = 0;
+    bool is_z_changed = false;
+    while (end_index < str_size) {
+        //find a full line
+        if (gcode_str[end_index] != '\n') {
+            end_index++;
+            continue;
+        }
+        //parse the line
+        if (end_index > start_index) {
+            std::string line_str = gcode_str.substr(start_index, end_index - start_index);
+            line_str.erase(0, line_str.find_first_not_of(" "));
+            line_str.erase(line_str.find_last_not_of(";") + 1);
+            line_str.erase(line_str.find_last_not_of(" ") + 1);
+
+            //command which may have z movement
+            if (line_str.size() > 5 && (line_str.find("G0 ") == 0
+                                       || line_str.find("G1 ") == 0
+                                       || line_str.find("G2 ") == 0
+                                       || line_str.find("G3 ") == 0))
+            {
+                auto z_pos = line_str.find(" Z");
+                double temp_z = 0;
+                if (z_pos != line_str.npos
+                    && z_pos + 2 < line_str.size()) {
+                    // Try to parse the numeric value.
+                    std::string z_sub = line_str.substr(z_pos + 2);
+                    char* c = &z_sub[0];
+                    char* end = c + sizeof(z_sub.c_str());
+
+                    auto is_end_of_word = [](char c) {
+                        return c == ' ' || c == '\t' || c == '\r' || c == '\n' || c == 0 || c == ';';
+                    };
+
+                    auto [pend, ec] = fast_float::from_chars(c, end, temp_z);
+                    if (pend != c && is_end_of_word(*pend)) {
+                        // The axis value has been parsed correctly.
+                        z = temp_z;
+                        is_z_changed = true;
+                    }
+                }
+            }
+        }
+        //loop to handle next line
+        start_index = end_index + 1;
+        end_index = start_index;
+    }
+    return is_z_changed;
+}
+
 void GCodeProcessor::process_tags(const std::string_view comment, bool producers_enabled)
 {
     // producers tags

src/libslic3r/GCode/GCodeProcessor.hpp

@@ -238,6 +238,7 @@ namespace Slic3r {
         static bool contains_reserved_tags(const std::string& gcode, unsigned int max_count, std::vector<std::string>& found_tag);
 
         static int get_gcode_last_filament(const std::string &gcode_str);
+        static bool get_last_z_from_gcode(const std::string& gcode_str, double& z);
 
         static const float Wipe_Width;
         static const float Wipe_Height;

src/libslic3r/GCodeWriter.cpp

@@ -374,20 +374,30 @@ std::string GCodeWriter::travel_to_xyz(const Vec3d &point, const std::string &co
         /*  In all the other cases, we perform an actual XYZ move and cancel
             the lift. */
         m_lifted = 0;
-        m_pos = point;
     }
     
     //BBS: take plate offset into consider
-    this->set_current_position_clear(true);
     Vec3d point_on_plate = { dest_point(0) - m_x_offset, dest_point(1) - m_y_offset, dest_point(2) };
-    m_pos = dest_point;
-
+    std::string out_string;
     GCodeG1Formatter w;
-    w.emit_xyz(point_on_plate);
-    w.emit_f(this->config.travel_speed.value * 60.0);
-    //BBS
-    w.emit_comment(GCodeWriter::full_gcode_comment, comment);
-    return w.string();
+    if (!this->is_current_position_clear())
+    {
+        //force to move xy first then z after filament change
+        w.emit_xy(Vec2d(point_on_plate.x(), point_on_plate.y()));
+        w.emit_f(this->config.travel_speed.value * 60.0);
+        w.emit_comment(GCodeWriter::full_gcode_comment, comment);
+        out_string = w.string() + _travel_to_z(point_on_plate.z(), comment);
+    } else {
+        GCodeG1Formatter w;
+        w.emit_xyz(point_on_plate);
+        w.emit_f(this->config.travel_speed.value * 60.0);
+        w.emit_comment(GCodeWriter::full_gcode_comment, comment);
+        out_string = w.string();
+    }
+
+    m_pos = dest_point;
+    this->set_current_position_clear(true);
+    return out_string;
 }
 
 std::string GCodeWriter::travel_to_z(double z, const std::string &comment)

src/libslic3r/GCodeWriter.hpp

@@ -76,6 +76,7 @@ public:
     std::string lift(LiftType lift_type = LiftType::NormalLift);
     std::string unlift();
     Vec3d       get_position() const { return m_pos; }
+    void       set_position(Vec3d& in) { m_pos = in; }
 
     //BBS: set offset for gcode writer
     void set_xy_offset(double x, double y) { m_x_offset = x; m_y_offset = y; }