More work on heightmap probing.

docs/todo.txt

@@ -0,0 +1,59 @@
+Heightmap probing:
+    1. define the grid:
+        - from: x, y
+        - to: x, y
+        - step every: 5mm?
+        - clearance height: Z1.5
+        - start probing at: Z0.5
+        - max negative z: Z-0.5 (when to fail probing)
+        - z final safety height: Z15
+
+        - from and to can be filled from current project bounding box
+
+    2. probing
+        - machine moves at grid start position (x, y)
+            G0X0Y0
+        - gets down for initial probing
+            G38.2 Z-65 F100
+        - step back a bit (1mm, relative)
+            G91 G0 Z1
+        - seek again with lower feed rate
+            G38.2 Z-5 F5
+        - after first probe ever, this Z becomes reference 0
+            G10 L20 P0 Z0
+
+        - all subsequent probes will be relative to this
+        - for each probing location:
+            - G0 Z"clearance height"
+            - G0 XxxYyyy (next point location)
+            - G0 Z"start probing at"
+            - G38.2 Z-5 F5
+            - store Z offset and place it in the heightmap
+
+    3. modify loaded program with heightmap data
+        - foreach line in program
+            - if line contains a Z coordinate, update the Z according to the X and Y
+            - if line does not contain a Z coordinate, add a Z coordinate according to the X and Y
+
+Edge finding
+    - prerequisites
+        - which edge to find? -X/+X/-Y/+Y
+        - bit diameter used
+        - put the  
+
+Corner finding
+    - which corner are we probing: up left, up right, bottom left, bottom right
+    - need to know the bit diameter
+    - use G38.2 to probe X edge and set that as current X - diam/2
+
+Render arcs
+Render quadratic splines
+Render bezier splines
+Synchronize executing program with render (show executed paths with different colors)
+Show a progress bar when executing programs.
+Add aggressive buffer execution policy and make it selectable.
+
+DONE - Show program extents
+
+DONE - Render program extents
+DONE    - show a bounding box for the program extents

grbl_machine.cpp

@@ -682,6 +682,12 @@ void grbl::machine::request_jog_fixed(grbl::jog_direction dir, float distance, f
     awaiting_responses++;
 }
 
+void grbl::machine::start_z_probe(float min_z, float feed_rate) {
+    std::string command = "G38.2 Z" + std::to_string(min_z) + "F" + std::to_string(feed_rate);
+    pipe->send(command);
+    awaiting_responses++;
+}
+
 bool grbl::jog_state::no_jogging() const {
     return !(up_pressed || down_pressed || left_pressed || right_pressed || z_up_pressed || z_down_pressed);
 }
@@ -812,6 +818,18 @@ void grbl::machine_state_idle::on_line_received(std::string line) {
         // TODO: some parameters have more than two :
         auto pieces = split_string(line, ":");
         cnc->parameters[pieces[0]] = pieces[1];
+
+        if (starts_with(line, "PRB")) {
+            auto pieces = split_string(line, ":");
+            auto coords_as_string = split_string(pieces[1], ",");
+
+            float probe_coords[3];
+            for (auto i = 0; i < 3; i++) {
+                probe_coords[i] = std::stof(coords_as_string[i]);
+            }
+            bool probe_touched = pieces[2] == "1";
+            cnc->listener->on_probe_result(probe_touched, probe_coords);
+        }
     }
 }
 

grbl_machine.h

@@ -126,6 +126,7 @@ struct machine_listener {
     virtual void on_check_completed(bool success, size_t failed_index, size_t error) = 0;
     virtual void on_settings_reloaded() = 0;
     virtual void on_parameters_reloaded() = 0;
+    virtual void on_probe_result(bool probe_touched, float probe_coords[3]) = 0;
 };
 
 
