|
@@ -107,10 +107,10 @@ inline static float dist1(float x1, float y1, float x2, float y2) { return FABS(
|
107
|
107
|
*/
|
108
|
108
|
void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS], const float offset[4], float fr_mm_s, uint8_t extruder) {
|
109
|
109
|
// Absolute first and second control points are recovered.
|
110
|
|
- float first0 = position[X_AXIS] + offset[0];
|
111
|
|
- float first1 = position[Y_AXIS] + offset[1];
|
112
|
|
- float second0 = target[X_AXIS] + offset[2];
|
113
|
|
- float second1 = target[Y_AXIS] + offset[3];
|
|
110
|
+ const float first0 = position[X_AXIS] + offset[0],
|
|
111
|
+ first1 = position[Y_AXIS] + offset[1],
|
|
112
|
+ second0 = target[X_AXIS] + offset[2],
|
|
113
|
+ second1 = target[Y_AXIS] + offset[3];
|
114
|
114
|
float t = 0.0;
|
115
|
115
|
|
116
|
116
|
float bez_target[4];
|
|
@@ -134,15 +134,15 @@ void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS]
|
134
|
134
|
bool did_reduce = false;
|
135
|
135
|
float new_t = t + step;
|
136
|
136
|
NOMORE(new_t, 1.0);
|
137
|
|
- float new_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], new_t);
|
138
|
|
- float new_pos1 = eval_bezier(position[Y_AXIS], first1, second1, target[Y_AXIS], new_t);
|
|
137
|
+ float new_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], new_t),
|
|
138
|
+ new_pos1 = eval_bezier(position[Y_AXIS], first1, second1, target[Y_AXIS], new_t);
|
139
|
139
|
for (;;) {
|
140
|
140
|
if (new_t - t < (MIN_STEP)) break;
|
141
|
|
- float candidate_t = 0.5 * (t + new_t);
|
142
|
|
- float candidate_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], candidate_t);
|
143
|
|
- float candidate_pos1 = eval_bezier(position[Y_AXIS], first1, second1, target[Y_AXIS], candidate_t);
|
144
|
|
- float interp_pos0 = 0.5 * (bez_target[X_AXIS] + new_pos0);
|
145
|
|
- float interp_pos1 = 0.5 * (bez_target[Y_AXIS] + new_pos1);
|
|
141
|
+ const float candidate_t = 0.5 * (t + new_t),
|
|
142
|
+ candidate_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], candidate_t),
|
|
143
|
+ candidate_pos1 = eval_bezier(position[Y_AXIS], first1, second1, target[Y_AXIS], candidate_t),
|
|
144
|
+ interp_pos0 = 0.5 * (bez_target[X_AXIS] + new_pos0),
|
|
145
|
+ interp_pos1 = 0.5 * (bez_target[Y_AXIS] + new_pos1);
|
146
|
146
|
if (dist1(candidate_pos0, candidate_pos1, interp_pos0, interp_pos1) <= (SIGMA)) break;
|
147
|
147
|
new_t = candidate_t;
|
148
|
148
|
new_pos0 = candidate_pos0;
|
|
@@ -153,12 +153,12 @@ void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS]
|
153
|
153
|
// If we did not reduce the step, maybe we should enlarge it.
|
154
|
154
|
if (!did_reduce) for (;;) {
|
155
|
155
|
if (new_t - t > MAX_STEP) break;
|
156
|
|
- float candidate_t = t + 2.0 * (new_t - t);
|
|
156
|
+ const float candidate_t = t + 2.0 * (new_t - t);
|
157
|
157
|
if (candidate_t >= 1.0) break;
|
158
|
|
- float candidate_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], candidate_t);
|
159
|
|
- float candidate_pos1 = eval_bezier(position[Y_AXIS], first1, second1, target[Y_AXIS], candidate_t);
|
160
|
|
- float interp_pos0 = 0.5 * (bez_target[X_AXIS] + candidate_pos0);
|
161
|
|
- float interp_pos1 = 0.5 * (bez_target[Y_AXIS] + candidate_pos1);
|
|
158
|
+ const float candidate_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], candidate_t),
|
|
159
|
+ candidate_pos1 = eval_bezier(position[Y_AXIS], first1, second1, target[Y_AXIS], candidate_t),
|
|
160
|
+ interp_pos0 = 0.5 * (bez_target[X_AXIS] + candidate_pos0),
|
|
161
|
+ interp_pos1 = 0.5 * (bez_target[Y_AXIS] + candidate_pos1);
|
162
|
162
|
if (dist1(new_pos0, new_pos1, interp_pos0, interp_pos1) > (SIGMA)) break;
|
163
|
163
|
new_t = candidate_t;
|
164
|
164
|
new_pos0 = candidate_pos0;
|