Merge branch 'lh_fix_arcwelder_travel'

This commit is contained in:
Lukas Matena 2024-05-03 16:19:24 +02:00
commit a697ca8f12
2 changed files with 24 additions and 53 deletions

View File

@ -31,75 +31,46 @@ std::optional<Point> sample_path_point_at_distance_from_start(const SmoothPath &
{ {
if (distance >= 0) { if (distance >= 0) {
for (const SmoothPathElement &el : path) { for (const SmoothPathElement &el : path) {
auto it = el.path.begin(); Point prev_point = el.path.front().point;
auto end = el.path.end(); for (auto segment_it = el.path.begin() + 1; segment_it != el.path.end(); ++segment_it) {
Point prev_point = it->point; Point point = segment_it->point;
for (++ it; it != end; ++ it) { if (segment_it->linear()) {
Point point = it->point;
if (it->linear()) {
// Linear segment // Linear segment
Vec2d v = (point - prev_point).cast<double>(); const Vec2d v = (point - prev_point).cast<double>();
double lsqr = v.squaredNorm(); const double lsqr = v.squaredNorm();
if (lsqr > sqr(distance)) if (lsqr > sqr(distance))
return std::make_optional<Point>(prev_point + (v * (distance / sqrt(lsqr))).cast<coord_t>()); return prev_point + (v * (distance / sqrt(lsqr))).cast<coord_t>();
distance -= sqrt(lsqr); distance -= sqrt(lsqr);
} else { } else {
// Circular segment // Circular segment
float angle = Geometry::ArcWelder::arc_angle(prev_point.cast<float>(), point.cast<float>(), it->radius); const float angle = Geometry::ArcWelder::arc_angle(prev_point.cast<float>(), point.cast<float>(), segment_it->radius);
double len = std::abs(it->radius) * angle; const double len = std::abs(segment_it->radius) * angle;
if (len > distance) { if (len > distance) {
// Rotate the segment end point in reverse towards the start point. const Point center_pt = Geometry::ArcWelder::arc_center(prev_point.cast<float>(), point.cast<float>(), segment_it->radius, segment_it->ccw()).cast<coord_t>();
return std::make_optional<Point>(prev_point.rotated(- angle * (distance / len), const float rotation_dir = (segment_it->ccw() ? 1.f : -1.f);
Geometry::ArcWelder::arc_center(prev_point.cast<float>(), point.cast<float>(), it->radius, it->ccw()).cast<coord_t>())); // Rotate the segment start point based on the arc orientation.
return prev_point.rotated(rotation_dir * angle * (distance / len), center_pt);
} }
distance -= len; distance -= len;
} }
if (distance < 0) if (distance < 0)
return std::make_optional<Point>(point); return point;
prev_point = point; prev_point = point;
} }
} }
} }
// Failed. // Failed.
return {}; return {};
} }
std::optional<Point> sample_path_point_at_distance_from_end(const SmoothPath &path, double distance) std::optional<Point> sample_path_point_at_distance_from_end(const SmoothPath &path, double distance) {
{ SmoothPath path_reversed = path;
if (distance >= 0) { reverse(path_reversed);
for (const SmoothPathElement& el : path) { return sample_path_point_at_distance_from_start(path_reversed, distance);
auto it = el.path.begin();
auto end = el.path.end();
Point prev_point = it->point;
for (++it; it != end; ++it) {
Point point = it->point;
if (it->linear()) {
// Linear segment
Vec2d v = (point - prev_point).cast<double>();
double lsqr = v.squaredNorm();
if (lsqr > sqr(distance))
return std::make_optional<Point>(prev_point + (v * (distance / sqrt(lsqr))).cast<coord_t>());
distance -= sqrt(lsqr);
}
else {
// Circular segment
float angle = Geometry::ArcWelder::arc_angle(prev_point.cast<float>(), point.cast<float>(), it->radius);
double len = std::abs(it->radius) * angle;
if (len > distance) {
// Rotate the segment end point in reverse towards the start point.
return std::make_optional<Point>(prev_point.rotated(-angle * (distance / len),
Geometry::ArcWelder::arc_center(prev_point.cast<float>(), point.cast<float>(), it->radius, it->ccw()).cast<coord_t>()));
}
distance -= len;
}
if (distance < 0)
return std::make_optional<Point>(point);
prev_point = point;
}
}
}
// Failed.
return {};
} }
// Clip length of a smooth path, for seam hiding. // Clip length of a smooth path, for seam hiding.

View File

@ -211,7 +211,7 @@ std::optional<Point> wipe_hide_seam(const SmoothPath &path, bool is_hole, double
// Wipe move cannot be calculated, the loop is not long enough. This should not happen due to the longer_than() test above. // Wipe move cannot be calculated, the loop is not long enough. This should not happen due to the longer_than() test above.
return {}; return {};
} }
if (std::optional<Point> p = sample_path_point_at_distance_from_end(path, wipe_length); p) if (std::optional<Point> p = sample_path_point_at_distance_from_start(path, wipe_length); p)
p_prev = *p; p_prev = *p;
else else
// Wipe move cannot be calculated, the loop is not long enough. This should not happen due to the longer_than() test above. // Wipe move cannot be calculated, the loop is not long enough. This should not happen due to the longer_than() test above.
@ -231,7 +231,7 @@ std::optional<Point> wipe_hide_seam(const SmoothPath &path, bool is_hole, double
} }
// Rotate the forward segment inside by 1/3 of the wedge angle. // Rotate the forward segment inside by 1/3 of the wedge angle.
auto v_rotated = Eigen::Rotation2D(angle_inside) * (p_next - p_current).cast<double>().normalized(); auto v_rotated = Eigen::Rotation2D(angle_inside) * (p_next - p_current).cast<double>().normalized();
return std::make_optional<Point>(p_current + (v_rotated * wipe_length).cast<coord_t>()); return p_current + (v_rotated * wipe_length).cast<coord_t>();
} }
return {}; return {};