@@ -277,6 +278,8 @@ struct machine : public transport_callbacks {
     // 0 = G54, 1 = G55, etc axis 0=X, 1=Y, 2=Z
     void zero_offset_axis(int offset_index, int axis);
 
+    void start_z_probe(float min_z, float feed_rate);
+
 protected:
     void on_connected(transport *transport) override;
     void on_disconnected(transport *transport) override;

main.cpp

@@ -33,6 +33,7 @@
 #include "render.h"
 #include "glm/gtx/quaternion.hpp"
 #include "nanogui/nanogui.h"
+#include "glm/gtc/matrix_inverse.hpp"
 #include <glm/ext/quaternion_float.hpp>
 #include <glm/ext/quaternion_trigonometric.hpp>
 #include <glm/vec3.hpp> // glm::vec3
@@ -68,7 +69,7 @@ public:
     float cam_zoom = 0;
     glm::quat cam_src_rotation = glm::quat(1.0, 0.0, 0.0, 0.0); // identity quaternion
 
-    Widget *info_layer;
+    Widget *info_layer, *heightmap_layer;
     TabWidget *tab_widget;
     VScrollPanel *settings_vscroll;
     Widget *settings_layer;
@@ -93,30 +94,71 @@ public:
         window->set_layout(new BoxLayout(nanogui::Orientation::Vertical));
 
         tab_widget = window->add<TabWidget>();
+        tab_widget->set_tabs_draggable(true);
         tab_widget->set_callback([&](int index) {
             tab_widget->set_selected_index(index);
         });
 
         info_layer = tab_widget->add<Widget>();
-        tab_widget->append_tab("Info", info_layer);
-
         info_layer->set_layout(new GroupLayout(10, 20, 30, 0));
         info_layer->add<Label>("");
 
+        tab_widget->append_tab("Info", info_layer);
+
+        auto probe_layer = tab_widget->add<Widget>();
+        probe_layer->set_layout(new GroupLayout(10, 20, 30, 0));
+        probe_layer->add<Label>("");
+
         add_status_markup();
         add_dro_markup();
+        add_program_extents();
         add_pins_markup();
         add_jogging_markup();
         add_work_parameters_markup();
+        add_z_probe_markup();
+
+        heightmap_layer = tab_widget->add<Widget>();
+        heightmap_layer->set_layout(new GroupLayout(10, 20, 30, 0));
+        heightmap_layer->add<Label>("");
+        tab_widget->append_tab("Heightmap", heightmap_layer);
+        add_heightmap_markup();
+
 
         perform_layout();
 
         m_render_pass = new RenderPass({this});
-        m_render_pass->set_clear_color(0, Color(0.3f, 0.3f, 0.32f, 1.f));
+        m_render_pass->set_clear_color(0, Color(0.13f, 0.13f, 0.13f, 1.f));
         m_render_pass->set_depth_test(RenderPass::DepthTest::Always, true);
         m_render_pass->set_cull_mode(RenderPass::CullMode::Disabled);
     }
 
+    TextBox *txt_min_z, *txt_feed;
+
+    void add_z_probe_markup() {
+        info_layer->add<Label>("Z-Probe", "sans-bold", 20);
+        auto z_probe_holder = info_layer->add<Widget>();
+        z_probe_holder->set_layout(new BoxLayout(Orientation::Vertical, Alignment::Fill, 2, 2));
+
+        auto another_holder = z_probe_holder->add<Widget>();
+        another_holder->set_layout(new GridLayout(Orientation::Horizontal, 2, Alignment::Fill, 2, 2));
+
+        another_holder->add<Label>("Feed", "sans-bold");
+        txt_feed = another_holder->add<TextBox>("100");
+        txt_feed->set_editable(true);
+        txt_feed->set_fixed_width(100);
+
+        another_holder->add<Label>("Min Z", "sans-bold");
+        txt_min_z = another_holder->add<TextBox>("-65");
+        txt_min_z->set_editable(true);
+        txt_min_z->set_fixed_width(100);
+
+        z_probe_holder->add<Label>("");
+        auto btn_start_probing = z_probe_holder->add<Button>("Start probing");
+        btn_start_probing->set_callback([&]() {
+            cnc.start_z_probe(std::stof(txt_min_z->value()), std::stof(txt_feed->value()));
+        });
+    }
+
     void add_work_parameters_markup() {
         // work parameters
         info_layer->add<Label>("Work parameters", "sans-bold", 20);
@@ -151,6 +193,9 @@ public:
 
             if (pgm.load_from_file(path)) {
                 init_program_geometry();
+                set_program_extents();
+                fill_heightmap_from_model();
+                update_grid();
             } else {
             }
         });
@@ -383,6 +428,122 @@ public:
         });
     }
 
