From c961dd084d5971b26161553c426230becd6c15f5 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Wed, 5 Apr 2017 02:55:25 -0500
Subject: [PATCH] Add hidden option to output Bilinear grids in JSON

---
 .gitattributes                          |   1 +
 Marlin/Marlin_main.cpp                  | 136 ++++++++-----
 buildroot/share/scripts/MarlinMesh.scad | 256 ++++++++++++++++++++++++
 3 files changed, 343 insertions(+), 50 deletions(-)
 create mode 100644 buildroot/share/scripts/MarlinMesh.scad

diff --git a/.gitattributes b/.gitattributes
index bc2df2da35..d511b4ea94 100644
--- a/.gitattributes
+++ b/.gitattributes
@@ -8,6 +8,7 @@
 *.ino text eol=lf
 *.py  text eol=lf
 *.sh  text eol=lf
+*.scad text eol=lf
 
 # Files with native line endings
 # *.sln text
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index 63c857b72b..817c4f9341 100755
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -399,11 +399,11 @@ int feedrate_percentage = 100, saved_feedrate_percentage,
 
 bool axis_relative_modes[] = AXIS_RELATIVE_MODES,
      volumetric_enabled =
-      #if ENABLED(VOLUMETRIC_DEFAULT_ON)
-        true
-      #else
-        false
-      #endif
+        #if ENABLED(VOLUMETRIC_DEFAULT_ON)
+          true
+        #else
+          false
+        #endif
       ;
 float filament_size[EXTRUDERS] = ARRAY_BY_EXTRUDERS1(DEFAULT_NOMINAL_FILAMENT_DIA),
       volumetric_multiplier[EXTRUDERS] = ARRAY_BY_EXTRUDERS1(1.0);
@@ -588,7 +588,6 @@ static uint8_t target_extruder;
 #endif
 
 #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
-  #define UNPROBED 9999.0f
   int bilinear_grid_spacing[2], bilinear_start[2];
   float bed_level_grid[ABL_GRID_MAX_POINTS_X][ABL_GRID_MAX_POINTS_Y];
 #endif
