Add BEZIER_CURVE_SUPPORT (G5 XYZEFIJPQ)

.travis.yml

   - opt_enable ULTIMAKERCONTROLLER FILAMENT_LCD_DISPLAY
   - build_marlin
   #
+  # Enable BEZIER_CURVE_SUPPORT
+  #
+  - restore_configs
+  - opt_enable_adv BEZIER_CURVE_SUPPORT
+  - build_marlin
+  #
   # Enable COREXY
   #
   - restore_configs

Marlin/Configuration_adv.h

 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 
+// Support for G5 with XYZE destination and IJPQ offsets
+//#define BEZIER_CURVE_SUPPORT
+
 const unsigned int dropsegments = 5; //everything with less than this number of steps will be ignored as move and joined with the next movement
 
 // @section temperature

Marlin/Marlin_main.cpp

   #include "mesh_bed_leveling.h"
 #endif
 
+#if ENABLED(BEZIER_CURVE_SUPPORT)
+  #include "planner_bezier.h"
+#endif
+
 #include "ultralcd.h"
 #include "planner.h"
 #include "stepper.h"
  * G2  - CW ARC
  * G3  - CCW ARC
  * G4  - Dwell S<seconds> or P<milliseconds>
+ * G5  - Cubic B-spline with
  * G10 - retract filament according to settings of M207
  * G11 - retract recover filament according to settings of M208
  * G28 - Home one or more axes
   void plan_arc(float target[NUM_AXIS], float* offset, uint8_t clockwise);
 #endif
 
+#if ENABLED(BEZIER_CURVE_SUPPORT)
+  void plan_cubic_move(const float offset[4]);
+#endif
+
 void serial_echopair_P(const char* s_P, int v)           { serialprintPGM(s_P); SERIAL_ECHO(v); }
 void serial_echopair_P(const char* s_P, long v)          { serialprintPGM(s_P); SERIAL_ECHO(v); }
 void serial_echopair_P(const char* s_P, float v)         { serialprintPGM(s_P); SERIAL_ECHO(v); }
   while (PENDING(millis(), codenum)) idle();
 }
 