+    TextBox *extents_min_x, *extents_max_x;
+    TextBox *extents_min_y, *extents_max_y;
+    TextBox *extents_min_z, *extents_max_z;
+
+    void add_program_extents() {
+        dro_ss << std::setprecision(3) << std::fixed;
+
+        // Program extents
+        info_layer->add<Label>("Program extents", "sans-bold", 20);
+        auto extents_holder = info_layer->add<Widget>();
+        extents_holder->set_layout(new GridLayout(Orientation::Horizontal, 3, Alignment::Fill, 4, 4));
+
+        extents_holder->add<Label>("X", "sans-bold", 20);
+        extents_min_x = extents_holder->add<TextBox>("");
+        extents_max_x = extents_holder->add<TextBox>("");
+
+        extents_holder->add<Label>("Y", "sans-bold", 20);
+        extents_min_y = extents_holder->add<TextBox>("");
+        extents_max_y = extents_holder->add<TextBox>("");
+
+        extents_holder->add<Label>("Z", "sans-bold", 20);
+        extents_min_z = extents_holder->add<TextBox>("");
+        extents_max_z = extents_holder->add<TextBox>("");
+    }
+
+    TextBox *txt_heightmap_from_x, *txt_heightmap_from_y;
+    TextBox *txt_heightmap_to_x, *txt_heightmap_to_y;
+    TextBox *txt_grid_res;
+
+    void add_heightmap_markup() {
+        heightmap_layer->add<Label>("Grid definition", "sans-bold", 20);
+        auto heightmap_params_holder = heightmap_layer->add<Widget>();
+        heightmap_params_holder->set_layout(new BoxLayout(Orientation::Vertical, Alignment::Fill, 4, 4));
+
+        // grid size
+        auto grid_size_holder = heightmap_params_holder->add<Widget>();
+        grid_size_holder->set_layout(new GridLayout(Orientation::Horizontal, 3, Alignment::Middle, 4, 4));
+
+        grid_size_holder->add<Label>("", "sans-bold", 20);
+        grid_size_holder->add<Label>("X", "sans-bold", 20);
+        grid_size_holder->add<Label>("Y", "sans-bold", 20);
+
+        grid_size_holder->add<Label>("From", "sans-bold", 20);
+        txt_heightmap_from_x = grid_size_holder->add<TextBox>("");
+        txt_heightmap_from_x->set_editable(true);
+        txt_heightmap_from_x->set_fixed_width(150);
+        txt_heightmap_from_y = grid_size_holder->add<TextBox>("");
+        txt_heightmap_from_y->set_editable(true);
+        txt_heightmap_from_y->set_fixed_width(150);
+
+        grid_size_holder->add<Label>("To", "sans-bold", 20);
+        txt_heightmap_to_x = grid_size_holder->add<TextBox>("");
+        txt_heightmap_to_x->set_editable(true);
+        txt_heightmap_to_x->set_fixed_width(150);
+        txt_heightmap_to_y = grid_size_holder->add<TextBox>("");
+        txt_heightmap_to_y->set_editable(true);
+        txt_heightmap_to_y->set_fixed_width(150);
+
+        grid_size_holder->add<Label>("");
+        auto btn = grid_size_holder->add<Button>("Fetch from model");
+        btn->set_callback([&]() {
+            fill_heightmap_from_model();
+        });
+        grid_size_holder->add<Label>("");
+
+        txt_heightmap_from_x->set_callback([&](const std::string& new_value) {
+            update_grid();
+            return true;
+        });
+        txt_heightmap_from_y->set_callback([&](const std::string& new_value) {
+            update_grid();
+            return true;
+        });
+
+        txt_heightmap_to_x->set_callback([&](const std::string& new_value) {
+            update_grid();
+            return true;
+        });
+        txt_heightmap_to_y->set_callback([&](const std::string& new_value) {
+            update_grid();
+            return true;
+        });
+
+
+        // grid resolution
+        auto grid_res_holder = heightmap_params_holder->add<Widget>();
+        grid_res_holder->set_layout(new BoxLayout(nanogui::Orientation::Horizontal, Alignment::Middle, 4, 4));
+        grid_res_holder->add<Label>("Grid resolution", "sans-bold", 20);
+        txt_grid_res = grid_res_holder->add<TextBox>("5");
+        txt_grid_res->set_editable(true);
+        txt_grid_res->set_fixed_width(150);
+        txt_grid_res->set_callback([&](const std::string& new_value) {
+            update_grid();
+            return true;
+        });
+        grid_res_holder->add<Label>("mm", "sans-bold", 20);
+    }
+
+    void fill_heightmap_from_model() {
+        auto min = renderer.get_extents_min();
+        auto max = renderer.get_extents_max();
+        txt_heightmap_from_x->set_value(std::to_string(min.x));
+        txt_heightmap_from_y->set_value(std::to_string(min.y));
+        txt_heightmap_to_x->set_value(std::to_string(max.x));
+        txt_heightmap_to_y->set_value(std::to_string(max.y));
+    }
+
+    void update_grid() {
+        auto from_x = std::stof(txt_heightmap_from_x->value());
+        auto from_y = std::stof(txt_heightmap_from_y->value());
+        auto to_x = std::stof(txt_heightmap_to_x->value());
+        auto to_y = std::stof(txt_heightmap_to_y->value());
+        auto res = std::stoi(txt_grid_res->value());
+        renderer.update_grid(from_x, from_y, to_x, to_y, res);
+    }
+
     void add_pins_markup() {
         // Pins
         info_layer->add<Label>("Pins", "sans-bold", 20);
@@ -501,6 +662,20 @@ public:
         cam_src_rotation = glm::quat(1.0, 0.0, 0.0, 0.0);
     }
 
+    void set_program_extents() {
+        auto min = renderer.get_extents_min();
+        auto max = renderer.get_extents_max();
+
+        extents_min_x->set_value(std::to_string(min.x));
+        extents_max_x->set_value(std::to_string(max.x));
+
+        extents_min_y->set_value(std::to_string(min.y));
+        extents_max_y->set_value(std::to_string(max.y));
+
+        extents_min_z->set_value(std::to_string(min.z));
+        extents_max_z->set_value(std::to_string(max.z));
+    }
+
     bool resize_event(const Vector2i& size) override {
 //        window->set_fixed_height(this->height() / 2);
         tab_widget->set_fixed_height((this->height() - 30));
@@ -589,6 +764,15 @@ public:
         last_alarm = alarm;
     }
 