@@ -2344,7 +2343,7 @@ static void clean_up_after_endstop_or_probe_move() {
         bilinear_grid_spacing[X_AXIS] = bilinear_grid_spacing[Y_AXIS] = 0;
         for (uint8_t x = 0; x < ABL_GRID_MAX_POINTS_X; x++)
           for (uint8_t y = 0; y < ABL_GRID_MAX_POINTS_Y; y++)
-            bed_level_grid[x][y] = UNPROBED;
+            bed_level_grid[x][y] = NAN;
       #elif ENABLED(AUTO_BED_LEVELING_UBL)
         ubl.reset();
       #endif
@@ -2353,6 +2352,76 @@ static void clean_up_after_endstop_or_probe_move() {
 
 #endif // PLANNER_LEVELING
 
+#if ENABLED(AUTO_BED_LEVELING_BILINEAR) || ENABLED(MESH_BED_LEVELING)
+
+  //
+  // Enable if you prefer your output in JSON format
+  // suitable for SCAD or JavaScript mesh visualizers.
+  // 
+  // Visualize meshes in OpenSCAD using the included script.
+  // 
+  //   buildroot/shared/scripts/MarlinMesh.scad
+  //
+  //#define SCAD_MESH_OUTPUT
+
+  /**
+   * Print calibration results for plotting or manual frame adjustment.
+   */
+  static void print_2d_array(const uint8_t sx, const uint8_t sy, const uint8_t precision, float (*fn)(const uint8_t, const uint8_t)) {
+    #ifndef SCAD_MESH_OUTPUT
+      for (uint8_t x = 0; x < sx; x++) {
+        for (uint8_t i = 0; i < precision + 2 + (x < 10 ? 1 : 0); i++)
+          SERIAL_PROTOCOLCHAR(' ');
+        SERIAL_PROTOCOL((int)x);
+      }
+      SERIAL_EOL;
+    #endif
+    #ifdef SCAD_MESH_OUTPUT
+      SERIAL_PROTOCOLLNPGM("measured_z = ["); // open 2D array
+    #endif
+    for (uint8_t y = 0; y < sy; y++) {
+      #ifdef SCAD_MESH_OUTPUT
+        SERIAL_PROTOCOLLNPGM(" [");           // open sub-array
+      #else
+        if (y < 10) SERIAL_PROTOCOLCHAR(' ');
+        SERIAL_PROTOCOL((int)y);
+      #endif
+      for (uint8_t x = 0; x < sx; x++) {
+        SERIAL_PROTOCOLCHAR(' ');
+        const float offset = fn(x, y);
+        if (offset != NAN) {
+          if (offset >= 0) SERIAL_PROTOCOLCHAR('+');
+          SERIAL_PROTOCOL_F(offset, precision);
+        }
+        else {
+          #ifdef SCAD_MESH_OUTPUT
+            for (uint8_t i = 3; i < precision + 3; i++)
+              SERIAL_PROTOCOLCHAR(' ');
+            SERIAL_PROTOCOLPGM("NAN");
+          #else
+            for (uint8_t i = 0; i < precision + 3; i++)
+              SERIAL_PROTOCOLCHAR(i ? '=' : ' ');
+          #endif
+        }
+        #ifdef SCAD_MESH_OUTPUT
+          if (x < sx - 1) SERIAL_PROTOCOLCHAR(',');
+        #endif
+      }
+      #ifdef SCAD_MESH_OUTPUT
+        SERIAL_PROTOCOLCHAR(' ');
+        SERIAL_PROTOCOLCHAR(']');                     // close sub-array
+        if (y < sy - 1) SERIAL_PROTOCOLCHAR(',');
+      #endif
+      SERIAL_EOL;
+    }
+    #ifdef SCAD_MESH_OUTPUT
+      SERIAL_PROTOCOLPGM("\n];");                     // close 2D array
+    #endif
+    SERIAL_EOL;
+  }
+
+#endif
+
 #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
 
   /**
@@ -2372,7 +2441,7 @@ static void clean_up_after_endstop_or_probe_move() {
         SERIAL_CHAR(']');
       }
     #endif
-    if (bed_level_grid[x][y] != UNPROBED) {
+    if (bed_level_grid[x][y] != NAN) {
       #if ENABLED(DEBUG_LEVELING_FEATURE)
         if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM(" (done)");
       #endif
@@ -2386,9 +2455,9 @@ static void clean_up_after_endstop_or_probe_move() {
           c1 = bed_level_grid[x + xdir][y + ydir], c2 = bed_level_grid[x + xdir * 2][y + ydir * 2];
 
     // Treat far unprobed points as zero, near as equal to far
-    if (a2 == UNPROBED) a2 = 0.0; if (a1 == UNPROBED) a1 = a2;
-    if (b2 == UNPROBED) b2 = 0.0; if (b1 == UNPROBED) b1 = b2;
-    if (c2 == UNPROBED) c2 = 0.0; if (c1 == UNPROBED) c1 = c2;
+    if (a2 == NAN) a2 = 0.0; if (a1 == NAN) a1 = a2;
+    if (b2 == NAN) b2 = 0.0; if (b1 == NAN) b1 = b2;
+    if (c2 == NAN) c2 = 0.0; if (c1 == NAN) c1 = c2;
 
     const float a = 2 * a1 - a2, b = 2 * b1 - b2, c = 2 * c1 - c2;
 
@@ -2453,39 +2522,10 @@ static void clean_up_after_endstop_or_probe_move() {
 
   }
 
-  /**
-   * Print calibration results for plotting or manual frame adjustment.
-   */
-  static void print_2d_array(const uint8_t sx, const uint8_t sy, const uint8_t precision, float (*fn)(const uint8_t, const uint8_t)) {
-    for (uint8_t x = 0; x < sx; x++) {
-      for (uint8_t i = 0; i < precision + 2 + (x < 10 ? 1 : 0); i++)
-        SERIAL_PROTOCOLCHAR(' ');
-      SERIAL_PROTOCOL((int)x);
-    }
-    SERIAL_EOL;
-    for (uint8_t y = 0; y < sy; y++) {
-      if (y < 10) SERIAL_PROTOCOLCHAR(' ');
-      SERIAL_PROTOCOL((int)y);
-      for (uint8_t x = 0; x < sx; x++) {
-        SERIAL_PROTOCOLCHAR(' ');
-        float offset = fn(x, y);
-        if (offset != UNPROBED) {
-          if (offset >= 0) SERIAL_PROTOCOLCHAR('+');
-          SERIAL_PROTOCOL_F(offset, precision);
-        }
-        else
-          for (uint8_t i = 0; i < precision + 3; i++)
-            SERIAL_PROTOCOLCHAR(i ? '=' : ' ');
-      }
-      SERIAL_EOL;
-    }
-    SERIAL_EOL;
-  }
-
   static void print_bilinear_leveling_grid() {
     SERIAL_ECHOLNPGM("Bilinear Leveling Grid:");
-    print_2d_array(ABL_GRID_MAX_POINTS_X, ABL_GRID_MAX_POINTS_Y, 2,
-      [](const uint8_t x, const uint8_t y) { return bed_level_grid[x][y]; }
+    print_2d_array(ABL_GRID_MAX_POINTS_X, ABL_GRID_MAX_POINTS_Y, 3,
+      [](const uint8_t ix, const uint8_t iy) { return bed_level_grid[ix][iy]; }
     );
   }
 
@@ -2501,7 +2541,7 @@ static void clean_up_after_endstop_or_probe_move() {
     static void bed_level_virt_print() {
       SERIAL_ECHOLNPGM("Subdivided with CATMULL ROM Leveling Grid:");
       print_2d_array(ABL_GRID_POINTS_VIRT_X, ABL_GRID_POINTS_VIRT_Y, 5,
-        [](const uint8_t x, const uint8_t y) { return bed_level_grid_virt[x][y]; }
+        [](const uint8_t ix, const uint8_t iy) { return bed_level_grid_virt[ix][iy]; }
       );
     }
 
@@ -3715,13 +3755,9 @@ inline void gcode_G28() {
     SERIAL_PROTOCOLLNPGM("Num X,Y: " STRINGIFY(MESH_NUM_X_POINTS) "," STRINGIFY(MESH_NUM_Y_POINTS));
     SERIAL_PROTOCOLPGM("Z offset: "); SERIAL_PROTOCOL_F(mbl.z_offset, 5);
     SERIAL_PROTOCOLLNPGM("\nMeasured points:");
-    for (uint8_t py = 0; py < MESH_NUM_Y_POINTS; py++) {
-      for (uint8_t px = 0; px < MESH_NUM_X_POINTS; px++) {
-        SERIAL_PROTOCOLPGM("  ");
-        SERIAL_PROTOCOL_F(mbl.z_values[py][px], 5);
-      }
-      SERIAL_EOL;
-    }
+    print_2d_array(MESH_NUM_X_POINTS, MESH_NUM_Y_POINTS, 5,
+      [](const uint8_t ix, const uint8_t iy) { return mbl.z_values[ix][iy]; }
+    );
   }
 
   /**
diff --git a/buildroot/share/scripts/MarlinMesh.scad b/buildroot/share/scripts/MarlinMesh.scad
new file mode 100644
index 0000000000..68e0eee66e
--- /dev/null
+++ b/buildroot/share/scripts/MarlinMesh.scad
@@ -0,0 +1,256 @@
+ /**************************************\
+ *                                      *
+ *   OpenSCAD Mesh Display              *
+ *   by Thinkyhead - April 2017         *
+ *                                      *
+ *   Copy the grid output from Marlin,  *
+ *   paste below as shown, and use      *
+ *   OpenSCAD to see a visualization    *
+ *   of your mesh.                      *
+ *                                      *
+ \**************************************/
+
+//$t = 0.15; // comment out during animation
+
+//
+// Mesh info and points
+//
+
+mesh_width    = 200;   // X Size in mm of the probed area
+mesh_height   = 200;   // Y Size...
+zprobe_offset = 0;     // Added to the points
+NAN           = 0;     // Z to use for un-measured points
+
+measured_z = [
+  [ -1.20, -1.13, -1.09, -1.03, -1.19 ],
+  [ -1.16, -1.25, -1.27, -1.25, -1.08 ],
+  [ -1.13, -1.26, -1.39, -1.31, -1.18 ],
+  [ -1.09, -1.20, -1.26, -1.21, -1.18 ],
+  [ -1.13, -0.99, -1.03, -1.06, -1.32 ]
+];
+
+//
+// Geometry
+//
+
+max_z_scale   = 100;   // Scale at Time 0.5
+min_z_scale   = 10;    // Scale at Time 0.0 and 1.0
+thickness     = 0.5;   // thickness of the mesh triangles
+tesselation   = 1;     // levels of tesselation from 0-2
+alternation   = 2;     // direction change modulus (try it)
+
+//
+// Appearance
+//
+
+show_plane    = true;
+show_labels   = true;
+arrow_length  = 5;
+
+label_font_lg = "Arial";
+label_font_sm = "Arial";
+mesh_color    = [1,1,1,0.5];
+plane_color   = [0.4,0.6,0.9,0.6];
+
+//================================================ Derive useful values
+
+big_z = max_2D(measured_z,0);
+lil_z = min_2D(measured_z,0);
+
+mean_value = (big_z + lil_z) / 2.0;
+
+mesh_points_y = len(measured_z);
+mesh_points_x = len(measured_z[0]);
+
+xspace = mesh_width / (mesh_points_x - 1);
+yspace = mesh_height / (mesh_points_y - 1);
+
+// At $t=0 and $t=1 scale will be 100%
+z_scale_factor = min_z_scale + (($t > 0.5) ? 1.0 - $t : $t) * (max_z_scale - min_z_scale) * 2;
+
+//
+// Min and max recursive functions for 1D and 2D arrays
+// Return the smallest or largest value in the array
+//
+function min_1D(b,i) = (i<len(b)-1) ? min(b[i], min_1D(b,i+1)) : b[i];
+function min_2D(a,j) = (j<len(a)-1) ? min_2D(a,j+1) : min_1D(a[j], 0);
+function max_1D(b,i) = (i<len(b)-1) ? max(b[i], max_1D(b,i+1)) : b[i];
+function max_2D(a,j) = (j<len(a)-1) ? max_2D(a,j+1) : max_1D(a[j], 0);
+
+//
+// Get the corner probe points of a grid square.
+//
+// Input  : x,y grid indexes
+// Output : An array of the 4 corner points
+//
+function grid_square(x,y) = [
+  [x * xspace, y * yspace, z_scale_factor * (measured_z[y][x] - mean_value)],
+  [x * xspace, (y+1) * yspace, z_scale_factor * (measured_z[y+1][x] - mean_value)],
+  [(x+1) * xspace, (y+1) * yspace, z_scale_factor * (measured_z[y+1][x+1] - mean_value)],
+  [(x+1) * xspace, y * yspace, z_scale_factor * (measured_z[y][x+1] - mean_value)]
+];
+
+// The corner point of a grid square with Z centered on the mean
+function pos(x,y,z) = [x * xspace, y * yspace, z_scale_factor * (z - mean_value)];
+
+//
+// Draw the point markers and labels
+//
+module point_markers(show_home=true) {
+  // Mark the home position 0,0
+  color([0,0,0,0.25]) translate([1,1]) cylinder(r=1, h=z_scale_factor, center=true);
+
+  for (x=[0:mesh_points_x-1], y=[0:mesh_points_y-1]) {
+    z = measured_z[y][x];
+    down = z < mean_value;
+    translate(pos(x, y, z)) {
+
+      // Label each point with the Z
+      if (show_labels) {
+        v = z - mean_value;
+
+        color(abs(v) < 0.1 ? [0,0.5,0] : [0.25,0,0])
+        translate([0,0,down?-10:10]) {
+
+          $fn=8;
+          rotate([90,0])
+            text(str(z), 6, label_font_lg, halign="center", valign="center");
+      
+          translate([0,0,down?-6:6]) rotate([90,0])
+            text(str(down ? "" : "+", v), 3, label_font_sm, halign="center", valign="center");
+        }
+      }
+
+      // Show an arrow pointing up or down
+      rotate([0, down ? 180 : 0]) translate([0,0,-1])
+        cylinder(
+          r1=0.5,
+          r2=0.1,
+          h=arrow_length, $fn=12, center=1
+        );
+    }
+  }
+}
+
+//
+// Split a square on the diagonal into
+// two triangles and render them.
+//
+//     s : a square
+//   alt : a flag to split on the other diagonal
+//
+module tesselated_square(s, alt=false) {
+  add = [0,0,thickness];
+  p1 = [
+    s[0], s[1], s[2], s[3],
+    s[0]+add, s[1]+add, s[2]+add, s[3]+add
+  ];
+  f1 = alt
+      ? [ [0,1,3], [4,5,1,0], [4,7,5], [5,7,3,1], [7,4,0,3] ]
+      : [ [0,1,2], [4,5,1,0], [4,6,5], [5,6,2,1], [6,4,0,2] ];
+  f2 = alt
+      ? [ [1,2,3], [5,6,2,1], [5,6,7], [6,7,3,2], [7,5,1,3] ]
+      : [ [0,2,3], [4,6,2,0], [4,7,6], [6,7,3,2], [7,4,0,3] ];
+
+  // Use the other diagonal
+  polyhedron(points=p1, faces=f1);
+  polyhedron(points=p1, faces=f2);
+}
+
+/**
+ * The simplest mesh display
+ */
+module simple_mesh(show_plane=show_plane) {
+  if (show_plane) color(plane_color) cube([mesh_width, mesh_height, thickness]);
+  color(mesh_color)
+    for (x=[0:mesh_points_x-2], y=[0:mesh_points_y-2])
+      tesselated_square(grid_square(x, y));
+}
+
+/**
+ * Subdivide the mesh into smaller squares.
+ */
+module bilinear_mesh(show_plane=show_plane,tesselation=tesselation) {
+  if (show_plane) color(plane_color) translate([-5,-5]) cube([mesh_width+10, mesh_height+10, thickness]);
+  tesselation = tesselation % 4;
+  color(mesh_color)
+  for (x=[0:mesh_points_x-2], y=[0:mesh_points_y-2]) {
+    square = grid_square(x, y);
+    if (tesselation < 1) {
+      tesselated_square(square,(x%alternation)-(y%alternation));
+    }
+    else {
+      subdiv_4 = subdivided_square(square);
+      if (tesselation < 2) {
+        for (i=[0:3]) tesselated_square(subdiv_4[i],i%alternation);
+      }
+      else {
+        for (i=[0:3]) {
+          subdiv_16 = subdivided_square(subdiv_4[i]);
+          if (tesselation < 3) {
+            for (j=[0:3]) tesselated_square(subdiv_16[j],j%alternation);
+          }
+          else {
+            for (j=[0:3]) {
+              subdiv_64 = subdivided_square(subdiv_16[j]);
+              if (tesselation < 4) {
+                for (k=[0:3]) tesselated_square(subdiv_64[k]);
+              }
+            }
+          }
+        }
+      }
+    }
+
+  }
+}
+
+//
+// Subdivision helpers
+//
+function ctrz(a) = (a[0][2]+a[1][2]+a[3][2]+a[2][2])/4;
+function avgx(a,i) = (a[i][0]+a[(i+1)%4][0])/2;
+function avgy(a,i) = (a[i][1]+a[(i+1)%4][1])/2;
+function avgz(a,i) = (a[i][2]+a[(i+1)%4][2])/2;
+
+//
+// Convert one square into 4, applying bilinear averaging
+//
+// Input  : 1 square (4 points)
+// Output : An array of 4 squares
+//
+function subdivided_square(a) = [
+  [ // SW square
+    a[0],                          // SW
+    [a[0][0],avgy(a,0),avgz(a,0)], // CW
+    [avgx(a,1),avgy(a,0),ctrz(a)], // CC
+    [avgx(a,1),a[0][1],avgz(a,3)]  // SC
+  ],
+  [ // NW square
+    [a[0][0],avgy(a,0),avgz(a,0)], // CW
+    a[1],                          // NW
+    [avgx(a,1),a[1][1],avgz(a,1)], // NC
+    [avgx(a,1),avgy(a,0),ctrz(a)]  // CC
+  ],
+  [ // NE square
+    [avgx(a,1),avgy(a,0),ctrz(a)], // CC
+    [avgx(a,1),a[1][1],avgz(a,1)], // NC
+    a[2],                          // NE
+    [a[2][0],avgy(a,0),avgz(a,2)]  // CE
+  ],
+  [ // SE square
+    [avgx(a,1),a[0][1],avgz(a,3)], // SC
+    [avgx(a,1),avgy(a,0),ctrz(a)], // CC
+    [a[2][0],avgy(a,0),avgz(a,2)], // CE
+    a[3]                           // SE
+  ]
+];
+
+
+//================================================ Run the plan
+
+translate([-mesh_width / 2, -mesh_height / 2]) {
+  $fn = 12;
+  point_markers();
+  bilinear_mesh();
+}