|
@@ -0,0 +1,182 @@
|
|
1
|
+/**
|
|
2
|
+ * Marlin 3D Printer Firmware
|
|
3
|
+ * Copyright (C) 2016 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
|
|
4
|
+ *
|
|
5
|
+ * Based on Sprinter and grbl.
|
|
6
|
+ * Copyright (C) 2011 Camiel Gubbels / Erik van der Zalm
|
|
7
|
+ *
|
|
8
|
+ * This program is free software: you can redistribute it and/or modify
|
|
9
|
+ * it under the terms of the GNU General Public License as published by
|
|
10
|
+ * the Free Software Foundation, either version 3 of the License, or
|
|
11
|
+ * (at your option) any later version.
|
|
12
|
+ *
|
|
13
|
+ * This program is distributed in the hope that it will be useful,
|
|
14
|
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
15
|
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
16
|
+ * GNU General Public License for more details.
|
|
17
|
+ *
|
|
18
|
+ * You should have received a copy of the GNU General Public License
|
|
19
|
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
20
|
+ *
|
|
21
|
+ */
|
|
22
|
+
|
|
23
|
+/**
|
|
24
|
+ * planner_bezier.cpp
|
|
25
|
+ *
|
|
26
|
+ * Compute and buffer movement commands for bezier curves
|
|
27
|
+ *
|
|
28
|
+ */
|
|
29
|
+
|
|
30
|
+#include "Marlin.h"
|
|
31
|
+
|
|
32
|
+#if ENABLED(BEZIER_CURVE_SUPPORT)
|
|
33
|
+
|
|
34
|
+#include "planner.h"
|
|
35
|
+#include "language.h"
|
|
36
|
+
|
|
37
|
+// See the meaning in the documentation of cubic_b_spline().
|
|
38
|
+#define MIN_STEP 0.002
|
|
39
|
+#define MAX_STEP 0.1
|
|
40
|
+#define SIGMA 0.1
|
|
41
|
+
|
|
42
|
+/* Compute the linear interpolation between to real numbers.
|
|
43
|
+*/
|
|
44
|
+inline static float interp(float a, float b, float t) { return (1.0 - t) * a + t * b; }
|
|
45
|
+
|
|
46
|
+/**
|
|
47
|
+ * Compute a Bézier curve using the De Casteljau's algorithm (see
|
|
48
|
+ * https://en.wikipedia.org/wiki/De_Casteljau%27s_algorithm), which is
|
|
49
|
+ * easy to code and has good numerical stability (very important,
|
|
50
|
+ * since Arudino works with limited precision real numbers).
|
|
51
|
+ */
|
|
52
|
+inline static float eval_bezier(float a, float b, float c, float d, float t) {
|
|
53
|
+ float iab = interp(a, b, t);
|
|
54
|
+ float ibc = interp(b, c, t);
|
|
55
|
+ float icd = interp(c, d, t);
|
|
56
|
+ float iabc = interp(iab, ibc, t);
|
|
57
|
+ float ibcd = interp(ibc, icd, t);
|
|
58
|
+ float iabcd = interp(iabc, ibcd, t);
|
|
59
|
+ return iabcd;
|
|
60
|
+}
|
|
61
|
+
|
|
62
|
+/**
|
|
63
|
+ * We approximate Euclidean distance with the sum of the coordinates
|
|
64
|
+ * offset (so-called "norm 1"), which is quicker to compute.
|
|
65
|
+ */
|
|
66
|
+inline static float dist1(float x1, float y1, float x2, float y2) { return fabs(x1 - x2) + fabs(y1 - y2); }
|
|
67
|
+
|
|
68
|
+/**
|
|
69
|
+ * The algorithm for computing the step is loosely based on the one in Kig
|
|
70
|
+ * (See https://sources.debian.net/src/kig/4:15.08.3-1/misc/kigpainter.cpp/#L759)
|
|
71
|
+ * However, we do not use the stack.
|
|
72
|
+ *
|
|
73
|
+ * The algorithm goes as it follows: the parameters t runs from 0.0 to
|
|
74
|
+ * 1.0 describing the curve, which is evaluated by eval_bezier(). At
|
|
75
|
+ * each iteration we have to choose a step, i.e., the increment of the
|
|
76
|
+ * t variable. By default the step of the previous iteration is taken,
|
|
77
|
+ * and then it is enlarged or reduced depending on how straight the
|
|
78
|
+ * curve locally is. The step is always clamped between MIN_STEP/2 and
|
|
79
|
+ * 2*MAX_STEP. MAX_STEP is taken at the first iteration.
|
|
80
|
+ *
|
|
81
|
+ * For some t, the step value is considered acceptable if the curve in
|
|
82
|
+ * the interval [t, t+step] is sufficiently straight, i.e.,
|
|
83
|
+ * sufficiently close to linear interpolation. In practice the
|
|
84
|
+ * following test is performed: the distance between eval_bezier(...,
|
|
85
|
+ * t+step/2) is evaluated and compared with 0.5*(eval_bezier(...,
|
|
86
|
+ * t)+eval_bezier(..., t+step)). If it is smaller than SIGMA, then the
|
|
87
|
+ * step value is considered acceptable, otherwise it is not. The code
|
|
88
|
+ * seeks to find the larger step value which is considered acceptable.
|
|
89
|
+ *
|
|
90
|
+ * At every iteration the recorded step value is considered and then
|
|
91
|
+ * iteratively halved until it becomes acceptable. If it was already
|
|
92
|
+ * acceptable in the beginning (i.e., no halving were done), then
|
|
93
|
+ * maybe it was necessary to enlarge it; then it is iteratively
|
|
94
|
+ * doubled while it remains acceptable. The last acceptable value
|
|
95
|
+ * found is taken, provided that it is between MIN_STEP and MAX_STEP
|
|
96
|
+ * and does not bring t over 1.0.
|
|
97
|
+ *
|
|
98
|
+ * Caveat: this algorithm is not perfect, since it can happen that a
|
|
99
|
+ * step is considered acceptable even when the curve is not linear at
|
|
100
|
+ * all in the interval [t, t+step] (but its mid point coincides "by
|
|
101
|
+ * chance" with the midpoint according to the parametrization). This
|
|
102
|
+ * kind of glitches can be eliminated with proper first derivative
|
|
103
|
+ * estimates; however, given the improbability of such configurations,
|
|
104
|
+ * the mitigation offered by MIN_STEP and the small computational
|
|
105
|
+ * power available on Arduino, I think it is not wise to implement it.
|
|
106
|
+ */
|
|
107
|
+void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS], const float offset[4], float feed_rate, uint8_t extruder) {
|
|
108
|
+ // Absolute first and second control points are recovered.
|
|
109
|
+ float first0 = position[X_AXIS] + offset[0];
|
|
110
|
+ float first1 = position[Y_AXIS] + offset[1];
|
|
111
|
+ float second0 = target[X_AXIS] + offset[2];
|
|
112
|
+ float second1 = target[Y_AXIS] + offset[3];
|
|
113
|
+ float t = 0.0;
|
|
114
|
+
|
|
115
|
+ float tmp[4];
|
|
116
|
+ tmp[X_AXIS] = position[X_AXIS];
|
|
117
|
+ tmp[Y_AXIS] = position[Y_AXIS];
|
|
118
|
+ float step = MAX_STEP;
|
|
119
|
+ while (t < 1.0) {
|
|
120
|
+ // First try to reduce the step in order to make it sufficiently
|
|
121
|
+ // close to a linear interpolation.
|
|
122
|
+ bool did_reduce = false;
|
|
123
|
+ float new_t = t + step;
|
|
124
|
+ NOMORE(new_t, 1.0);
|
|
125
|
+ float new_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], new_t);
|
|
126
|
+ float new_pos1 = eval_bezier(position[Y_AXIS], first1, second1, target[Y_AXIS], new_t);
|
|
127
|
+ for (;;) {
|
|
128
|
+ if (new_t - t < (MIN_STEP)) break;
|
|
129
|
+ float candidate_t = 0.5 * (t + new_t);
|
|
130
|
+ float candidate_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], candidate_t);
|
|
131
|
+ float candidate_pos1 = eval_bezier(position[Y_AXIS], first1, second1, target[Y_AXIS], candidate_t);
|
|
132
|
+ float interp_pos0 = 0.5 * (tmp[X_AXIS] + new_pos0);
|
|
133
|
+ float interp_pos1 = 0.5 * (tmp[Y_AXIS] + new_pos1);
|
|
134
|
+ if (dist1(candidate_pos0, candidate_pos1, interp_pos0, interp_pos1) <= (SIGMA)) break;
|
|
135
|
+ new_t = candidate_t;
|
|
136
|
+ new_pos0 = candidate_pos0;
|
|
137
|
+ new_pos1 = candidate_pos1;
|
|
138
|
+ did_reduce = true;
|
|
139
|
+ }
|
|
140
|
+
|
|
141
|
+ // If we did not reduce the step, maybe we should enlarge it.
|
|
142
|
+ if (!did_reduce) for (;;) {
|
|
143
|
+ if (new_t - t > MAX_STEP) break;
|
|
144
|
+ float candidate_t = t + 2.0 * (new_t - t);
|
|
145
|
+ if (candidate_t >= 1.0) break;
|
|
146
|
+ float candidate_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], candidate_t);
|
|
147
|
+ float candidate_pos1 = eval_bezier(position[Y_AXIS], first1, second1, target[Y_AXIS], candidate_t);
|
|
148
|
+ float interp_pos0 = 0.5 * (tmp[X_AXIS] + candidate_pos0);
|
|
149
|
+ float interp_pos1 = 0.5 * (tmp[Y_AXIS] + candidate_pos1);
|
|
150
|
+ if (dist1(new_pos0, new_pos1, interp_pos0, interp_pos1) > (SIGMA)) break;
|
|
151
|
+ new_t = candidate_t;
|
|
152
|
+ new_pos0 = candidate_pos0;
|
|
153
|
+ new_pos1 = candidate_pos1;
|
|
154
|
+ }
|
|
155
|
+
|
|
156
|
+ // Check some postcondition; they are disabled in the actual
|
|
157
|
+ // Marlin build, but if you test the same code on a computer you
|
|
158
|
+ // may want to check they are respect.
|
|
159
|
+ /*
|
|
160
|
+ assert(new_t <= 1.0);
|
|
161
|
+ if (new_t < 1.0) {
|
|
162
|
+ assert(new_t - t >= (MIN_STEP) / 2.0);
|
|
163
|
+ assert(new_t - t <= (MAX_STEP) * 2.0);
|
|
164
|
+ }
|
|
165
|
+ */
|
|
166
|
+
|
|
167
|
+ step = new_t - t;
|
|
168
|
+ t = new_t;
|
|
169
|
+
|
|
170
|
+ // Compute and send new position
|
|
171
|
+ tmp[X_AXIS] = new_pos0;
|
|
172
|
+ tmp[Y_AXIS] = new_pos1;
|
|
173
|
+ // FIXME. The following two are wrong, since the parameter t is
|
|
174
|
+ // not linear in the distance.
|
|
175
|
+ tmp[Z_AXIS] = interp(position[Z_AXIS], target[Z_AXIS], t);
|
|
176
|
+ tmp[E_AXIS] = interp(position[E_AXIS], target[E_AXIS], t);
|
|
177
|
+ clamp_to_software_endstops(tmp);
|
|
178
|
+ planner.buffer_line(tmp[X_AXIS], tmp[Y_AXIS], tmp[Z_AXIS], tmp[E_AXIS], feed_rate, extruder);
|
|
179
|
+ }
|
|
180
|
+}
|
|
181
|
+
|
|
182
|
+#endif // BEZIER_CURVE_SUPPORT
|