+    void on_probe_result(bool probe_touched, float *probe_coords) override {
+        std::stringstream ss;
+        ss << "Probing ended. Result: " << std::boolalpha << probe_touched << " at coords: ";
+        for (auto i = 0; i < 3; i++) {
+            ss << probe_coords[i] << ", ";
+        }
+        new MessageDialog(this, MessageDialog::Type::Warning, "Probe result", ss.str());
+    }
+
     void on_check_completed(bool success, size_t failed_index, size_t error) override {
         btn_check_program->set_background_color(success ? colGreen : color_red);
         if (success) {
@@ -777,9 +961,8 @@ public:
                              glm::translate(glm::mat4(1.0f), -cam_target);
 
             glm::mat4 model = glm::mat4(1.0f);
-            auto mvp = projection * view * model;
+            glm::mat3 normal_mat = glm::inverseTranspose(glm::mat3(view * model));
-
+            renderer.render(model, view, projection, normal_mat, glm::vec2(fb_size.x(), fb_size.y()));
-            renderer.render(mvp, glm::vec2(fb_size.x(), fb_size.y()));
         }
 
         m_render_pass->end();

render.cpp

@@ -4,7 +4,6 @@
 #include "glm/vec3.hpp"
 #include "glm/vec4.hpp"
 #include "glm/gtc/type_ptr.hpp"
-//#include <GL/glext.h>
 
 static const char *vs_code = R"(
     #version 330
@@ -54,8 +53,113 @@ static const char *ps_code = R"(
     }
 )";
 
+static const char *vs_heightmap_code = R"(
+    #version 330
 