+#if ENABLED(BEZIER_CURVE_SUPPORT)
+
+  /**
+   * Parameters interpreted according to:
+   * http://linuxcnc.org/docs/2.6/html/gcode/gcode.html#sec:G5-Cubic-Spline
+   * However I, J omission is not supported at this point; all
+   * parameters can be omitted and default to zero.
+   */
+
+  /**
+   * G5: Cubic B-spline
+   */
+  inline void gcode_G5() {
+    if (IsRunning()) {
+
+      #ifdef SF_ARC_FIX
+        bool relative_mode_backup = relative_mode;
+        relative_mode = true;
+      #endif
+      gcode_get_destination();
+      #ifdef SF_ARC_FIX
+        relative_mode = relative_mode_backup;
+      #endif
+
+      float offset[] = {
+        code_seen('I') ? code_value() : 0.0,
+        code_seen('J') ? code_value() : 0.0,
+        code_seen('P') ? code_value() : 0.0,
+        code_seen('Q') ? code_value() : 0.0
+      };
+
+      plan_cubic_move(offset);
+    }
+  }
+
+#endif // BEZIER_CURVE_SUPPORT
+
 #if ENABLED(FWRETRACT)
 
   /**
 
       // G2, G3
       #if ENABLED(ARC_SUPPORT) && DISABLED(SCARA)
+
         case 2: // G2  - CW ARC
         case 3: // G3  - CCW ARC
           gcode_G2_G3(codenum == 2);
           break;
+
       #endif
 
       // G4 Dwell
         gcode_G4();
         break;
 
+      #if ENABLED(BEZIER_CURVE_SUPPORT)
+
+        // G5
+        case 5: // G5  - Cubic B_spline
+          gcode_G5();
+          break;
+
+      #endif // BEZIER_CURVE_SUPPORT
+
       #if ENABLED(FWRETRACT)
 
         case 10: // G10: retract
           gcode_G10_G11(codenum == 10);
           break;
 
-      #endif //FWRETRACT
+      #endif // FWRETRACT
 
       case 28: // G28: Home all axes, one at a time
         gcode_G28();
   }
 #endif
 
+#if ENABLED(BEZIER_CURVE_SUPPORT)
+
+  void plan_cubic_move(const float offset[4]) {
+    cubic_b_spline(current_position, destination, offset, feedrate * feedrate_multiplier / 60 / 100.0, active_extruder);
+
+    // As far as the parser is concerned, the position is now == target. In reality the
+    // motion control system might still be processing the action and the real tool position
+    // in any intermediate location.
+    set_current_to_destination();
+  }
+
+#endif // BEZIER_CURVE_SUPPORT
+
 #if HAS_CONTROLLERFAN
 
   void controllerFan() {

Marlin/example_configurations/Felix/Configuration_adv.h

 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 
+// Support for G5 with XYZE destination and IJPQ offsets
+//#define BEZIER_CURVE_SUPPORT
+
 const unsigned int dropsegments = 5; //everything with less than this number of steps will be ignored as move and joined with the next movement
 
 // @section temperature

Marlin/example_configurations/Hephestos/Configuration_adv.h

 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 
+// Support for G5 with XYZE destination and IJPQ offsets
+//#define BEZIER_CURVE_SUPPORT
+
 const unsigned int dropsegments = 5; //everything with less than this number of steps will be ignored as move and joined with the next movement
 
 // @section temperature

Marlin/example_configurations/Hephestos_2/Configuration_adv.h

 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 
+// Support for G5 with XYZE destination and IJPQ offsets
+//#define BEZIER_CURVE_SUPPORT
+
 const unsigned int dropsegments = 5; //everything with less than this number of steps will be ignored as move and joined with the next movement
 
 // @section temperature

Marlin/example_configurations/K8200/Configuration_adv.h

 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 
+// Support for G5 with XYZE destination and IJPQ offsets
+//#define BEZIER_CURVE_SUPPORT
+
 const unsigned int dropsegments = 2; //everything with less than this number of steps will be ignored as move and joined with the next movement
 
 // @section temperature

Marlin/example_configurations/RigidBot/Configuration_adv.h

 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 
+// Support for G5 with XYZE destination and IJPQ offsets
+//#define BEZIER_CURVE_SUPPORT
+
 const unsigned int dropsegments = 5; //everything with less than this number of steps will be ignored as move and joined with the next movement
 
 // @section temperature

Marlin/example_configurations/SCARA/Configuration_adv.h

 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 
+// Support for G5 with XYZE destination and IJPQ offsets
+//#define BEZIER_CURVE_SUPPORT
+
 const unsigned int dropsegments = 5; //everything with less than this number of steps will be ignored as move and joined with the next movement
 
 // @section temperature

Marlin/example_configurations/TAZ4/Configuration_adv.h

 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 
+// Support for G5 with XYZE destination and IJPQ offsets
+//#define BEZIER_CURVE_SUPPORT
+
 const unsigned int dropsegments = 5; //everything with less than this number of steps will be ignored as move and joined with the next movement
 
 // @section temperature

Marlin/example_configurations/WITBOX/Configuration_adv.h

 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 
+// Support for G5 with XYZE destination and IJPQ offsets
+//#define BEZIER_CURVE_SUPPORT
+
 const unsigned int dropsegments = 5; //everything with less than this number of steps will be ignored as move and joined with the next movement
 
 // @section temperature

Marlin/example_configurations/delta/biv2.5/Configuration_adv.h

 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 
+// Support for G5 with XYZE destination and IJPQ offsets
+//#define BEZIER_CURVE_SUPPORT
+
 const unsigned int dropsegments = 5; //everything with less than this number of steps will be ignored as move and joined with the next movement
 
 // @section temperature

Marlin/example_configurations/delta/generic/Configuration_adv.h

 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 
+// Support for G5 with XYZE destination and IJPQ offsets
+//#define BEZIER_CURVE_SUPPORT
+
 const unsigned int dropsegments = 5; //everything with less than this number of steps will be ignored as move and joined with the next movement
 
 // @section temperature

Marlin/example_configurations/delta/kossel_mini/Configuration_adv.h

 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 
+// Support for G5 with XYZE destination and IJPQ offsets
+//#define BEZIER_CURVE_SUPPORT
+
 const unsigned int dropsegments = 5; //everything with less than this number of steps will be ignored as move and joined with the next movement
 
 // @section temperature

Marlin/example_configurations/delta/kossel_pro/Configuration_adv.h

 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 
+// Support for G5 with XYZE destination and IJPQ offsets
+//#define BEZIER_CURVE_SUPPORT
+
 const unsigned int dropsegments = 5; //everything with less than this number of steps will be ignored as move and joined with the next movement
 
 // @section temperature

Marlin/example_configurations/delta/kossel_xl/Configuration_adv.h

 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 
+// Support for G5 with XYZE destination and IJPQ offsets
+//#define BEZIER_CURVE_SUPPORT
+
 const unsigned int dropsegments = 5; //everything with less than this number of steps will be ignored as move and joined with the next movement
 
 // @section temperature

Marlin/example_configurations/makibox/Configuration_adv.h

 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 
+// Support for G5 with XYZE destination and IJPQ offsets
+//#define BEZIER_CURVE_SUPPORT
+
 const unsigned int dropsegments = 5; //everything with less than this number of steps will be ignored as move and joined with the next movement
 
 // @section temperature

Marlin/example_configurations/tvrrug/Round2/Configuration_adv.h

 #define MM_PER_ARC_SEGMENT 1
 #define N_ARC_CORRECTION 25
 
+// Support for G5 with XYZE destination and IJPQ offsets
+//#define BEZIER_CURVE_SUPPORT
+
 const unsigned int dropsegments = 5; //everything with less than this number of steps will be ignored as move and joined with the next movement
 
 // @section temperature

Marlin/planner_bezier.cpp

+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * planner_bezier.cpp
+ *
+ * Compute and buffer movement commands for bezier curves
+ *
+ */
+
+#include "Marlin.h"
+
+#if ENABLED(BEZIER_CURVE_SUPPORT)
+
+#include "planner.h"
+#include "language.h"
+
+// See the meaning in the documentation of cubic_b_spline().
+#define MIN_STEP 0.002
+#define MAX_STEP 0.1
+#define SIGMA 0.1
+
+/* Compute the linear interpolation between to real numbers.
+*/
+inline static float interp(float a, float b, float t) { return (1.0 - t) * a + t * b; }
+
+/**
+ * Compute a Bézier curve using the De Casteljau's algorithm (see
+ * https://en.wikipedia.org/wiki/De_Casteljau%27s_algorithm), which is
+ * easy to code and has good numerical stability (very important,
+ * since Arudino works with limited precision real numbers).
+ */
+inline static float eval_bezier(float a, float b, float c, float d, float t) {
+  float iab = interp(a, b, t);
+  float ibc = interp(b, c, t);
+  float icd = interp(c, d, t);
+  float iabc = interp(iab, ibc, t);
+  float ibcd = interp(ibc, icd, t);
+  float iabcd = interp(iabc, ibcd, t);
+  return iabcd;
+}
+
+/**
+ * We approximate Euclidean distance with the sum of the coordinates
+ * offset (so-called "norm 1"), which is quicker to compute.
+ */
+inline static float dist1(float x1, float y1, float x2, float y2) { return fabs(x1 - x2) + fabs(y1 - y2); }
+
+/**
+ * The algorithm for computing the step is loosely based on the one in Kig
+ * (See https://sources.debian.net/src/kig/4:15.08.3-1/misc/kigpainter.cpp/#L759)
+ * However, we do not use the stack.
+ *
+ * The algorithm goes as it follows: the parameters t runs from 0.0 to
+ * 1.0 describing the curve, which is evaluated by eval_bezier(). At
+ * each iteration we have to choose a step, i.e., the increment of the
+ * t variable. By default the step of the previous iteration is taken,
+ * and then it is enlarged or reduced depending on how straight the
+ * curve locally is. The step is always clamped between MIN_STEP/2 and
+ * 2*MAX_STEP. MAX_STEP is taken at the first iteration.
+ *
+ * For some t, the step value is considered acceptable if the curve in
+ * the interval [t, t+step] is sufficiently straight, i.e.,
+ * sufficiently close to linear interpolation. In practice the
+ * following test is performed: the distance between eval_bezier(...,
+ * t+step/2) is evaluated and compared with 0.5*(eval_bezier(...,
+ * t)+eval_bezier(..., t+step)). If it is smaller than SIGMA, then the
+ * step value is considered acceptable, otherwise it is not. The code
+ * seeks to find the larger step value which is considered acceptable.
+ *
+ * At every iteration the recorded step value is considered and then
+ * iteratively halved until it becomes acceptable. If it was already
+ * acceptable in the beginning (i.e., no halving were done), then
+ * maybe it was necessary to enlarge it; then it is iteratively
+ * doubled while it remains acceptable. The last acceptable value
+ * found is taken, provided that it is between MIN_STEP and MAX_STEP
+ * and does not bring t over 1.0.
+ *
+ * Caveat: this algorithm is not perfect, since it can happen that a
+ * step is considered acceptable even when the curve is not linear at
+ * all in the interval [t, t+step] (but its mid point coincides "by
+ * chance" with the midpoint according to the parametrization). This
+ * kind of glitches can be eliminated with proper first derivative
+ * estimates; however, given the improbability of such configurations,
+ * the mitigation offered by MIN_STEP and the small computational
+ * power available on Arduino, I think it is not wise to implement it.
+ */
+void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS], const float offset[4], float feed_rate, uint8_t extruder) {
+  // Absolute first and second control points are recovered.
+  float first0 = position[X_AXIS] + offset[0];
+  float first1 = position[Y_AXIS] + offset[1];
+  float second0 = target[X_AXIS] + offset[2];
+  float second1 = target[Y_AXIS] + offset[3];
+  float t = 0.0;
+
+  float tmp[4];
+  tmp[X_AXIS] = position[X_AXIS];
+  tmp[Y_AXIS] = position[Y_AXIS];
+  float step = MAX_STEP;
+  while (t < 1.0) {
+    // First try to reduce the step in order to make it sufficiently
+    // close to a linear interpolation.
+    bool did_reduce = false;
+    float new_t = t + step;
+    NOMORE(new_t, 1.0);
+    float new_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], new_t);
+    float new_pos1 = eval_bezier(position[Y_AXIS], first1, second1, target[Y_AXIS], new_t);
+    for (;;) {
+      if (new_t - t < (MIN_STEP)) break;
+      float candidate_t = 0.5 * (t + new_t);
+      float candidate_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], candidate_t);
+      float candidate_pos1 = eval_bezier(position[Y_AXIS], first1, second1, target[Y_AXIS], candidate_t);
+      float interp_pos0 = 0.5 * (tmp[X_AXIS] + new_pos0);
+      float interp_pos1 = 0.5 * (tmp[Y_AXIS] + new_pos1);
+      if (dist1(candidate_pos0, candidate_pos1, interp_pos0, interp_pos1) <= (SIGMA)) break;
+      new_t = candidate_t;
+      new_pos0 = candidate_pos0;
+      new_pos1 = candidate_pos1;
+      did_reduce = true;
+    }
+
+    // If we did not reduce the step, maybe we should enlarge it.
+    if (!did_reduce) for (;;) {
+      if (new_t - t > MAX_STEP) break;
+      float candidate_t = t + 2.0 * (new_t - t);
+      if (candidate_t >= 1.0) break;
+      float candidate_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], candidate_t);
+      float candidate_pos1 = eval_bezier(position[Y_AXIS], first1, second1, target[Y_AXIS], candidate_t);
+      float interp_pos0 = 0.5 * (tmp[X_AXIS] + candidate_pos0);
+      float interp_pos1 = 0.5 * (tmp[Y_AXIS] + candidate_pos1);
+      if (dist1(new_pos0, new_pos1, interp_pos0, interp_pos1) > (SIGMA)) break;
+      new_t = candidate_t;
+      new_pos0 = candidate_pos0;
+      new_pos1 = candidate_pos1;
+    }
+
+    // Check some postcondition; they are disabled in the actual
+    // Marlin build, but if you test the same code on a computer you
+    // may want to check they are respect.
+    /*
+      assert(new_t <= 1.0);
+      if (new_t < 1.0) {
+        assert(new_t - t >= (MIN_STEP) / 2.0);
+        assert(new_t - t <= (MAX_STEP) * 2.0);
+      }
+    */
+
+    step = new_t - t;
+    t = new_t;
+
+    // Compute and send new position
+    tmp[X_AXIS] = new_pos0;
+    tmp[Y_AXIS] = new_pos1;
+    // FIXME. The following two are wrong, since the parameter t is
+    // not linear in the distance.
+    tmp[Z_AXIS] = interp(position[Z_AXIS], target[Z_AXIS], t);
+    tmp[E_AXIS] = interp(position[E_AXIS], target[E_AXIS], t);
+    clamp_to_software_endstops(tmp);
+    planner.buffer_line(tmp[X_AXIS], tmp[Y_AXIS], tmp[Z_AXIS], tmp[E_AXIS], feed_rate, extruder);
+  }
+}
+
+#endif // BEZIER_CURVE_SUPPORT

Marlin/planner_bezier.h

+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+/**
+ * planner_bezier.h
+ *
+ * Compute and buffer movement commands for bezier curves
+ *
+ */
+
+#ifndef PLANNER_BEZIER_H
+#define PLANNER_BEZIER_H
+
+#include "Marlin.h"
+
+void cubic_b_spline(
+              const float position[NUM_AXIS], // current position
+              const float target[NUM_AXIS],   // target position
+              const float offset[4],          // a pair of offsets
+              float feed_rate,
+              uint8_t extruder
+            );
+
+#endif // PLANNER_BEZIER_H