calculation of COG in Slic3r (#4970)

Implement center of gravity (COG) calculation and gcode output in Slic3r.

lib/Slic3r/Print/GCode.pm

@@ -323,6 +323,7 @@ sub export {
 
     print $fh $gcodegen->writer->update_progress($gcodegen->layer_count, $gcodegen->layer_count, 1);  # 100%
     print $fh $gcodegen->writer->postamble;
+    print $fh $gcodegen->cog_stats;
     
     # get filament stats
     $self->print->clear_filament_stats;

src/test/libslic3r/test_gcode.cpp

@@ -1,4 +1,11 @@
 #include <catch.hpp>
+#include <regex>
+#include "test_data.hpp"
+#include "GCodeReader.hpp"
+#include "GCode.hpp"
+
+using namespace Slic3r::Test;
+using namespace Slic3r;
 
 
 #include "GCode/CoolingBuffer.hpp"
@@ -13,3 +20,50 @@ SCENARIO("Cooling buffer speed factor rewrite enforces precision") {
         }
     }
 }
+
+SCENARIO( "Test of COG calculation") {
+    GIVEN("A default configuration and a print test object") {
+        auto config {Slic3r::Config::new_from_defaults()};
+        auto gcode {std::stringstream("")};
+
+        WHEN("the output is executed with no support material") {
+            Slic3r::Model model;
+            auto print {Slic3r::Test::init_print({TestMesh::cube_20x20x20}, model, config)};
+            print->process();
+            Slic3r::Test::gcode(gcode, print);
+            auto exported {gcode.str()};
+
+            THEN("Some text output is generated.") {
+                REQUIRE(exported.size() > 0);
+            }
+
+            THEN("COG values are contained in output") {
+                REQUIRE(exported.find("; cog_x") != std::string::npos);
+                REQUIRE(exported.find("; cog_y") != std::string::npos);
+                REQUIRE(exported.find("; cog_z") != std::string::npos);
+            }
+
+            THEN("Check if COG values are correct") {
+
+                int cog_x_start = exported.find("; cog_x = ");
+                int cog_x_len = exported.substr(cog_x_start).find('\n');
+                int cog_y_start = exported.find("; cog_y = ");
+                int cog_y_len = exported.substr(cog_y_start).find('\n');
+                int cog_z_start = exported.find("; cog_z = ");
+                int cog_z_len = exported.substr(cog_z_start).find('\n');
+
+                float val_x, val_y, val_z;
+                // crop cog_x text
+                val_x = std::stof(exported.substr(cog_x_start + 10, cog_x_len - 10));
+                val_y = std::stof(exported.substr(cog_y_start + 10, cog_y_len - 10));
+                val_z = std::stof(exported.substr(cog_z_start + 10, cog_z_len - 10));
+
+                REQUIRE(abs(val_x-100.0) <= 0.5);
+                REQUIRE(abs(val_y-100.0) <= 0.5);
+                REQUIRE(abs(val_z-10.0) <= 0.5);
+            }
+        }
+
+        gcode.clear();
+    }
+}

xs/src/libslic3r/GCode.cpp

@@ -575,6 +575,7 @@ GCode::_extrude(ExtrusionPath path, std::string description, double speed)
     std::string comment = ";_EXTRUDE_SET_SPEED";
     if (path.role == erExternalPerimeter) comment += ";_EXTERNAL_PERIMETER";
     gcode += this->writer.set_speed(F, "", this->enable_cooling_markers ? comment : "");
+    Pointf start;
     double path_length = 0;
     {
         std::string comment = this->config.gcode_comments ? description : "";
@@ -582,7 +583,12 @@ GCode::_extrude(ExtrusionPath path, std::string description, double speed)
         for (Lines::const_iterator line = lines.begin(); line != lines.end(); ++line) {
             const double line_length = line->length() * SCALING_FACTOR;
             path_length += line_length;
-            
+
+            this->_cog.x += (this->point_to_gcode(line->a).x + this->point_to_gcode(line->b).x)/2 * line_length;
+            this->_cog.y += (this->point_to_gcode(line->a).y + this->point_to_gcode(line->b).y)/2 * line_length;
+            this->_cog.z += this->writer.get_position().z * line_length;
+            this->_extrusion_length += line_length;
+
             gcode += this->writer.extrude_to_xy(
                 this->point_to_gcode(line->b),
                 e_per_mm * line_length,
@@ -773,5 +779,27 @@ GCode::point_to_gcode(const Point &point)
         unscale(point.y) + this->origin.y - extruder_offset.y
     );
 }
-
+}
+
+
+Pointf3
+GCode::get_cog() {
+    Pointf3 result_cog;
+
+    result_cog.x = this->_cog.x/this->_extrusion_length;
+    result_cog.y = this->_cog.y/this->_extrusion_length;
+    result_cog.z = this->_cog.z/this->_extrusion_length;
+
+    return result_cog;
+}
+
+std::string
+GCode::cog_stats() {
+    std::string gcode;
+
+    gcode += "; cog_x = " + std::to_string(this->_cog.x/this->_extrusion_length) + "\n";
+    gcode += "; cog_y = " + std::to_string(this->_cog.y/this->_extrusion_length) + "\n";
+    gcode += "; cog_z = " + std::to_string(this->_cog.z/this->_extrusion_length) + "\n";
+
+    return gcode;
 }

xs/src/libslic3r/GCode.hpp

@@ -137,9 +137,13 @@ class GCode {
     std::string unretract();
     std::string set_extruder(unsigned int extruder_id);
     Pointf point_to_gcode(const Point &point);
+    Pointf3 get_cog();
+    std::string cog_stats();
     
     private:
     Point _last_pos;
+    Pointf3 _cog;
+    float _extrusion_length;
     bool _last_pos_defined;
     std::string _extrude(ExtrusionPath path, std::string description = "", double speed = -1);
 };

xs/src/libslic3r/PrintGCode.cpp

@@ -320,6 +320,8 @@ PrintGCode::output()
         fh << _gcodegen.writer.set_bed_temperature(0, 0);
     }
 
+    fh << _gcodegen.cog_stats();
+
     // Get filament stats
     _print.filament_stats.clear();
     _print.total_used_filament = 0.0;

xs/xsp/GCode.xsp

@@ -195,6 +195,8 @@
     std::string retract(bool toolchange = false);
     std::string unretract();
     std::string set_extruder(unsigned int extruder_id);
+    Clone<Pointf3> get_cog();
+    std::string cog_stats();
     Clone<Pointf> point_to_gcode(Point* point)
         %code{% RETVAL = THIS->point_to_gcode(*point); %};