-void grbl::program_renderer::render(glm::mat4 mvp, glm::vec2 viewport_size) {
+    layout (location = 0) in vec3 position;
+    layout (location = 1) in vec3 normal;
+    layout (location = 2) in vec4 color;
+
+    uniform mat4 mmtx;
+    uniform mat4 vmtx;
+    uniform mat4 pmtx;
+    uniform mat3 nmtx;
+
+    out vec3 eye_space_normal;
+    out vec3 eye_space_position;
+    out vec4 diffuse_color;
+
+    void main() {
+        mat4 mvp = pmtx * vmtx * mmtx;
+        gl_Position = mvp * vec4(position, 1.0);
+
+        eye_space_position = (vmtx * mmtx * vec4(position, 1)).xyz;
+        eye_space_normal = normalize(nmtx * normal);
+
+        diffuse_color = color;
+    }
+)";
+
+
+static const char *ps_heightmap_code = R"(
+    #version 330
+
+    in vec3 eye_space_normal;
+    in vec3 eye_space_position;
+    in vec4 diffuse_color;
+
+
+    out vec4 color;
+
+    void main() {
+        vec3 normal = normalize(eye_space_normal);
+        vec3 light_dir = normalize(vec3(0, 0, 1));
+        vec4 ambient_color = vec4(0.1, 0.1, 0.1, 1);
+
+        float n_dot_l = max(dot(normal, light_dir), 0.0);
+        color = n_dot_l * diffuse_color + ambient_color;
+    }
+)";
+
+static void add_vertex(std::vector<float>& buffer_data, glm::vec3 v, glm::vec3 n, glm::vec4 col) {
+    buffer_data.push_back(v.x);
+    buffer_data.push_back(v.y);
+    buffer_data.push_back(v.z);
+
+    buffer_data.push_back(n.x);
+    buffer_data.push_back(n.y);
+    buffer_data.push_back(n.z);
+
+    buffer_data.push_back(col.r);
+    buffer_data.push_back(col.g);
+    buffer_data.push_back(col.b);
+    buffer_data.push_back(col.a);
+}
+
+static void add_vertex(std::vector<float>& buffer_data, glm::vec3 v, glm::vec4 col) {
+    buffer_data.push_back(v.x);
+    buffer_data.push_back(v.y);
+    buffer_data.push_back(v.z);
+
+    buffer_data.push_back(col.r);
+    buffer_data.push_back(col.g);
+    buffer_data.push_back(col.b);
+    buffer_data.push_back(col.a);
+}
+
+static void add_line(std::vector<float>& buffer_data, glm::vec3 from, glm::vec4 from_col, glm::vec3 to, glm::vec4 to_col) {
+    add_vertex(buffer_data, from, from_col);
+    add_vertex(buffer_data, to, to_col);
+}
+
+static void add_line(std::vector<float>& buffer_data, glm::vec3 from, glm::vec3 to, glm::vec4 col) {
+    add_vertex(buffer_data, from, col);
+    add_vertex(buffer_data, to, col);
+}
+
+static void add_triangle(std::vector<float>& buffer_data, glm::vec3 p1, glm::vec3 p2, glm::vec3 p3, glm::vec3 n, glm::vec4 col) {
+    add_vertex(buffer_data, p1, n, col);
+    add_vertex(buffer_data, p2, n, col);
+    add_vertex(buffer_data, p3, n, col);
+}
+
+
+void grbl::program_renderer::render(glm::mat4 model, glm::mat4 view, glm::mat4 projection, glm::mat3 normal_mat, glm::vec2 viewport_size) {
+    // draw heightmap
+    heightmap_shader->bind();
+    heightmap_shader->set_mat4(glm::value_ptr(model), "mmtx");
+    heightmap_shader->set_mat4(glm::value_ptr(view), "vmtx");
+    heightmap_shader->set_mat4(glm::value_ptr(projection), "pmtx");
+    heightmap_shader->set_mat3(glm::value_ptr(normal_mat), "nmtx");
+
+    glBindVertexArray(heightmap_vao_id);
+    glDrawArrays(GL_TRIANGLES, 0, heightmap_vertices_count);
+
+    heightmap_shader->unbind();
+
+    auto mvp = projection * view * model;
+
+    // draw model
     shader->bind();
     shader->set_mat4(glm::value_ptr(mvp), "mvp");
     shader->set_vec2(glm::value_ptr(viewport_size), "u_resolution");
@@ -63,7 +167,7 @@ void grbl::program_renderer::render(glm::mat4 mvp, glm::vec2 viewport_size) {
     glBindVertexArray(vao_id);
     glDrawArrays(GL_LINES, 0, vertices_count);
 
-
+    // draw spindle
     auto spindle_xform = glm::translate(glm::mat4(1.0f), spindle_pos);
     auto spindle_view = mvp * spindle_xform;
     shader->set_mat4(glm::value_ptr(spindle_view), "mvp");
@@ -71,34 +175,46 @@ void grbl::program_renderer::render(glm::mat4 mvp, glm::vec2 viewport_size) {
     glBindVertexArray(spindle_vao_id);
     glDrawArrays(GL_LINES, 0, spindle_vertices_count);
 
-    shader->unbind();
+    // draw bounding box
+    auto bbox_size = max_pos - min_pos;
 
-    // draw bit location
+    auto bbox_translation = glm::translate(glm::mat4(1.0f), min_pos);
+    auto bbox_scale = glm::scale(glm::mat4(1.0f), bbox_size);
+    auto bbox_view = mvp * bbox_translation * bbox_scale;
+    shader->set_mat4(glm::value_ptr(bbox_view), "mvp");
+
+    glBindVertexArray(extents_vao_id);
+    glDrawArrays(GL_LINES, 0, extents_vertices_count);
+
+    shader->unbind();
 }
 
 void grbl::program_renderer::update(const grbl::program& pgm, const grbl::machine& cnc) {
     if (!initialized) {
-
         shader = new shader_program(vs_code, ps_code);
+        heightmap_shader = new shader_program(vs_heightmap_code, ps_heightmap_code);
 
         initialize_spindle_buffers();
         initialize_program_buffers();
+        initialize_extents_buffers();
+        initialize_heightmap_buffers();
 
         initialized = true;
     }
 
     // update program with machine status
     // build vbo and vao
-    vertices_count = build_vbo(pgm);
+    vertices_count = update_model_vbo(pgm);
 
-    auto work_pos = cnc.get_status().machine_pos;
+    auto machine_pos = glm::vec3{cnc.get_status().machine_pos[0],
-    auto offsets = cnc.get_current_work_offset_values();
+                                 cnc.get_status().machine_pos[1],
-    for (int i = 0; i < 3; i++) {
+                                 cnc.get_status().machine_pos[2]};
-        work_pos[i] -= offsets[i];
+    auto offsets = glm::vec3{cnc.get_current_work_offset_values()[0],
+                             cnc.get_current_work_offset_values()[1],
+                             cnc.get_current_work_offset_values()[2]};
+    spindle_pos = machine_pos - offsets;
 }
 
-    spindle_pos = glm::vec3(work_pos[0], work_pos[1], work_pos[2]);
-}
 
 void grbl::program_renderer::initialize_spindle_buffers() {
     glGenBuffers(1, &spindle_vbo_id);
@@ -106,56 +222,97 @@ void grbl::program_renderer::initialize_spindle_buffers() {
 
     // vertex format: x, y, z, r, g, b, a
     // stride: 28 bytes
-    const GLsizei sizeOfVertexInBytes = 28;
+    const GLsizei size_of_vertex_in_bytes = 28;
 
     glBindVertexArray(spindle_vao_id);
     glBindBuffer(GL_ARRAY_BUFFER, spindle_vbo_id);
 
     glEnableVertexAttribArray(0); // vertices on stream 0
-    glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, sizeOfVertexInBytes, (void *) 0);
+    glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, size_of_vertex_in_bytes, (void *) nullptr);
 
     glEnableVertexAttribArray(1); // vertex colors on stream 1
-    glVertexAttribPointer(1, 4, GL_FLOAT, GL_FALSE, sizeOfVertexInBytes, (void *) (sizeof(float) * 3));
+    glVertexAttribPointer(1, 4, GL_FLOAT, GL_FALSE, size_of_vertex_in_bytes, (void *) (sizeof(float) * 3));
 
     // unbind vao
     glBindVertexArray(0);
 
     // construct spindle model
-    glm::vec4 col(1, 1, 0, 1); // yellow
+    glm::vec4 col(1, 1, 0, 1);
 
     std::vector<float> buffer_data;
 
     const float spindle_length = 3.5;
     const size_t cone_granularity = 20;
     for (int i = 0; i < cone_granularity; i++) {
-        float x = sinf((i / (float) cone_granularity) * 2.0f * M_PI);
+        float x = sinf((i / (float) cone_granularity) * 2.0f * glm::pi<float>());
-        float y = cosf((i / (float) cone_granularity) * 2.0f * M_PI);
+        float y = cosf((i / (float) cone_granularity) * 2.0f * glm::pi<float>());
 
-        // from
+        add_line(buffer_data, {0, 0, 0}, {x, y, spindle_length}, col);
-        buffer_data.push_back(0);
-        buffer_data.push_back(0);
-        buffer_data.push_back(0);
-
-        buffer_data.push_back(col.r);
-        buffer_data.push_back(col.g);
-        buffer_data.push_back(col.b);
-        buffer_data.push_back(col.a);
-
-        // to
-        buffer_data.push_back(x);
-        buffer_data.push_back(y);
-        buffer_data.push_back(spindle_length);
-
-        buffer_data.push_back(col.r);
-        buffer_data.push_back(col.g);
-        buffer_data.push_back(col.b);
-        buffer_data.push_back(col.a);
     }
 
     glBindBuffer(GL_ARRAY_BUFFER, spindle_vbo_id);
     glBufferData(GL_ARRAY_BUFFER, sizeof(float) * buffer_data.size(), buffer_data.data(), GL_STATIC_DRAW);
 
-    spindle_vertices_count = buffer_data.size() * sizeof(float) / sizeOfVertexInBytes;
+    spindle_vertices_count = buffer_data.size() * sizeof(float) / size_of_vertex_in_bytes;
+}
+
+void grbl::program_renderer::initialize_extents_buffers() {
+    glGenBuffers(1, &extents_vbo_id);
+    glGenVertexArrays(1, &extents_vao_id);
+
+    // vertex format: x, y, z, r, g, b, a
+    // stride: 28 bytes
+    const GLsizei size_of_vertex_in_bytes = 28;
+
+    // we're going to draw a simple box for the extents
+    // box is made up of 8 vertices and 12 lines
+
+    glBindVertexArray(extents_vao_id);
+    glBindBuffer(GL_ARRAY_BUFFER, extents_vbo_id);
+
+    glEnableVertexAttribArray(0); // vertices on stream 0
+    glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, size_of_vertex_in_bytes, (void *) 0);
+
+    glEnableVertexAttribArray(1); // vertex colors on stream 1
+    glVertexAttribPointer(1, 4, GL_FLOAT, GL_FALSE, size_of_vertex_in_bytes, (void *) (sizeof(float) * 3));
+
+    // unbind vao
+    glBindVertexArray(0);
+
+    // we're going to make the box as unit box and scale it when rendering
+    // since we're going to reuse the same box for all loaded programs.
+    // or at least we'll try to.
+
+    // construct box
+    glm::vec4 col(1, 0.7, 0.3, 1);
+
+
+    // box will range from [0,0,0] to [1,1,1] and we'll use
+    // translation and scaling afterward if needed to place it
+
+    std::vector<float> buffer_data;
+    // bottom plane
+    add_line(buffer_data, {0, 0, 0}, {1, 0, 0}, col);
+    add_line(buffer_data, {1, 0, 0}, {1, 1, 0}, col);
+    add_line(buffer_data, {1, 1, 0}, {0, 1, 0}, col);
+    add_line(buffer_data, {0, 1, 0}, {0, 0, 0}, col);
+
+    // top plane
+    add_line(buffer_data, {0, 0, 1}, {1, 0, 1}, col);
+    add_line(buffer_data, {1, 0, 1}, {1, 1, 1}, col);
+    add_line(buffer_data, {1, 1, 1}, {0, 1, 1}, col);
+    add_line(buffer_data, {0, 1, 1}, {0, 0, 1}, col);
+
+    // add vertical lines connecting the planes
+    add_line(buffer_data, {0, 0, 0}, {0, 0, 1}, col);
+    add_line(buffer_data, {1, 0, 0}, {1, 0, 1}, col);
+    add_line(buffer_data, {1, 1, 0}, {1, 1, 1}, col);
+    add_line(buffer_data, {0, 1, 0}, {0, 1, 1}, col);
+
+    glBindBuffer(GL_ARRAY_BUFFER, extents_vbo_id);
+    glBufferData(GL_ARRAY_BUFFER, sizeof(float) * buffer_data.size(), buffer_data.data(), GL_STATIC_DRAW);
+
+    extents_vertices_count = buffer_data.size() * sizeof(float) / size_of_vertex_in_bytes;
 }
 
 void grbl::program_renderer::initialize_program_buffers() {
@@ -179,16 +336,14 @@ void grbl::program_renderer::initialize_program_buffers() {
     glBindVertexArray(0);
 }
 
-GLsizei grbl::program_renderer::build_vbo(const grbl::program& pgm) {
+GLsizei grbl::program_renderer::update_model_vbo(const grbl::program& pgm) {
     static auto movement_re = std::regex(R"(([gG]0*1?\s+|[xXyYzZ]\s*[0-9\.\-]+))");
     bool is_tool_on = false;
 
-
-    buffer_data.clear();
-
     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) {
 
@@ -231,35 +386,17 @@ GLsizei grbl::program_renderer::build_vbo(const grbl::program& pgm) {
                 auto from_color = glm::vec4(1.0f);
                 auto to_color = glm::vec4(1.0f);
                 if (is_tool_on) {
-                    from_color = glm::vec4(1, 0, 0, 1);
+                    from_color = glm::vec4(1, 0.6, 0.6, 1);
-                    to_color = glm::vec4(1, 0, 0, 1);
+                    to_color = glm::vec4(1, 0.6, 0.6, 1);
                 }
 
                 if (is_plunge) {
-                    to_color = glm::vec4(1, 0, 0, 1);
+                    to_color = glm::vec4(1, 0.6, 0.6, 1);
                 } else if (is_retract) {
-                    to_color = glm::vec4(0, 1, 0, 1);
+                    to_color = glm::vec4(0.6, 1, 0.6, 1);
                 }
 
-                // from
+                add_line(buffer_data, tool_pos, from_color, new_pos, to_color);
-                buffer_data.push_back(tool_pos.x);
-                buffer_data.push_back(tool_pos.y);
-                buffer_data.push_back(tool_pos.z);
-
-                buffer_data.push_back(from_color.r);
-                buffer_data.push_back(from_color.g);
-                buffer_data.push_back(from_color.b);
-                buffer_data.push_back(from_color.a);
-
-                // to
-                buffer_data.push_back(new_pos.x);
-                buffer_data.push_back(new_pos.y);
-                buffer_data.push_back(new_pos.z);
-
-                buffer_data.push_back(to_color.r);
-                buffer_data.push_back(to_color.g);
-                buffer_data.push_back(to_color.b);
-                buffer_data.push_back(to_color.a);
 
                 // calculate extents
                 min_pos.x = std::min(min_pos.x, new_pos.x);
@@ -275,8 +412,8 @@ GLsizei grbl::program_renderer::build_vbo(const grbl::program& pgm) {
         }
     }
 
-    const GLsizei sizeOfVertexInBytes = 28;
+    const GLsizei size_of_vertex_in_bytes = 28;
-    auto number_of_vertices = buffer_data.size() * sizeof(float) / sizeOfVertexInBytes;
+    auto number_of_vertices = buffer_data.size() * sizeof(float) / size_of_vertex_in_bytes;
 
     glBindBuffer(GL_ARRAY_BUFFER, vbo_id);
     glBufferData(GL_ARRAY_BUFFER, sizeof(float) * buffer_data.size(), buffer_data.data(), GL_STATIC_DRAW);
@@ -284,6 +421,83 @@ GLsizei grbl::program_renderer::build_vbo(const grbl::program& pgm) {
     return number_of_vertices;
 }
 
+
+void grbl::program_renderer::update_grid(float from_x, float from_y, float to_x, float to_y, int resolution) {
+
+    int x_segments = ceil((to_x - from_x) / resolution);
+    int y_segments = ceil((to_y - from_y) / resolution);
+
+    glm::vec4 color = {0.5, 0.3, 0, 1};
+
+    std::vector<glm::vec3> vertices;
+
+    float y_pos = from_y;
+    for (int y = 0; y < (y_segments + 1); y++) {
+        float x_pos = from_x;
+        for (int x = 0; x < (x_segments + 1); x++) {
+            vertices.emplace_back(x_pos, y_pos, 0);
+            x_pos += resolution;
+        }
+        y_pos += resolution;
+    }
+
+    std::vector<float> buffer_data;
+
+    for (int y = 0; y < y_segments; y++) {
+        for (int x = 0; x < x_segments; x++) {
+            int current = x + y * (x_segments + 1);
+            int next = current + 1;
+            int bottom = next + x_segments;
+            int bottom_next = bottom + 1;
+
+            auto p1 = vertices[current];
+            auto p2 = vertices[bottom];
+            auto p3 = vertices[next];
+            glm::vec3 normal = glm::normalize(glm::cross(p1 - p2, p3 - p1));
+            add_triangle(buffer_data, p1, p2, p3, normal, color);
+
+            p1 = vertices[next];
+            p2 = vertices[bottom];
+            p3 = vertices[bottom_next];
+            normal = glm::normalize(glm::cross(p1 - p2, p3 - p1));
+            add_triangle(buffer_data, p1, p2, p3, normal, color);
+        }
+    }
+
+    const GLsizei size_of_vertex_in_bytes = 40;
+    heightmap_vertices_count = buffer_data.size() * sizeof(float) / size_of_vertex_in_bytes;
+
+    glBindBuffer(GL_ARRAY_BUFFER, heightmap_vbo_id);
+    glBufferData(GL_ARRAY_BUFFER, sizeof(float) * buffer_data.size(), buffer_data.data(), GL_STATIC_DRAW);
+}
+
+void grbl::program_renderer::initialize_heightmap_buffers() {
+    glGenBuffers(1, &heightmap_vbo_id);
+    glGenVertexArrays(1, &heightmap_vao_id);
+
+    // for height map we are going to use solid rendering (triangles)
+    // we also need to send normals so that we can better see the shape of the map through lighting
+
+    // vertex format: v.x, v.y, v.z, n.x, n.y, n.z, r, g, b, a
+    // stride: 10 * 4 bytes => 40 bytes
+    const GLsizei size_of_vertex_in_bytes = 40;
+
+    glBindVertexArray(heightmap_vao_id);
+    glBindBuffer(GL_ARRAY_BUFFER, heightmap_vbo_id);
+
+    glEnableVertexAttribArray(0); // vertices on stream 0
+    glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, size_of_vertex_in_bytes, (void *) 0);
+
+    glEnableVertexAttribArray(1); // normals on stream 1
+    glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, size_of_vertex_in_bytes, (void *) (sizeof(float) * 3));
+
+    glEnableVertexAttribArray(2); // vertex colors on stream 2
+    glVertexAttribPointer(2, 4, GL_FLOAT, GL_FALSE, size_of_vertex_in_bytes, (void *) (sizeof(float) * 6));
+
+    // unbind vao
+    glBindVertexArray(0);
+}
+
 std::string get_shader_info_log(GLuint id) {
     GLint log_length = 0;
     glGetShaderiv(id, GL_INFO_LOG_LENGTH, &log_length);
@@ -301,7 +515,7 @@ std::string get_program_info_log(GLuint id) {
     char error_log[log_length]; // length includes the NULL character
     glGetProgramInfoLog(id, log_length, &log_length, &error_log[0]);
 
-    return std::string(error_log, log_length);
+    return {error_log, (size_t) log_length};
 }
 
 
@@ -313,6 +527,7 @@ bool check_compile_error(GLuint shader_id) {
 
     std::cerr << "Shader compile error: " << "(id: " << shader_id << ") - " << get_shader_info_log(shader_id) << std::endl;
     glDeleteShader(shader_id);
+    exit(1);
     return false;
 }
 
@@ -324,6 +539,7 @@ bool check_link_error(GLuint program_id) {
 
     std::cerr << "Shader program link error: " << "(id: " << program_id << ") - " << get_program_info_log(program_id) << std::endl;
     glDeleteProgram(program_id);
+    exit(1);
     return false;
 }
 

render.h

@@ -12,14 +12,15 @@ class shader_program;
 class program_renderer {
 public:
     void update(const program& pgm, const machine& cnc);
-    void render(glm::mat4 mvp, glm::vec2 viewport_size);
+    void render(glm::mat4 model, glm::mat4 view, glm::mat4 projection, glm::mat3 normal_mat, glm::vec2 viewport_size);
 
     glm::vec3 get_extents_min() const { return min_pos; };
 
     glm::vec3 get_extents_max() const { return max_pos; };
 
+    void update_grid(float from_x, float from_y, float to_x, float to_y, int resolution);
 private:
-    GLsizei build_vbo(const grbl::program& pgm);
+    GLsizei update_model_vbo(const grbl::program& pgm);
 
     GLuint spindle_vbo_id;
     GLuint spindle_vao_id;
@@ -27,16 +28,24 @@ private:
     GLuint vbo_id;
     GLuint vao_id;
 
-    std::vector<float> buffer_data;
+    GLuint extents_vbo_id;
+    GLuint extents_vao_id;
+
+    GLuint heightmap_vbo_id;
+    GLuint heightmap_vao_id;
+
     shader_program *shader = nullptr;
+    shader_program *heightmap_shader = nullptr;
 
     bool initialized = false;
 
     glm::vec3 min_pos, max_pos, spindle_pos;
-    GLsizei vertices_count, spindle_vertices_count;
+    GLsizei vertices_count, spindle_vertices_count, extents_vertices_count, heightmap_vertices_count;
 
     void initialize_program_buffers();
     void initialize_spindle_buffers();
+    void initialize_extents_buffers();
+    void initialize_heightmap_buffers();
 };