Exposed exaggeration factor when rendering heightmap.

Added checkbox to select whether to run autoleveled program or not.
Heightmap can handle non integer from_x and to_x.

CMakeLists.txt

@@ -26,7 +26,7 @@ set(CMAKE_CXX_VISIBILITY_PRESET hidden)
 
 set(TESTS grbl_test.cpp heightmap_test.cpp)
 set(TERJE terje/gcode.cpp terje/gcode.h terje/gcode_parser.h terje/gcode_parser.cpp terje/machine.h terje/machine.cpp terje/grbl.h terje/grbl.cpp terje/comms.h terje/comms.cpp terje/telnet.h terje/telnet.cpp terje/measure_view_model.h terje/measure_view_model.cpp)
-set(SENDER_GRBL_SRC grbl.h grbl.cpp grbl_communication.h grbl_communication.cpp grbl_machine.h grbl_machine.cpp string_utils.h render.h render.cpp heightmap.h heightmap.cpp gcode_parser.h gcode_parser.cpp gcode_commands.h gcode_commands.cpp)
+set(SENDER_GRBL_SRC grbl.h grbl.cpp grbl_communication.h grbl_communication.cpp grbl_machine.h grbl_machine.cpp string_utils.h render.h render.cpp heightmap.h heightmap.cpp gcode_parser.h gcode_parser.cpp gcode_commands.h gcode_commands.cpp sender_app.h sender_app.cpp gcode_file.h gcode_file.cpp)
 
 add_executable(sender main.cpp ${TESTS} ${TERJE} ${SENDER_GRBL_SRC})
 target_link_libraries(sender nanogui GL gtest gtest_main)

gcode_commands.cpp

@@ -37,13 +37,13 @@ glm::vec<3, double> grbl::line_motion_cmd::interpolate(double ratio) {
     return start + delta() * ratio;
 }
 
-std::vector<std::shared_ptr<grbl::motion_cmd>> grbl::line_motion_cmd::split(double lngth) {
+std::vector<grbl::motion_cmd*> grbl::line_motion_cmd::split(double lngth) {
     //don't split up rapid or not fully defined motions
     if (rapid || !start_valid || !position_valid[0] || !position_valid[1] || !position_valid[2]) {
-        return {std::shared_ptr<grbl::motion_cmd>(this)};
+        return {this};
     }
 
-    std::vector<std::shared_ptr<motion_cmd>> result;
+    std::vector<motion_cmd*> result;
     int divisions = (int) std::ceil(length() / lngth);
 
     if (divisions < 1) {
@@ -61,7 +61,7 @@ std::vector<std::shared_ptr<grbl::motion_cmd>> grbl::line_motion_cmd::split(doub
         immediate->feed = feed;
         immediate->position_valid[0] = immediate->position_valid[1] = immediate->position_valid[2] = true;
         immediate->start_valid = true;
-        result.push_back(std::shared_ptr<line_motion_cmd>(immediate));
+        result.push_back(immediate);
 
         last_end = end;
     }
@@ -90,7 +90,7 @@ glm::vec<3, double> grbl::arc_motion_cmd::interpolate(double ratio) {
     return interpolation;
 }
 
-std::vector<std::shared_ptr<grbl::motion_cmd>> grbl::arc_motion_cmd::split(double lngth) {
+std::vector<grbl::motion_cmd*> grbl::arc_motion_cmd::split(double lngth) {
     int divisions = (int) ceil(length() / lngth);
 
     if (divisions < 1)
@@ -99,7 +99,7 @@ std::vector<std::shared_ptr<grbl::motion_cmd>> grbl::arc_motion_cmd::split(doubl
     glm::vec3 lastEnd = start;
 
 
-    std::vector<std::shared_ptr<grbl::motion_cmd>> result;
+    std::vector<grbl::motion_cmd*> result;
 
     for (int i = 1; i <= divisions; i++) {
         glm::vec<3, double> end = interpolate(((double) i) / divisions);
@@ -113,7 +113,7 @@ std::vector<std::shared_ptr<grbl::motion_cmd>> grbl::arc_motion_cmd::split(doubl
         immediate->u = u;
         immediate->v = v;
 
-        result.push_back(std::shared_ptr<arc_motion_cmd>(immediate));
+        result.push_back(immediate);
 
         lastEnd = end;
     }

gcode_commands.h

@@ -52,7 +52,7 @@ namespace grbl {
         // Total travel distance of tool
         virtual double length() const = 0;
         virtual glm::vec<3, double> interpolate(double ratio) = 0;
-        virtual std::vector<std::shared_ptr<motion_cmd>> split(double length) = 0;
+        virtual std::vector<motion_cmd*> split(double length) = 0;
     };
 
     struct line_motion_cmd : public motion_cmd {
@@ -65,7 +65,7 @@ namespace grbl {
 
         double length() const override;
         glm::vec<3, double> interpolate(double ratio) override;
-        std::vector<std::shared_ptr<motion_cmd>> split(double length) override;
+        std::vector<motion_cmd*> split(double length) override;
     };
 
     struct arc_motion_cmd : public motion_cmd {
@@ -112,7 +112,7 @@ namespace grbl {
         }
 
         glm::vec<3, double> interpolate(double ratio) override;
-        std::vector<std::shared_ptr<motion_cmd>> split(double length) override;
+        std::vector<motion_cmd*> split(double length) override;
 
         double length() const override;
 

gcode_file.cpp

@@ -0,0 +1,157 @@
+#include "gcode_file.h"
+#include "gcode_parser.h"
+#include <fstream>
+#include <iomanip>
+#include <utility>
+
+grbl::grbl_file grbl::grbl_file::load(std::string path) {
+    grbl::grbl_parser parser;
+
+    grbl::grbl_file result{};
+
+    std::ifstream in_file{path};
+    if (in_file) {
+        parser.parse(in_file);
+        result = grbl_file(parser.commands);
+    }
+
+    return result;
+}
+
+grbl::grbl_file::grbl_file(std::vector<grbl::command*> cmd)
+        : commands(cmd) {
+
+}
+
+std::vector<std::string> grbl::grbl_file::get_gcode() {
+    std::vector<std::string> result;
+
+    result.emplace_back("G90 G91.1 G21 G17");
+
+    grbl::parser_state state;
+    auto xyz = "XYZ";
+
+    std::stringstream ss;
+    ss << std::fixed << std::setprecision(3);
+
+    for (auto &c: commands) {
+        auto *motion = dynamic_cast<motion_cmd *>(c);
+        bool is_motion = motion != nullptr;
+
+        auto *line = dynamic_cast<line_motion_cmd *>(c);
+        auto *arc = dynamic_cast<arc_motion_cmd *>(c);
+        auto *mcode = dynamic_cast<mcode_cmd *>(c);
+        auto *spindle = dynamic_cast<spindle_cmd *>(c);
+        auto *dwell = dynamic_cast<dwell_cmd *>(c);
+
+        if (is_motion) {
+            if (motion->feed != state.feed) {
+                ss.str("");
+                ss << "F" << motion->feed;
+                result.push_back(ss.str());
+                state.feed = motion->feed;
+            }
+        }
+
+        if (line != nullptr) {
+            std::string code = line->rapid ? "G0" : "G1";
+
+            for (int i = 0; i < 3; i++) {
+                if (!line->position_valid[i])
+                    continue;
+                if (!line->start_valid || state.position[i] != line->end[i]) {
+                    ss.str("");
+                    ss << xyz[i] << line->end[i];
+                    code += ss.str();
+                }
+            }
+
+            result.push_back(code);
+
+            state.position = line->end;
+            continue;
+        }
+
+        if (arc != nullptr) {
+            if (state.plane != arc->plane) {
+                switch (arc->plane) {
+                    case arc_plane::xy:
+                        result.emplace_back("G17");
+                        break;
+                    case arc_plane::yz:
+                        result.emplace_back("G19");
+                        break;
+                    case arc_plane::zx:
+                        result.emplace_back("G18");
+                        break;
+                }
+                state.plane = arc->plane;
+            }
+
+            std::string code = arc->direction == arc_direction::cw ? "G2" : "G3";
+            if (state.position.x != arc->end.x) {
+                ss.str("");
+                ss << "X" << arc->end.x;
+                code += ss.str();
+            }
+            if (state.position.y != arc->end.y) {
+                ss.str("");
+                ss << "Y" << arc->end.y;
+                code += ss.str();
+            }
+            if (state.position.z != arc->end.z) {
+                ss.str("");
+                ss << "Z" << arc->end.z;
+                code += ss.str();
+            }
+
+            glm::vec3 center = roll_components({arc->u, arc->v, 0}, (int) arc->plane) - state.position;
+
+            if (center.x != 0 && arc->plane != arc_plane::yz) {
+                ss.str("");
+                ss << " I" << center.x;
+                code += ss.str();
+            }
+            if (center.y != 0 && arc->plane != arc_plane::zx) {
+                ss.str("");
+                ss << " J" << center.y;
+                code += ss.str();
+            }
+            if (center.z != 0 && arc->plane != arc_plane::xy) {
+                ss.str("");
+                ss << " K" << center.z;
+                code += ss.str();
+            }
+
+            result.push_back(code);
+            state.position = arc->end;
+            continue;
+        }
+
+        if (mcode != nullptr) {
+            int code = mcode->code;
+            if (code == 2 || code == 30)
+                continue;
+
+            result.push_back("M" + std::to_string(code));
+            continue;
+        }
+
+        if (spindle != nullptr) {
+            ss.str("");
+            ss << "S" << spindle->speed;
+            result.push_back(ss.str());
+            continue;
+        }
+
+        if (dwell != nullptr) {
+            ss.str("");
+            ss << "G4 P" << dwell->seconds;
+            result.push_back(ss.str());
+            continue;
+        }
+    }
+
+    return result;
+}
+

gcode_file.h

@@ -0,0 +1,18 @@
+#pragma once
+
+#include <string>
+#include "gcode_commands.h"
+
+namespace grbl {
+
+    struct grbl_file {
+
+        grbl_file() = default;
+        grbl_file(std::vector<grbl::command *> commands);
+        static grbl_file load(std::string path);
+
+        std::vector<std::string> get_gcode();
+
+        std::vector<grbl::command *> commands;
+    };
+}

gcode_parser.cpp

@@ -121,7 +121,7 @@ void grbl::grbl_parser::parse(std::string line, int line_number) {
                 throw parse_exception("M code can only have positive integer parameters", line_number);
             }
 
-            commands.push_back(std::make_shared<mcode_cmd>(param, line_number));
+            commands.push_back(new mcode_cmd{param, line_number});
             it = words.erase(it);
             continue;
         }
@@ -132,7 +132,7 @@ void grbl::grbl_parser::parse(std::string line, int line_number) {
                 warnings.push_back("spindle speed must be positive. (line " + std::to_string(line_number) + ")");
             }
 
-            commands.push_back(std::make_shared<spindle_cmd>(std::abs(param), line_number));
+            commands.push_back(new spindle_cmd{std::abs(param), line_number});
             it = words.erase(it);
             continue;
         }
@@ -203,7 +203,7 @@ void grbl::grbl_parser::parse(std::string line, int line_number) {
                         warnings.push_back("dwell time must be positive. (line " + std::to_string(line_number) + ")");
                     }
 
-                    commands.push_back(std::make_shared<dwell_cmd>(std::abs((*next).parameter), line_number));
+                    commands.push_back(new dwell_cmd{std::abs((*next).parameter), line_number});
                     it = words.erase(it);
                     it = words.erase(it);
                     continue;
@@ -341,7 +341,7 @@ void grbl::grbl_parser::parse(std::string line, int line_number) {
 
             std::memcpy(l->position_valid, state.position_valid, sizeof(bool) * 3);
 
-            commands.push_back(std::shared_ptr<line_motion_cmd>(l));
+            commands.push_back(l);
             state.position = end_pos;
             return;
         }
@@ -512,7 +512,7 @@ void grbl::grbl_parser::parse(std::string line, int line_number) {
             a->line_number = line_number;
             a->plane = state.plane;
 
-            commands.push_back(std::shared_ptr<arc_motion_cmd>(a));
+            commands.push_back(a);
             state.position = end_pos;
             return;
         }

gcode_parser.h

@@ -52,7 +52,7 @@ namespace grbl {
 
     struct grbl_parser {
         parser_state state;
-        std::vector<std::shared_ptr<command>> commands;
+        std::vector<command *> commands;
         std::vector<std::string> warnings;
 
         grbl_parser();

grbl.cpp

@@ -1,5 +1,7 @@
 #include "grbl.h"
 #include "string_utils.h"
+#include "gcode_parser.h"
+#include "gcode_file.h"
 
 #include <utility>
 #include <fstream>
@@ -38,7 +40,7 @@ grbl::program::program(std::string filename) {
 static auto comment_re = std::regex(R"(^\s*\(([^)]*)\)\s*$)");
 static auto gcode_re = std::regex(R"(([a-zA-Z0-9\s.\-]+)\s*(;.*|\(([^)]*)\))?)");
 
-bool grbl::program::load_from_stream(std::istream& in) {
+bool grbl::program::load_from_stream(std::istream &in) {
     instructions.clear();
 
     is_loaded = true;
@@ -74,7 +76,7 @@ bool grbl::program::load_from_stream(std::istream& in) {
     return is_loaded;
 }
 
-bool grbl::program::load_from_string(const std::string& content) {
+bool grbl::program::load_from_string(const std::string &content) {
     std::stringstream in_stream;
     in_stream << content;
 
@@ -95,7 +97,7 @@ bool grbl::program::load_from_file(std::string path) {
     return is_loaded;
 }
 
-std::ostream& operator<<(std::ostream& out, const grbl::instruction_type& t) {
+std::ostream &operator<<(std::ostream &out, const grbl::instruction_type &t) {
     switch (t) {
         case grbl::instruction_type::gcode:
             out << "gcode";
@@ -110,13 +112,118 @@ std::ostream& operator<<(std::ostream& out, const grbl::instruction_type& t) {
     return out;
 }
 
-std::ostream& operator<<(std::ostream& out, const grbl::instruction& i) {
+std::ostream &operator<<(std::ostream &out, const grbl::instruction &i) {
-    out << "{.line: " << i.line << ", .type: " << i.type << ", .cmd: " << i.command << ", .comment: " << i.comment << " }";
+    out << "{.line: " << i.line << ", .type: " << i.type << ", .cmd: " << i.command << ", .comment: " << i.comment
+        << " }";
     return out;
 }
 
-void grbl::program::dump(std::ostream& out) {
+void grbl::program::dump(std::ostream &out) {
-    for (auto& i: instructions) {
+    for (auto &i: instructions) {
         out << i << std::endl;
     }
 }
+
+grbl::program grbl::program::apply_heightmap(grbl::heightmap& grid) {
+
+    double segmentLength = grid.resolution;
+
+    grbl::program result;
+
+    std::vector<command*> new_commands;
+
+    grbl::grbl_parser parser;
+    std::ifstream in_file{filename};
+    if (in_file) {
+        parser.parse(in_file);
+
+        for (auto &c: parser.commands) {
+            auto line = dynamic_cast<grbl::line_motion_cmd *>(c);
+            auto arc = dynamic_cast<grbl::arc_motion_cmd *>(c);
+            if (line != nullptr) {
+
+                // do not split up or modify any lines that are rapid or not fully defined
+                if (!line->start_valid ||
+                    (!line->position_valid[0] || !line->position_valid[1] || !line->position_valid[2]) || line->rapid) {
+                    new_commands.push_back(line);
+                    continue;
+                }
+
+                for (auto &m: line->split(segmentLength)) {
+                    m->start.z += grid.get_z_at(m->start.x, m->start.y);
+                    m->end.z += grid.get_z_at(m->end.x, m->end.y);
+
+                    new_commands.push_back(m);
+                }
+
+            } else if (arc != nullptr) {
+                if (arc->plane != arc_plane::xy) {
+                    throw std::runtime_error(
+                            "GCode contains arcs in YZ or XZ plane (G18/19), can't apply height map. Use 'Arcs to Lines' if you really need this.");
+                }
+
+                for (auto &m: arc->split(segmentLength)) {
+                    m->start.z += grid.get_z_at(m->start.x, m->start.y);
+                    m->end.z += grid.get_z_at(m->end.x, m->end.y);
+
+                    new_commands.push_back(m);
+                }
+
+            } else {
+                new_commands.push_back(c);
+            }
+        }
+
+        grbl::grbl_file file(new_commands);
+        auto new_lines = file.get_gcode();
+        result.load_from_lines(new_lines);
+    }
+
+    return result;
+
+}
+
+bool grbl::program::load_from_lines(std::vector<std::string> lines) {
+    is_loaded = true;
+    size_t line_number = 0;
+    for (auto &line: lines) {
+        line_number++;
+        if (!line.empty()) {
+
+            std::smatch sm{};
+            if (std::regex_match(line, sm, comment_re)) {
+                auto comment = sm.str(1);
+                instructions.emplace_back(instruction::new_comment(line_number, comment));
+            } else if (std::regex_match(line, sm, gcode_re)) {
+                auto command = trim(sm.str(1));
+                auto comment = trim(sm.str(3));
+                instructions.emplace_back(instruction::new_gcode(line_number, command, comment));
+            } else {
+                std::cerr << "Failed to parse line " << line << std::endl;
+                is_loaded = false;
+            }
+        }
+    }
+
+    return is_loaded;
+}
+
+void grbl::program::save(std::string path) {
+    std::ofstream out_file{path};
+    if (out_file) {
+        for (auto &i: instructions) {
+            switch (i.type) {
+                case instruction_type::gcode:
+                    out_file << i.command;
+                    if (!i.comment.empty())
+                        out_file << " (" << i.comment << ")";
+                    out_file << std::endl;
+                    break;
+                default:
+                    break;
+            }
+
+        }
+    }
+
+}

grbl.h

@@ -3,6 +3,7 @@
 #include <string>
 #include <utility>
 #include <vector>
+#include "heightmap.h"
 
 namespace grbl {
 
@@ -32,6 +33,7 @@ struct program {
     bool load_from_file(std::string filename);
     bool load_from_stream(std::istream& in);
     bool load_from_string(const std::string& content);
+    bool load_from_lines(std::vector<std::string> lines);
 
     void dump(std::ostream& out);
 
@@ -42,10 +44,11 @@ struct program {
     instruction instruction_at(size_t index) {
         return instructions.at(index);
     }
-
     bool is_loaded = false;
     std::string filename;
     std::vector<instruction> instructions{};
+    grbl::program apply_heightmap(grbl::heightmap& grid);
+    void save(std::string path);
 };
 
 }

grbl_machine.cpp

@@ -38,7 +38,7 @@ void grbl::machine::on_disconnected(grbl::transport *transport) {
     switch_to_state(grbl_machine_state::disconnected);
 }
 
-grbl::realtime_status_report grbl::parse_status_report(std::string line, grbl::realtime_status_report& result) {
+grbl::realtime_status_report grbl::parse_status_report(std::string line, grbl::realtime_status_report &result) {
 //    grbl::realtime_status_report result;
 
     // pin values are always reset when a report arrives
@@ -71,7 +71,7 @@ grbl::realtime_status_report grbl::parse_status_report(std::string line, grbl::r
                 result.buffers_free = std::stoi(p[0]);
                 result.rx_chars_free = std::stoi(p[1]);
             } else if (elements[0] == "Pn") {
-                for (auto& c: elements[1]) {
+                for (auto &c: elements[1]) {
                     switch (c) {
                         case 'P':
                             result.signals.bit.probe = true;
@@ -111,7 +111,7 @@ grbl::realtime_status_report grbl::parse_status_report(std::string line, grbl::r
 }
 
 
-grbl::machine_status grbl::status_from_string(const std::string& status) {
+grbl::machine_status grbl::status_from_string(const std::string &status) {
     if (status == "Idle") return machine_status::idle;
     if (status == "Run") return machine_status::run;
     if (status == "Hold") return machine_status::hold;
@@ -125,9 +125,10 @@ grbl::machine_status grbl::status_from_string(const std::string& status) {
     return machine_status::unknown;
 }
 
-void grbl::machine::check_program(const grbl::program& pgm) {
+void grbl::machine::check_program(const grbl::program &pgm) {
     running_program = pgm;
-    std::cout << "checking program (" << running_program.filename << ") with " << running_program.number_of_instructions()
+    std::cout << "checking program (" << running_program.filename << ") with "
+              << running_program.number_of_instructions()
               << " instructions" << std::endl;
     switch_to_state(grbl_machine_state::check_program);
 }
@@ -149,9 +150,10 @@ void grbl::machine::set_work_offset(std::string work_offset) {
 
 }
 
-void grbl::machine::run_program(const grbl::program& pgm, const std::string& work_offset) {
+void grbl::machine::run_program(const grbl::program &pgm, const std::string &work_offset) {
     running_program = pgm;
-    std::cout << "running program (" << running_program.filename << ") with " << running_program.number_of_instructions() << " instructions"
+    std::cout << "running program (" << running_program.filename << ") with "
+              << running_program.number_of_instructions() << " instructions"
               << " on work offset " << work_offset << std::endl;
 
     set_work_offset(work_offset);
@@ -159,7 +161,7 @@ void grbl::machine::run_program(const grbl::program& pgm, const std::string& wor
     switch_to_state(grbl_machine_state::run_program);
 }
 
-std::string grbl::status_to_string(const grbl::machine_status& status) {
+std::string grbl::status_to_string(const grbl::machine_status &status) {
     switch (status) {
         case machine_status::idle:
             return "Idle";
@@ -590,11 +592,11 @@ void grbl::machine::switch_to_state(grbl::grbl_machine_state new_state) {
     states[state]->on_enter(this);
 }
 
-const std::map<std::string, std::string, grbl::settings_cmp>& grbl::machine::get_settings() const {
+const std::map<std::string, std::string, grbl::settings_cmp> &grbl::machine::get_settings() const {
     return settings;
 }
 
-const std::map<std::string, std::string, grbl::parameters_cmp>& grbl::machine::get_parameters() const {
+const std::map<std::string, std::string, grbl::parameters_cmp> &grbl::machine::get_parameters() const {
     return parameters;
 }
 
@@ -683,7 +685,7 @@ void grbl::machine::start_z_probe(float min_z, float feed_rate) {
     awaiting_responses++;
 }
 
-void grbl::machine::probe_heightmap(grbl::heightmap& grid) {
+void grbl::machine::probe_heightmap(grbl::heightmap &grid) {
     std::cout << "probing heightmap" << std::endl;
     dynamic_cast<machine_state_heightmap_probing *>(states[grbl_machine_state::heightmap_probing])->grid = &grid;
     switch_to_state(grbl_machine_state::heightmap_probing);
@@ -756,7 +758,8 @@ void grbl::machine_state_init::on_line_received(std::string line) {
         if (starts_with(line, "$")) {
             auto pieces = split_string(line, "=");
             cnc->settings[pieces[0]] = pieces[1];
-        } else if (starts_with(line, "[G") || starts_with(line, "[H") || starts_with(line, "[T") || starts_with(line, "[P")) {
+        } else if (starts_with(line, "[G") || starts_with(line, "[H") || starts_with(line, "[T") ||
+                   starts_with(line, "[P")) {
             line = line.substr(1, line.size() - 2);
             // TODO: some parameters have more than two :
             auto pieces = split_string(line, ":");
@@ -833,7 +836,8 @@ void grbl::machine_state_idle::on_line_received(std::string line) {
     } else if (starts_with(line, "$")) {
         auto pieces = split_string(line, "=");
         cnc->settings[pieces[0]] = pieces[1];
-    } else if (starts_with(line, "[G") || starts_with(line, "[H") || starts_with(line, "[T") || starts_with(line, "[P")) {
+    } else if (starts_with(line, "[G") || starts_with(line, "[H") || starts_with(line, "[T") ||
+               starts_with(line, "[P")) {
         line = line.substr(1, line.size() - 2);
         // TODO: some parameters have more than two :
         auto pieces = split_string(line, ":");
@@ -948,7 +952,8 @@ bool grbl::machine_state_check_program::continue_program() {
     instruction to_send;
     do {
         to_send = cnc->running_program.instruction_at(cnc->executed_instructions++);
-    } while (to_send.type != instruction_type::gcode && cnc->executed_instructions < cnc->running_program.number_of_instructions());
+    } while (to_send.type != instruction_type::gcode &&
+             cnc->executed_instructions < cnc->running_program.number_of_instructions());
 
     if (to_send.type == instruction_type::gcode) {
         cnc->pipe->send(to_send.command);
@@ -1014,7 +1019,8 @@ bool grbl::machine_state_run_program::continue_program() {
     instruction to_send;
     do {
         to_send = cnc->running_program.instruction_at(cnc->executed_instructions++);
-    } while (to_send.type != instruction_type::gcode && cnc->executed_instructions < cnc->running_program.number_of_instructions());
+    } while (to_send.type != instruction_type::gcode &&
+             cnc->executed_instructions < cnc->running_program.number_of_instructions());
 
     if (to_send.type == instruction_type::gcode) {
         cnc->pipe->send(to_send.command);
@@ -1116,21 +1122,23 @@ bool grbl::machine_state_heightmap_probing::continue_program() {
 void grbl::machine_state_heightmap_probing::move_to_next_stage() {
     switch (stage) {
         case heightmap_probing_stage::start:
-            cnc->pipe->send("G0X0Y0Z15");
+            cnc->pipe->send("G0 X" + std::to_string(grid->vertices[0].x) + " Y" +
+                            std::to_string(grid->vertices[0].y) + "Z0.5");
+
             stage = heightmap_probing_stage::goto_home;
             break;
         case heightmap_probing_stage::goto_home:
-            cnc->pipe->send("G38.2 Z-65 F100");
+            cnc->pipe->send("G38.2 Z-65 F5");
             stage = heightmap_probing_stage::initial_probe_step_back;
             break;
         case heightmap_probing_stage::initial_probe_step_back:
             // step back a bit (1mm, relative)
-            cnc->pipe->send("G91 G0 Z1");
+            cnc->pipe->send("G91 G0 Z0.3");
             stage = heightmap_probing_stage::initial_probe_fine_seek;
             break;
         case heightmap_probing_stage::initial_probe_fine_seek:
             // probe again but finer
-            cnc->pipe->send(" G38.2 Z-5 F5");
+            cnc->pipe->send("G38.2 Z-1 F5");
             stage = heightmap_probing_stage::initial_probe;
             break;
         case heightmap_probing_stage::initial_probe:
@@ -1147,7 +1155,6 @@ void grbl::machine_state_heightmap_probing::move_to_next_stage() {
                 grid->vertices[probed_locations].z = 0;
             }
 
-
             stage = heightmap_probing_stage::goto_next_location;
             break;
         case heightmap_probing_stage::goto_next_location: {
@@ -1172,7 +1179,7 @@ void grbl::machine_state_heightmap_probing::move_to_next_stage() {
                 cnc->pipe->send("G0Z15"); // safe height
                 stage = heightmap_probing_stage::done;
             } else {
-                cnc->pipe->send("G90 G0 Z1.5");
+                cnc->pipe->send("G90 G0 Z1");
                 stage = heightmap_probing_stage::goto_next_location_move;
             }
             break;
@@ -1182,13 +1189,13 @@ void grbl::machine_state_heightmap_probing::move_to_next_stage() {
             stage = heightmap_probing_stage::goto_start_probing_at;
             break;
         case heightmap_probing_stage::goto_start_probing_at:
-            cnc->pipe->send("G0 Z0.5"); // this appears to move Z upwards instead of downwards. why?
+            cnc->pipe->send("G0 Z0.3"); // this appears to move Z upwards instead of downwards. why?
             // faking the G0 Z0.5
 //            cnc->pipe->send("G91 G0 Z-1"); // 1.5 - 1 = 0.5.//
             stage = heightmap_probing_stage::probing;
             break;
         case heightmap_probing_stage::probing:
-            cnc->pipe->send("G38.2 Z-5 F5");
+            cnc->pipe->send("G38.2 Z-1 F5");
             stage = heightmap_probing_stage::goto_next_location;
             break;
         case heightmap_probing_stage::done:
@@ -1208,17 +1215,17 @@ grbl::machine_event_disconnect::machine_event_disconnect() {
     machine_event::type = machine_event_type::disconnected;
 }
 
-grbl::machine_event_report_received::machine_event_report_received(const grbl::realtime_status_report& r)
+grbl::machine_event_report_received::machine_event_report_received(const grbl::realtime_status_report &r)
         : report(r) {
     machine_event::type = machine_event_type::report_received;
 }
 
-grbl::machine_event_banner::machine_event_banner(const std::string& b)
+grbl::machine_event_banner::machine_event_banner(const std::string &b)
         : banner{b} {
     machine_event::type = machine_event_type::banner;
 }
 
-grbl::machine_event_message::machine_event_message(const std::string& m)
+grbl::machine_event_message::machine_event_message(const std::string &m)
         : message{m} {
     machine_event::type = machine_event_type::message;
 }
@@ -1260,7 +1267,8 @@ grbl::machine_event_probe_result::machine_event_probe_result(bool touched, const
     machine_event::type = machine_event_type::probe_result;
 }
 
-grbl::machine_event_heightmap_probe_acquired::machine_event_heightmap_probe_acquired(grbl::heightmap *g, size_t location)
+grbl::machine_event_heightmap_probe_acquired::machine_event_heightmap_probe_acquired(grbl::heightmap *g,
+                                                                                     size_t location)
         : grid(g),
           probed_location(location) {
     machine_event::type = machine_event_type::heightmap_probe_acquired;

grbl_test.cpp

@@ -81,14 +81,14 @@ G00 X0. Y0. Z0.25
     }
 
     for (auto &c: parser.commands) {
-        auto line = dynamic_cast<grbl::line_motion_cmd *>(c.get());
+        auto line = dynamic_cast<grbl::line_motion_cmd *>(c);
         if (line != nullptr) {
             std::cout << "Line from " << glm::to_string(line->start) << " to " << glm::to_string(line->end)
                       << std::endl;
             continue;
         }
 
-        auto arc = dynamic_cast<grbl::arc_motion_cmd *>(c.get());
+        auto arc = dynamic_cast<grbl::arc_motion_cmd *>(c);
         if (arc != nullptr) {
             std::cout << "Arc from " << glm::to_string(arc->start) << " to " << glm::to_string(arc->end) << std::endl;
             continue;

heightmap.cpp

@@ -30,8 +30,8 @@ grbl::heightmap grbl::heightmap::from_params(float from_x, float from_y, float t
 
 size_t grbl::heightmap::index_from_coords(float x, float y) const {
     size_t result = 0;
-    for (auto& v: vertices) {
+    for (auto &v: vertices) {
-        if (v.x == x && v.y == y) {
+        if (float_equal(v.x, x) && float_equal(v.y, y)) {
             return result;
         }
         result++;
@@ -44,21 +44,25 @@ float grbl::heightmap::get_z_at(float x, float y) const {
 
     // TODO: make faster by precalculating indices
 
-    float x1 = x - fmodf(x, resolution);
+    double x1 = x - fmod(x - from_x, resolution);
-    float x2 = x1 + resolution;
+    double x2 = x1 + resolution;
-    float y1 = y - fmodf(y, resolution);
+    double y1 = y - fmod(y - from_y, resolution);
-    float y2 = y1 + resolution;
+    double y2 = y1 + resolution;
-    float z11 = vertices[index_from_coords(x1, y1)].z;
+    double z11 = vertices[index_from_coords(x1, y1)].z;
-    float z12 = vertices[index_from_coords(x1, y2)].z;
+    double z12 = vertices[index_from_coords(x1, y2)].z;
-    float z21 = vertices[index_from_coords(x2, y1)].z;
+    double z21 = vertices[index_from_coords(x2, y1)].z;
-    float z22 = vertices[index_from_coords(x2, y2)].z;
+    double z22 = vertices[index_from_coords(x2, y2)].z;
 
-    float alpha_x = (x - x1) / resolution;
+    double alpha_x = (x - x1) / (double) resolution;
-    float a = z11 + (z21 - z11) * alpha_x;
+    double a = z11 + (z21 - z11) * alpha_x;
-    float b = z12 + (z22 - z12) * alpha_x;
+    double b = z12 + (z22 - z12) * alpha_x;
 
-    float alpha_y = (y - y1) / resolution;
+    double alpha_y = (y - y1) / (double) resolution;
-    float c = a + (b - a) * alpha_y;
+    double c = a + (b - a) * alpha_y;
 
-    return c;
+    return (float) c;
+}
+
+bool grbl::float_equal(float a, float b) {
+    return fabs(a - b) < 0.0001;
 }

heightmap.h

@@ -7,7 +7,9 @@
 
 namespace grbl {
 
-struct heightmap {
+    bool float_equal(float a, float b);
+
+    struct heightmap {
         static heightmap from_params(float from_x, float from_y, float to_x, float to_y, float resolution);
         float get_z_at(float x, float y) const;
         size_t index_from_coords(float x, float y) const;
@@ -17,7 +19,6 @@ struct heightmap {
         float resolution;
         size_t x_segments, y_segments;
         std::vector<glm::vec3> vertices;
-};
+    };
-
 
 }

heightmap_test.cpp

@@ -24,3 +24,26 @@ TEST(heightmap, get_z_at) {
     z = grid.get_z_at(1, 0.5f);
     EXPECT_EQ(2, z);
 }
+
+TEST(heightmap, get_z_at_non_zero_origin) {
+    auto grid = grbl::heightmap::from_params(0 - 0.123, 0 + 0.234, 1 - 0.123, 1 + 0.234, 1);
+    grid.vertices[0].z = 0;
+    grid.vertices[1].z = 1;
+    grid.vertices[2].z = 2;
+    grid.vertices[3].z = 3;
+
+    auto z = grid.get_z_at(0.5f - 0.123f, 0.5f + 0.234f);
+    EXPECT_EQ(true, grbl::float_equal(1.5, z));
+
+    z = grid.get_z_at(0.5f - 0.123f, 0 + 0.234f);
+    EXPECT_EQ(true, grbl::float_equal(0.5, z));
+
+    z = grid.get_z_at(0.5f - 0.123f, 1 + 0.234f);
+    EXPECT_EQ(true, grbl::float_equal(2.5, z));
+
+    z = grid.get_z_at(0 - 0.123f, 0.5f + 0.234f);
+    EXPECT_EQ(true, grbl::float_equal(1, z));
+
+    z = grid.get_z_at(1 - 0.123f, 0.5f + 0.234f);
+    EXPECT_EQ(true, grbl::float_equal(2, z));
+}

render.cpp

@@ -346,84 +346,8 @@ void grbl::program_renderer::initialize_program_buffers() {
 GLsizei grbl::program_renderer::update_model_vbo(const grbl::program &pgm) {
     if (!pgm.is_loaded) return 0;
 
-    min_pos = max_pos = glm::vec3(0);
+    min_pos = glm::vec3(std::numeric_limits<float>::max());
-    /*
+    max_pos = glm::vec3(std::numeric_limits<float>::min());
-    static auto movement_re = std::regex(R"(([gG]0*1?\s+|[xXyYzZ]\s*[0-9\.\-]+))");
-    bool is_tool_on = false;
-
-    glm::vec3 tool_pos;
-    min_pos = max_pos = tool_pos = glm::vec3(0);
-
-    std::vector<float> buffer_data;
-    for (auto& i: pgm.instructions) {
-        if (i.type == grbl::instruction_type::gcode) {
-
-            std::vector<std::string> tokens;
-            std::smatch res;
-            std::string::const_iterator start(i.command.cbegin());
-            while (std::regex_search(start, i.command.cend(), res, movement_re)) {
-                auto str = res[0].str();
-
-                // make upper case
-                std::transform(str.begin(), str.end(), str.begin(), ::toupper);
-
-                // remove whitespace from things like "X 123.1234"
-                str.erase(remove_if(str.begin(), str.end(), isspace), str.end());
-
-                tokens.push_back(str);
-                start = res.suffix().first;
-            }
-
-            if (!tokens.empty()) {
-                auto new_pos = tool_pos;
-
-                for (auto& t: tokens) {
-                    if (t[0] == 'X') {
-                        new_pos.x = std::stof(t.substr(1));
-                    } else if (t[0] == 'Y') {
-                        new_pos.y = std::stof(t.substr(1));
-                    } else if (t[0] == 'Z') {
-                        new_pos.z = std::stof(t.substr(1));
-                    }
-                }
-
-                bool has_g1 = tokens[0][0] == 'G' && std::stoi(tokens[0].substr(1)) == 1;
-                bool has_g0 = tokens[0][0] == 'G' && std::stoi(tokens[0].substr(1)) == 0;
-                bool is_plunge = has_g1 && tokens.size() > 1 && tokens[1][0] == 'Z';
-                bool is_retract = has_g0 && tokens.size() > 1 && tokens[1][0] == 'Z';
-
-                is_tool_on = has_g1 || (is_retract ? false : is_tool_on);
-
-                auto from_color = glm::vec4(1.0f);
-                auto to_color = glm::vec4(1.0f);
-                if (is_tool_on) {
-                    from_color = glm::vec4(1, 0.6, 0.6, 1);
-                    to_color = glm::vec4(1, 0.6, 0.6, 1);
-                }
-
-                if (is_plunge) {
-                    to_color = glm::vec4(1, 0.6, 0.6, 1);
-                } else if (is_retract) {
-                    to_color = glm::vec4(0.6, 1, 0.6, 1);
-                }
-
-                add_line(buffer_data, tool_pos, from_color, new_pos, to_color);
-
-                // calculate extents
-                min_pos.x = std::min(min_pos.x, new_pos.x);
-                min_pos.y = std::min(min_pos.y, new_pos.y);
-                min_pos.z = std::min(min_pos.z, new_pos.z);
-
-                max_pos.x = std::max(max_pos.x, new_pos.x);
-                max_pos.y = std::max(max_pos.y, new_pos.y);
-                max_pos.z = std::max(max_pos.z, new_pos.z);
-
-                tool_pos = new_pos;
-            }
-        }
-    }
-     */
-
 
     std::vector<float> buffer_data;
 
@@ -436,19 +360,19 @@ GLsizei grbl::program_renderer::update_model_vbo(const grbl::program &pgm) {
         auto feed_color = glm::vec4(1.0f, 0, 0, 1);
 
         for (auto &c: parser.commands) {
-            auto line = dynamic_cast<grbl::line_motion_cmd *>(c.get());
+            auto line = dynamic_cast<grbl::line_motion_cmd *>(c);
-            auto arc = dynamic_cast<grbl::arc_motion_cmd *>(c.get());
+            auto arc = dynamic_cast<grbl::arc_motion_cmd *>(c);
             if (line != nullptr) {
-                if (line->rapid)
+                if (line->rapid) {
                     add_line(buffer_data, line->start, rapid_color, line->end, rapid_color);
-                else
+                } else {
                     add_line(buffer_data, line->start, feed_color, line->end, feed_color);
 
-                if (!line->rapid) {
                     update_model_extents(line->start);
                     update_model_extents(line->end);
                 }
 
+
             } else if (arc != nullptr) {
                 auto pieces = arc->split(0.1);
                 for (auto &p: pieces) {
@@ -472,7 +396,7 @@ GLsizei grbl::program_renderer::update_model_vbo(const grbl::program &pgm) {
 }
 
 
-void grbl::program_renderer::update_grid(const grbl::heightmap &grid) {
+void grbl::program_renderer::update_grid(const grbl::heightmap &grid, float exaggeration_factor) {
 
     glm::vec4 color = {0.5, 0.3, 0, 1};
 
@@ -493,7 +417,6 @@ void grbl::program_renderer::update_grid(const grbl::heightmap &grid) {
             auto p3 = grid.vertices[next];
 
             // exaggerate Z
-            auto exaggeration_factor = 100.0f;
             p1.z *= exaggeration_factor;
             p2.z *= exaggeration_factor;
             p3.z *= exaggeration_factor;

render.h

@@ -19,7 +19,7 @@ public:
 
     glm::vec3 get_extents_max() const { return max_pos; };
 
-    void update_grid(const grbl::heightmap& grid);
+    void update_grid(const grbl::heightmap& grid, float exaggeration_factor);
 private:
     GLsizei update_model_vbo(const grbl::program& pgm);
     void update_model_extents(glm::vec3 point);

sender_app.h

@@ -0,0 +1,152 @@
+#pragma once
+
+#include <nanogui/opengl.h>
+#include <nanogui/screen.h>
+#include <nanogui/window.h>
+#include <nanogui/layout.h>
+#include <nanogui/label.h>
+#include <nanogui/button.h>
+#include <nanogui/popupbutton.h>
+#include <nanogui/progressbar.h>
+#include <nanogui/messagedialog.h>
+#include <nanogui/texture.h>
+#include <nanogui/textarea.h>
+#include <nanogui/textbox.h>
+#include <nanogui/tabwidget.h>
+#include <nanogui/shader.h>
+#include <nanogui/renderpass.h>
+#include <iostream>
+#include <memory>
+#include "grbl_machine.h"
+#include "render.h"
+#include "glm/glm.hpp"
+
+#define STB_IMAGE_STATIC
+#define STB_IMAGE_IMPLEMENTATION
+#if defined(_MSC_VER)
+#  pragma warning (disable: 4505) // don't warn about dead code in stb_image.h
+#elif defined(__GNUC__)
+#   pragma GCC diagnostic ignored "-Wunused-function"
+#endif
+
+#include <stb_image.h>
+#include "grbl.h"
+#include <gtest/gtest.h>
+#include <regex>
+#include "grbl_machine.h"
+#include "string_utils.h"
+#include "render.h"
+#include "glm/gtx/quaternion.hpp"
+#include "nanogui/nanogui.h"
+#include "glm/gtc/matrix_inverse.hpp"
+#include "heightmap.h"
+#include <glm/ext/quaternion_float.hpp>
+#include <glm/ext/quaternion_trigonometric.hpp>
+#include <glm/vec3.hpp> // glm::vec3
+#include <glm/ext/matrix_transform.hpp> // glm::translate, glm::rotate, glm::scale
+#include <glm/ext/matrix_clip_space.hpp> // glm::perspective
+#include <glm/ext/scalar_constants.hpp> // glm::pi
+#include <fstream>
+
+class SenderApp : public nanogui::Screen {
+public:
+    SenderApp();
+
+private:
+    void add_z_probe_markup();
+    void add_work_parameters_markup();
+    void add_status_markup();
+    void add_jogging_markup();
+    void add_dro_markup();
+    void add_program_extents();
+    void add_heightmap_markup();
+    void add_pins_markup();
+
+    static void save_heightmap(const grbl::heightmap &grid, std::string path);
+    grbl::heightmap load_heightmap(std::string path);
+
+    void update_dro();
+    void update_grid();
+    void refresh_offset();
+
+    void fill_heightmap_controls_from_grid(const grbl::heightmap &grid) const;
+    void fill_heightmap_from_model() const;
+    void fill_in_parameters();
+    void fill_in_settings();
+    void init_program_geometry();
+    void set_program_extents() const;
+
+    bool resize_event(const nanogui::Vector2i &size) override;
+    bool keyboard_event(int key, int scancode, int action, int modifiers) override;
+    bool mouse_motion_event(const nanogui::Vector2i &p,
+                            const nanogui::Vector2i &rel,
+                            int button,
+                            int modifiers) override;
+    bool scroll_event(const nanogui::Vector2i &p, const nanogui::Vector2f &rel) override;
+    void draw(NVGcontext *ctx) override;
+    void draw_contents() override;
+
+    grbl::machine cnc{};
+
+    nanogui::TextBox *txt_min_z = nullptr, *txt_feed = nullptr;
+    nanogui::TextBox *txt_heightmap_from_x = nullptr, *txt_heightmap_from_y = nullptr;
+    nanogui::TextBox *txt_heightmap_to_x = nullptr, *txt_heightmap_to_y = nullptr;
+    nanogui::TextBox *txt_heightmap_res = nullptr;
+    nanogui::TextBox *txt_clearance_height = nullptr, *txt_start_probing_at = nullptr, *txt_max_negative_z = nullptr, *txt_final_z_height = nullptr;
+    nanogui::Window *window = nullptr;
+    nanogui::TextBox *txt_status = nullptr;
+    nanogui::TextBox *txt_message = nullptr;
+    nanogui::Color color_red = nanogui::Color(255, 0, 0, 255);
+    nanogui::Color color_green = nanogui::Color(0, 255, 0, 255);
+    nanogui::Button *btn_load_program = nullptr, *btn_check_program = nullptr, *btn_run_program = nullptr;
+
+    nanogui::TabWidget *tab_widget = nullptr;
+
+    nanogui::VScrollPanel *info_vscroll = nullptr;
+    nanogui::Widget *info_layer = nullptr;
+
+    nanogui::Widget *heightmap_layer = nullptr;
+
+
+    nanogui::VScrollPanel *settings_vscroll = nullptr;
+    nanogui::Widget *settings_layer = nullptr;
+
+    nanogui::VScrollPanel *parameters_vscroll = nullptr;
+    nanogui::Widget *parameters_layer = nullptr;
+
+    nanogui::TextBox *mpos_x_text = nullptr, *mpos_y_text = nullptr, *mpos_z_text = nullptr;
+    nanogui::ComboBox *cbo_work_offset = nullptr, *cbo_tool = nullptr, *cbo_jog_feed_rates = nullptr, *cbo_jog_distance = nullptr, *cbo_exaggeration = nullptr;
+
+    nanogui::ToolButton *btn_pin_door = nullptr, *btn_pin_hold = nullptr, *btn_pin_reset = nullptr, *btn_pin_cycle_start = nullptr;
+    nanogui::ToolButton *btn_pin_limit_x = nullptr, *btn_pin_limit_y = nullptr, *btn_pin_limit_z = nullptr, *btn_pin_probe = nullptr;
+
+    nanogui::Button *btn_keyboard_jog = nullptr;
+    nanogui::Button *btn_run_autoleveled = nullptr;
+    std::stringstream dro_ss;
+    grbl::heightmap heightmap_grid;
+
+    nanogui::TextBox *extents_min_x = nullptr, *extents_max_x = nullptr;
+    nanogui::TextBox *extents_min_y = nullptr, *extents_max_y = nullptr;
+    nanogui::TextBox *extents_min_z = nullptr, *extents_max_z = nullptr;
+
+    nanogui::ProgressBar *m_progress = nullptr;
+    nanogui::ref<nanogui::RenderPass> m_render_pass;
+
+    using ImageHolder = std::unique_ptr<uint8_t[], void (*)(void *)>;
+    std::vector<std::pair<nanogui::ref<nanogui::Texture>, ImageHolder>> m_images;
+
+    std::vector<std::string> jog_distances = {"0.01", "0.1", "1", "10"};
+    std::vector<std::string> jog_feed_rates = {"5", "100", "500", "1000"};
+    std::vector<std::string> exaggeration_factors = {"0", "1", "5", "10", "20", "50", "100"};
+
+    int last_alarm = 0;
+
+    grbl::program pgm{};
+    grbl::realtime_status_report last_report;
+    grbl::jog_state jog;
+    grbl::program_renderer renderer{};
+    glm::vec3 cam_target = glm::vec3(0);
+    glm::vec3 cam_pan = glm::vec3(0);
+    float cam_zoom = 0;
+    glm::quat cam_src_rotation = glm::quat(1.0, 0.0, 0.0, 0.0); // identity quaternion
+};