diff options
Diffstat (limited to 'src/libslic3r/VoronoiOffset.cpp')
| -rw-r--r-- | src/libslic3r/VoronoiOffset.cpp | 855 |
1 files changed, 855 insertions, 0 deletions
diff --git a/src/libslic3r/VoronoiOffset.cpp b/src/libslic3r/VoronoiOffset.cpp new file mode 100644 index 000000000..c0541bd9f --- /dev/null +++ b/src/libslic3r/VoronoiOffset.cpp @@ -0,0 +1,855 @@ +// Polygon offsetting using Voronoi diagram prodiced by boost::polygon. + +#include "VoronoiOffset.hpp" + +#include <cmath> + +// #define VORONOI_DEBUG_OUT + +#ifdef VORONOI_DEBUG_OUT +#include <libslic3r/VoronoiVisualUtils.hpp> +#endif + +namespace Slic3r { + +using VD = Geometry::VoronoiDiagram; + +namespace detail { + // Intersect a circle with a ray, return the two parameters. + // Currently used for unbounded Voronoi edges only. + double first_circle_segment_intersection_parameter( + const Vec2d ¢er, const double r, const Vec2d &pt, const Vec2d &v) + { + const Vec2d d = pt - center; +#ifndef NDEBUG + double d0 = (pt - center).norm(); + double d1 = (pt + v - center).norm(); + assert(r < std::max(d0, d1) + EPSILON); +#endif /* NDEBUG */ + const double a = v.squaredNorm(); + const double b = 2. * d.dot(v); + const double c = d.squaredNorm() - r * r; + std::pair<int, std::array<double, 2>> out; + double u = b * b - 4. * a * c; + assert(u > - EPSILON); + double t; + if (u <= 0) { + // Degenerate to a single closest point. + t = - b / (2. * a); + assert(t >= - EPSILON && t <= 1. + EPSILON); + return Slic3r::clamp(0., 1., t); + } else { + u = sqrt(u); + out.first = 2; + double t0 = (- b - u) / (2. * a); + double t1 = (- b + u) / (2. * a); + // One of the intersections shall be found inside the segment. + assert((t0 >= - EPSILON && t0 <= 1. + EPSILON) || (t1 >= - EPSILON && t1 <= 1. + EPSILON)); + if (t1 < 0.) + return 0.; + if (t0 > 1.) + return 1.; + return (t0 > 0.) ? t0 : t1; + } + } + + struct Intersections + { + int count; + Vec2d pts[2]; + }; + + // Return maximum two points, that are at distance "d" from both points + Intersections point_point_equal_distance_points(const Point &pt1, const Point &pt2, const double d) + { + // Calculate the two intersection points. + // With the help of Python package sympy: + // res = solve([(x - cx)**2 + (y - cy)**2 - d**2, x**2 + y**2 - d**2], [x, y]) + // ccode(cse((res[0][0], res[0][1], res[1][0], res[1][1]))) + // where cx, cy is the center of pt1 relative to pt2, + // d is distance from the line and the point (0, 0). + // The result is then shifted to pt2. + auto cx = double(pt1.x() - pt2.x()); + auto cy = double(pt1.y() - pt2.y()); + double cl = cx * cx + cy * cy; + double discr = 4. * d * d - cl; + if (discr < 0.) { + // No intersection point found, the two circles are too far away. + return Intersections { 0, { Vec2d(), Vec2d() } }; + } + // Avoid division by zero if a gets too small. + bool xy_swapped = std::abs(cx) < std::abs(cy); + if (xy_swapped) + std::swap(cx, cy); + double u; + int cnt; + if (discr == 0.) { + cnt = 1; + u = 0; + } else { + cnt = 2; + u = 0.5 * cx * sqrt(cl * discr) / cl; + } + double v = 0.5 * cy - u; + double w = 2. * cy; + double e = 0.5 / cx; + double f = 0.5 * cy + u; + Intersections out { cnt, { Vec2d(-e * (v * w - cl), v), + Vec2d(-e * (w * f - cl), f) } }; + if (xy_swapped) { + std::swap(out.pts[0].x(), out.pts[0].y()); + std::swap(out.pts[1].x(), out.pts[1].y()); + } + out.pts[0] += pt2.cast<double>(); + out.pts[1] += pt2.cast<double>(); + + assert(std::abs((out.pts[0] - pt1.cast<double>()).norm() - d) < SCALED_EPSILON); + assert(std::abs((out.pts[1] - pt1.cast<double>()).norm() - d) < SCALED_EPSILON); + assert(std::abs((out.pts[0] - pt2.cast<double>()).norm() - d) < SCALED_EPSILON); + assert(std::abs((out.pts[1] - pt2.cast<double>()).norm() - d) < SCALED_EPSILON); + return out; + } + + // Return maximum two points, that are at distance "d" from both the line and point. + Intersections line_point_equal_distance_points(const Line &line, const Point &ipt, const double d) + { + assert(line.a != ipt && line.b != ipt); + // Calculating two points of distance "d" to a ray and a point. + // Point. + Vec2d pt = ipt.cast<double>(); + Vec2d lv = (line.b - line.a).cast<double>(); + double l2 = lv.squaredNorm(); + Vec2d lpv = (line.a - ipt).cast<double>(); + double c = cross2(lpv, lv); + if (c < 0) { + lv = - lv; + c = - c; + } + + // Line equation (ax + by + c - d * sqrt(l2)). + auto a = - lv.y(); + auto b = lv.x(); + // Line point shifted by -ipt is on the line. + assert(std::abs(lpv.x() * a + lpv.y() * b + c) < SCALED_EPSILON); + // Line vector (a, b) points towards ipt. + assert(a * lpv.x() + b * lpv.y() < - SCALED_EPSILON); + +#ifndef NDEBUG + { + // Foot point of ipt on line. + Vec2d ft = Geometry::foot_pt(line, ipt); + // Center point between ipt and line, its distance to both line and ipt is equal. + Vec2d centerpt = 0.5 * (ft + pt) - pt; + double dcenter = 0.5 * (ft - pt).norm(); + // Verify that the center point + assert(std::abs(centerpt.x() * a + centerpt.y() * b + c - dcenter * sqrt(l2)) < SCALED_EPSILON * sqrt(l2)); + } +#endif // NDEBUG + + // Calculate the two intersection points. + // With the help of Python package sympy: + // res = solve([a * x + b * y + c - d * sqrt(a**2 + b**2), x**2 + y**2 - d**2], [x, y]) + // ccode(cse((res[0][0], res[0][1], res[1][0], res[1][1]))) + // where (a, b, c, d) is the line equation, not normalized (vector a,b is not normalized), + // d is distance from the line and the point (0, 0). + // The result is then shifted to ipt. + + double dscaled = d * sqrt(l2); + double s = c * (2. * dscaled - c); + if (s < 0.) + // Distance of pt from line is bigger than 2 * d. + return Intersections { 0 }; + double u; + int cnt; + // Avoid division by zero if a gets too small. + bool xy_swapped = std::abs(a) < std::abs(b); + if (xy_swapped) + std::swap(a, b); + if (s == 0.) { + // Distance of pt from line is 2 * d. + cnt = 1; + u = 0.; + } else { + // Distance of pt from line is smaller than 2 * d. + cnt = 2; + u = a * sqrt(s) / l2; + } + double e = dscaled - c; + double f = b * e / l2; + double g = f - u; + double h = f + u; + Intersections out { cnt, { Vec2d((- b * g + e) / a, g), + Vec2d((- b * h + e) / a, h) } }; + if (xy_swapped) { + std::swap(out.pts[0].x(), out.pts[0].y()); + std::swap(out.pts[1].x(), out.pts[1].y()); + } + out.pts[0] += pt; + out.pts[1] += pt; + + assert(std::abs(Geometry::ray_point_distance<Vec2d>(line.a.cast<double>(), (line.b - line.a).cast<double>(), out.pts[0]) - d) < SCALED_EPSILON); + assert(std::abs(Geometry::ray_point_distance<Vec2d>(line.a.cast<double>(), (line.b - line.a).cast<double>(), out.pts[1]) - d) < SCALED_EPSILON); + assert(std::abs((out.pts[0] - ipt.cast<double>()).norm() - d) < SCALED_EPSILON); + assert(std::abs((out.pts[1] - ipt.cast<double>()).norm() - d) < SCALED_EPSILON); + return out; + } + +} // namespace detail + +Polygons voronoi_offset( + const Geometry::VoronoiDiagram &vd, + const Lines &lines, + double offset_distance, + double discretization_error) +{ +#ifndef NDEBUG + // Verify that twin halfedges are stored next to the other in vd. + for (size_t i = 0; i < vd.num_edges(); i += 2) { + const VD::edge_type &e = vd.edges()[i]; + const VD::edge_type &e2 = vd.edges()[i + 1]; + assert(e.twin() == &e2); + assert(e2.twin() == &e); + assert(e.is_secondary() == e2.is_secondary()); + if (e.is_secondary()) { + assert(e.cell()->contains_point() != e2.cell()->contains_point()); + const VD::edge_type &ex = (e.cell()->contains_point() ? e : e2); + // Verify that the Point defining the cell left of ex is an end point of a segment + // defining the cell right of ex. + const Line &line0 = lines[ex.cell()->source_index()]; + const Line &line1 = lines[ex.twin()->cell()->source_index()]; + const Point &pt = (ex.cell()->source_category() == boost::polygon::SOURCE_CATEGORY_SEGMENT_START_POINT) ? line0.a : line0.b; + assert(pt == line1.a || pt == line1.b); + } + } +#endif // NDEBUG + + enum class EdgeState : unsigned char { + // Initial state, don't know. + Unknown, + // This edge will certainly not be intersected by the offset curve. + Inactive, + // This edge will certainly be intersected by the offset curve. + Active, + // This edge will possibly be intersected by the offset curve. + Possible + }; + + enum class CellState : unsigned char { + // Initial state, don't know. + Unknown, + // Inactive cell is inside for outside curves and outside for inside curves. + Inactive, + // Active cell is outside for outside curves and inside for inside curves. + Active, + // Boundary cell is intersected by the input segment, part of it is active. + Boundary + }; + + // Mark edges with outward vertex pointing outside the polygons, thus there is a chance + // that such an edge will have an intersection with our desired offset curve. + bool outside = offset_distance > 0.; + std::vector<EdgeState> edge_state(vd.num_edges(), EdgeState::Unknown); + std::vector<CellState> cell_state(vd.num_cells(), CellState::Unknown); + const VD::edge_type *front_edge = &vd.edges().front(); + const VD::cell_type *front_cell = &vd.cells().front(); + auto set_edge_state_initial = [&edge_state, front_edge](const VD::edge_type *edge, EdgeState new_edge_type) { + EdgeState &edge_type = edge_state[edge - front_edge]; + assert(edge_type == EdgeState::Unknown || edge_type == new_edge_type); + assert(new_edge_type == EdgeState::Possible || new_edge_type == EdgeState::Inactive); + edge_type = new_edge_type; + }; + auto set_edge_state_final = [&edge_state, front_edge](const size_t edge_id, EdgeState new_edge_type) { + EdgeState &edge_type = edge_state[edge_id]; + assert(edge_type == EdgeState::Possible || edge_type == new_edge_type); + assert(new_edge_type == EdgeState::Active || new_edge_type == EdgeState::Inactive); + edge_type = new_edge_type; + }; + auto set_cell_state = [&cell_state, front_cell](const VD::cell_type *cell, CellState new_cell_type) -> bool { + CellState &cell_type = cell_state[cell - front_cell]; + assert(cell_type == CellState::Active || cell_type == CellState::Inactive || cell_type == CellState::Boundary || cell_type == CellState::Unknown); + assert(new_cell_type == CellState::Active || new_cell_type == CellState::Inactive || new_cell_type == CellState::Boundary); + switch (cell_type) { + case CellState::Unknown: + break; + case CellState::Active: + if (new_cell_type == CellState::Inactive) + new_cell_type = CellState::Boundary; + break; + case CellState::Inactive: + if (new_cell_type == CellState::Active) + new_cell_type = CellState::Boundary; + break; + case CellState::Boundary: + return false; + } + if (cell_type != new_cell_type) { + cell_type = new_cell_type; + return true; + } + return false; + }; + + for (const VD::edge_type &edge : vd.edges()) + if (edge.vertex1() == nullptr) { + // Infinite Voronoi edge separating two Point sites or a Point site and a Segment site. + // Infinite edge is always outside and it has at least one valid vertex. + assert(edge.vertex0() != nullptr); + set_edge_state_initial(&edge, outside ? EdgeState::Possible : EdgeState::Inactive); + // Opposite edge of an infinite edge is certainly not active. + set_edge_state_initial(edge.twin(), EdgeState::Inactive); + if (edge.is_secondary()) { + // edge.vertex0() must lie on source contour. + const VD::cell_type *cell = edge.cell(); + const VD::cell_type *cell2 = edge.twin()->cell(); + if (cell->contains_segment()) + std::swap(cell, cell2); + // State of a cell containing a boundary point is known. + assert(cell->contains_point()); + set_cell_state(cell, outside ? CellState::Active : CellState::Inactive); + // State of a cell containing a boundary edge is Boundary. + assert(cell2->contains_segment()); + set_cell_state(cell2, CellState::Boundary); + } + } else if (edge.vertex0() != nullptr) { + // Finite edge. + const VD::cell_type *cell = edge.cell(); + const Line *line = cell->contains_segment() ? &lines[cell->source_index()] : nullptr; + if (line == nullptr) { + cell = edge.twin()->cell(); + line = cell->contains_segment() ? &lines[cell->source_index()] : nullptr; + } + if (line) { + const VD::vertex_type *v1 = edge.vertex1(); + const VD::cell_type *cell2 = (cell == edge.cell()) ? edge.twin()->cell() : edge.cell(); + assert(v1); + const Point *pt_on_contour = nullptr; + if (cell == edge.cell() && edge.twin()->cell()->contains_segment()) { + // Constrained bisector of two segments. + // If the two segments share a point, then one end of the current Voronoi edge shares this point as well. + // Find pt_on_contour if it exists. + const Line &line2 = lines[cell2->source_index()]; + if (line->a == line2.b) + pt_on_contour = &line->a; + else if (line->b == line2.a) + pt_on_contour = &line->b; + } else if (edge.is_secondary()) { + assert(edge.is_linear()); + // One end of the current Voronoi edge shares a point of a contour. + assert(edge.cell()->contains_point() != edge.twin()->cell()->contains_point()); + const Line &line2 = lines[cell2->source_index()]; + pt_on_contour = &((cell2->source_category() == boost::polygon::SOURCE_CATEGORY_SEGMENT_START_POINT) ? line2.a : line2.b); + } + if (pt_on_contour) { + // One end of the current Voronoi edge shares a point of a contour. + // Find out which one it is. + const VD::vertex_type *v0 = edge.vertex0(); + Vec2d vec0(v0->x() - pt_on_contour->x(), v0->y() - pt_on_contour->y()); + Vec2d vec1(v1->x() - pt_on_contour->x(), v1->y() - pt_on_contour->y()); + double d0 = vec0.squaredNorm(); + double d1 = vec1.squaredNorm(); + assert(std::min(d0, d1) < SCALED_EPSILON * SCALED_EPSILON); + if (d0 < d1) { + // v0 is equal to pt. + } else { + // Skip secondary edge pointing to a contour point. + set_edge_state_initial(&edge, EdgeState::Inactive); + continue; + } + } + Vec2d l0(line->a.cast<double>()); + Vec2d lv((line->b - line->a).cast<double>()); + double side = cross2(lv, Vec2d(v1->x(), v1->y()) - l0); + bool edge_active = outside ? (side < 0.) : (side > 0.); + set_edge_state_initial(&edge, edge_active ? EdgeState::Possible : EdgeState::Inactive); + assert(cell->contains_segment()); + set_cell_state(cell, + pt_on_contour ? CellState::Boundary : + edge_active ? CellState::Active : CellState::Inactive); + set_cell_state(cell2, + (pt_on_contour && cell2->contains_segment()) ? + CellState::Boundary : + edge_active ? CellState::Active : CellState::Inactive); + } + } + { + // Perform one round of expansion marking Voronoi edges and cells next to boundary cells as active / inactive. + std::vector<const VD::cell_type*> cell_queue; + for (const VD::edge_type &edge : vd.edges()) + if (edge_state[&edge - front_edge] == EdgeState::Unknown) { + assert(edge.cell()->contains_point() && edge.twin()->cell()->contains_point()); + // Edge separating two point sources, not yet classified as inside / outside. + CellState cs = cell_state[edge.cell() - front_cell]; + CellState cs2 = cell_state[edge.twin()->cell() - front_cell]; + if (cs != CellState::Unknown || cs2 != CellState::Unknown) { + if (cs == CellState::Unknown) { + cs = cs2; + if (set_cell_state(edge.cell(), cs)) + cell_queue.emplace_back(edge.cell()); + } else if (set_cell_state(edge.twin()->cell(), cs)) + cell_queue.emplace_back(edge.twin()->cell()); + EdgeState es = (cs == CellState::Active) ? EdgeState::Possible : EdgeState::Inactive; + set_edge_state_initial(&edge, es); + set_edge_state_initial(edge.twin(), es); + } else { + const VD::edge_type *e = edge.twin()->rot_prev(); + do { + EdgeState es = edge_state[e->twin() - front_edge]; + if (es != EdgeState::Unknown) { + assert(es == EdgeState::Possible || es == EdgeState::Inactive); + set_edge_state_initial(&edge, es); + CellState cs = (es == EdgeState::Possible) ? CellState::Active : CellState::Inactive; + if (set_cell_state(edge.cell(), cs)) + cell_queue.emplace_back(edge.cell()); + if (set_cell_state(edge.twin()->cell(), cs)) + cell_queue.emplace_back(edge.twin()->cell()); + break; + } + e = e->rot_prev(); + } while (e != edge.twin()); + } + } + // Do a final seed fill over Voronoi cells and unmarked Voronoi edges. + while (! cell_queue.empty()) { + const VD::cell_type *cell = cell_queue.back(); + const CellState cs = cell_state[cell - front_cell]; + cell_queue.pop_back(); + const VD::edge_type *first_edge = cell->incident_edge(); + const VD::edge_type *edge = cell->incident_edge(); + EdgeState es = (cs == CellState::Active) ? EdgeState::Possible : EdgeState::Inactive; + do { + if (set_cell_state(edge->twin()->cell(), cs)) { + set_edge_state_initial(edge, es); + set_edge_state_initial(edge->twin(), es); + cell_queue.emplace_back(edge->twin()->cell()); + } + edge = edge->next(); + } while (edge != first_edge); + } + } + + if (! outside) + offset_distance = - offset_distance; + +#ifdef VORONOI_DEBUG_OUT + BoundingBox bbox; + { + bbox.merge(get_extents(lines)); + bbox.min -= (0.01 * bbox.size().cast<double>()).cast<coord_t>(); + bbox.max += (0.01 * bbox.size().cast<double>()).cast<coord_t>(); + } + static int irun = 0; + ++ irun; + { + Lines helper_lines; + for (const VD::edge_type &edge : vd.edges()) + if (edge_state[&edge - front_edge] == EdgeState::Possible) { + const VD::vertex_type *v0 = edge.vertex0(); + const VD::vertex_type *v1 = edge.vertex1(); + assert(v0 != nullptr); + Vec2d pt1(v0->x(), v0->y()); + Vec2d pt2; + if (v1 == nullptr) { + // Unconstrained edge. Calculate a trimmed position. + assert(edge.is_linear()); + const VD::cell_type *cell = edge.cell(); + const VD::cell_type *cell2 = edge.twin()->cell(); + const Line &line0 = lines[cell->source_index()]; + const Line &line1 = lines[cell2->source_index()]; + if (cell->contains_point() && cell2->contains_point()) { + const Point &pt0 = (cell->source_category() == boost::polygon::SOURCE_CATEGORY_SEGMENT_START_POINT) ? line0.a : line0.b; + const Point &pt1 = (cell2->source_category() == boost::polygon::SOURCE_CATEGORY_SEGMENT_START_POINT) ? line1.a : line1.b; + // Direction vector of this unconstrained Voronoi edge. + Vec2d dir(double(pt0.y() - pt1.y()), double(pt1.x() - pt0.x())); + pt2 = Vec2d(v0->x(), v0->y()) + dir.normalized() * scale_(10.); + } else { + // Infinite edges could not be created by two segment sites. + assert(cell->contains_point() != cell2->contains_point()); + // Linear edge goes through the endpoint of a segment. + assert(edge.is_secondary()); + const Point &ipt = cell->contains_segment() ? + ((cell2->source_category() == boost::polygon::SOURCE_CATEGORY_SEGMENT_START_POINT) ? line1.a : line1.b) : + ((cell->source_category() == boost::polygon::SOURCE_CATEGORY_SEGMENT_START_POINT) ? line0.a : line0.b); + // Infinite edge starts at an input contour, therefore there is always an intersection with an offset curve. + const Line &line = cell->contains_segment() ? line0 : line1; + assert(line.a == ipt || line.b == ipt); + // dir is perpendicular to line. + Vec2d dir(line.a.y() - line.b.y(), line.b.x() - line.a.x()); + assert(dir.norm() > 0.); + if (((line.a == ipt) == cell->contains_point()) == (v0 == nullptr)) + dir = - dir; + pt2 = ipt.cast<double>() + dir.normalized() * scale_(10.); + } + } else { + pt2 = Vec2d(v1->x(), v1->y()); + // Clip the line by the bounding box, so that the coloring of the line will be visible. + Geometry::liang_barsky_line_clipping(pt1, pt2, BoundingBoxf(bbox.min.cast<double>(), bbox.max.cast<double>())); + } + helper_lines.emplace_back(Line(Point(pt1.cast<coord_t>()), Point(((pt1 + pt2) * 0.5).cast<coord_t>()))); + } + dump_voronoi_to_svg(debug_out_path("voronoi-offset-candidates1-%d.svg", irun).c_str(), vd, Points(), lines, Polygons(), helper_lines); + } +#endif // VORONOI_DEBUG_OUT + + std::vector<Vec2d> edge_offset_point(vd.num_edges(), Vec2d()); + const double offset_distance2 = offset_distance * offset_distance; + for (const VD::edge_type &edge : vd.edges()) { + assert(edge_state[&edge - front_edge] != EdgeState::Unknown); + size_t edge_idx = &edge - front_edge; + if (edge_state[edge_idx] == EdgeState::Possible) { + // Edge candidate, intersection points were not calculated yet. + const VD::vertex_type *v0 = edge.vertex0(); + const VD::vertex_type *v1 = edge.vertex1(); + assert(v0 != nullptr); + const VD::cell_type *cell = edge.cell(); + const VD::cell_type *cell2 = edge.twin()->cell(); + const Line &line0 = lines[cell->source_index()]; + const Line &line1 = lines[cell2->source_index()]; + size_t edge_idx2 = edge.twin() - front_edge; + if (v1 == nullptr) { + assert(edge.is_infinite()); + assert(edge.is_linear()); + assert(edge_state[edge_idx2] == EdgeState::Inactive); + if (cell->contains_point() && cell2->contains_point()) { + assert(! edge.is_secondary()); + const Point &pt0 = (cell->source_category() == boost::polygon::SOURCE_CATEGORY_SEGMENT_START_POINT) ? line0.a : line0.b; + const Point &pt1 = (cell2->source_category() == boost::polygon::SOURCE_CATEGORY_SEGMENT_START_POINT) ? line1.a : line1.b; + double dmin2 = (Vec2d(v0->x(), v0->y()) - pt0.cast<double>()).squaredNorm(); + assert(dmin2 >= SCALED_EPSILON * SCALED_EPSILON); + if (dmin2 <= offset_distance2) { + // There shall be an intersection of this unconstrained edge with the offset curve. + // Direction vector of this unconstrained Voronoi edge. + Vec2d dir(double(pt0.y() - pt1.y()), double(pt1.x() - pt0.x())); + Vec2d pt(v0->x(), v0->y()); + double t = detail::first_circle_segment_intersection_parameter(Vec2d(pt0.x(), pt0.y()), offset_distance, pt, dir); + edge_offset_point[edge_idx] = pt + t * dir; + set_edge_state_final(edge_idx, EdgeState::Active); + } else + set_edge_state_final(edge_idx, EdgeState::Inactive); + } else { + // Infinite edges could not be created by two segment sites. + assert(cell->contains_point() != cell2->contains_point()); + // Linear edge goes through the endpoint of a segment. + assert(edge.is_secondary()); + const Point &ipt = cell->contains_segment() ? + ((cell2->source_category() == boost::polygon::SOURCE_CATEGORY_SEGMENT_START_POINT) ? line1.a : line1.b) : + ((cell->source_category() == boost::polygon::SOURCE_CATEGORY_SEGMENT_START_POINT) ? line0.a : line0.b); + #ifndef NDEBUG + if (cell->contains_segment()) { + const Point &pt1 = (cell2->source_category() == boost::polygon::SOURCE_CATEGORY_SEGMENT_START_POINT) ? line1.a : line1.b; + assert((pt1.x() == line0.a.x() && pt1.y() == line0.a.y()) || + (pt1.x() == line0.b.x() && pt1.y() == line0.b.y())); + } else { + const Point &pt0 = (cell->source_category() == boost::polygon::SOURCE_CATEGORY_SEGMENT_START_POINT) ? line0.a : line0.b; + assert((pt0.x() == line1.a.x() && pt0.y() == line1.a.y()) || + (pt0.x() == line1.b.x() && pt0.y() == line1.b.y())); + } + assert((Vec2d(v0->x(), v0->y()) - ipt.cast<double>()).norm() < SCALED_EPSILON); + #endif /* NDEBUG */ + // Infinite edge starts at an input contour, therefore there is always an intersection with an offset curve. + const Line &line = cell->contains_segment() ? line0 : line1; + assert(line.a == ipt || line.b == ipt); + edge_offset_point[edge_idx] = ipt.cast<double>() + offset_distance * Vec2d(line.b.y() - line.a.y(), line.a.x() - line.b.x()).normalized(); + set_edge_state_final(edge_idx, EdgeState::Active); + } + // The other edge of an unconstrained edge starting with null vertex shall never be intersected. + set_edge_state_final(edge_idx2, EdgeState::Inactive); + } else if (edge.is_secondary()) { + assert(edge.is_linear()); + assert(cell->contains_point() != cell2->contains_point()); + const Line &line0 = lines[edge.cell()->source_index()]; + const Line &line1 = lines[edge.twin()->cell()->source_index()]; + const Point &pt = cell->contains_point() ? + ((cell->source_category() == boost::polygon::SOURCE_CATEGORY_SEGMENT_START_POINT) ? line0.a : line0.b) : + ((cell2->source_category() == boost::polygon::SOURCE_CATEGORY_SEGMENT_START_POINT) ? line1.a : line1.b); + const Line &line = cell->contains_segment() ? line0 : line1; + assert(pt == line.a || pt == line.b); + assert((pt.cast<double>() - Vec2d(v0->x(), v0->y())).norm() < SCALED_EPSILON); + Vec2d dir(v1->x() - v0->x(), v1->y() - v0->y()); + double l2 = dir.squaredNorm(); + if (offset_distance2 <= l2) { + edge_offset_point[edge_idx] = pt.cast<double>() + (offset_distance / sqrt(l2)) * dir; + set_edge_state_final(edge_idx, EdgeState::Active); + } else { + set_edge_state_final(edge_idx, EdgeState::Inactive); + } + set_edge_state_final(edge_idx2, EdgeState::Inactive); + } else { + // Finite edge has valid points at both sides. + bool done = false; + if (cell->contains_segment() && cell2->contains_segment()) { + // This edge is a bisector of two line segments. Project v0, v1 onto one of the line segments. + Vec2d pt(line0.a.cast<double>()); + Vec2d dir(line0.b.cast<double>() - pt); + Vec2d vec0 = Vec2d(v0->x(), v0->y()) - pt; + Vec2d vec1 = Vec2d(v1->x(), v1->y()) - pt; + double l2 = dir.squaredNorm(); + assert(l2 > 0.); + double dmin = (dir * (vec0.dot(dir) / l2) - vec0).squaredNorm(); + double dmax = (dir * (vec1.dot(dir) / l2) - vec1).squaredNorm(); + bool flip = dmin > dmax; + if (flip) + std::swap(dmin, dmax); + if (offset_distance2 >= dmin && offset_distance2 <= dmax) { + // Intersect. Maximum one intersection will be found. + // This edge is a bisector of two line segments. Distance to the input polygon increases/decreases monotonically. + dmin = sqrt(dmin); + dmax = sqrt(dmax); + assert(offset_distance > dmin - EPSILON && offset_distance < dmax + EPSILON); + double ddif = dmax - dmin; + if (ddif == 0.) { + // line, line2 are exactly parallel. This is a singular case, the offset curve should miss it. + } else { + if (flip) { + std::swap(edge_idx, edge_idx2); + std::swap(v0, v1); + } + double t = clamp(0., 1., (offset_distance - dmin) / ddif); + edge_offset_point[edge_idx] = Vec2d(lerp(v0->x(), v1->x(), t), lerp(v0->y(), v1->y(), t)); + set_edge_state_final(edge_idx, EdgeState::Active); + set_edge_state_final(edge_idx2, EdgeState::Inactive); + done = true; + } + } + } else { + assert(cell->contains_point() || cell2->contains_point()); + bool point_vs_segment = cell->contains_point() != cell2->contains_point(); + const Point &pt0 = cell->contains_point() ? + ((cell->source_category() == boost::polygon::SOURCE_CATEGORY_SEGMENT_START_POINT) ? line0.a : line0.b) : + ((cell2->source_category() == boost::polygon::SOURCE_CATEGORY_SEGMENT_START_POINT) ? line1.a : line1.b); + // Project p0 to line segment <v0, v1>. + Vec2d p0(v0->x(), v0->y()); + Vec2d p1(v1->x(), v1->y()); + Vec2d px(pt0.x(), pt0.y()); + double d0 = (p0 - px).squaredNorm(); + double d1 = (p1 - px).squaredNorm(); + double dmin = std::min(d0, d1); + double dmax = std::max(d0, d1); + bool has_intersection = false; + bool possibly_two_points = false; + if (offset_distance2 <= dmax) { + if (offset_distance2 >= dmin) { + has_intersection = true; + } else { + double dmin_new = dmin; + if (point_vs_segment) { + // Project on the source segment. + const Line &line = cell->contains_segment() ? line0 : line1; + const Vec2d pt_line = line.a.cast<double>(); + const Vec2d v_line = (line.b - line.a).cast<double>(); + double t0 = (p0 - pt_line).dot(v_line); + double t1 = (p1 - pt_line).dot(v_line); + double tx = (px - pt_line).dot(v_line); + if ((tx >= t0 && tx <= t1) || (tx >= t1 && tx <= t0)) { + // Projection of the Point site falls between the projections of the Voronoi edge end points + // onto the Line site. + Vec2d ft = pt_line + (tx / v_line.squaredNorm()) * v_line; + dmin_new = (ft - px).squaredNorm() * 0.25; + } + } else { + // Point-Point Voronoi sites. Project point site onto the current Voronoi edge. + Vec2d v = p1 - p0; + auto l2 = v.squaredNorm(); + assert(l2 > 0); + auto t = v.dot(px - p0); + if (t >= 0. && t <= l2) { + // Projection falls onto the Voronoi edge. Calculate foot point and distance. + Vec2d ft = p0 + (t / l2) * v; + dmin_new = (ft - px).squaredNorm(); + } + } + assert(dmin_new < dmax + SCALED_EPSILON); + assert(dmin_new < dmin + SCALED_EPSILON); + if (dmin_new < dmin) { + dmin = dmin_new; + has_intersection = possibly_two_points = offset_distance2 >= dmin; + } + } + } + if (has_intersection) { + detail::Intersections intersections; + if (point_vs_segment) { + assert(cell->contains_point() || cell2->contains_point()); + intersections = detail::line_point_equal_distance_points(cell->contains_segment() ? line0 : line1, pt0, offset_distance); + } else { + const Point &pt1 = (cell2->source_category() == boost::polygon::SOURCE_CATEGORY_SEGMENT_START_POINT) ? line1.a : line1.b; + intersections = detail::point_point_equal_distance_points(pt0, pt1, offset_distance); + } + // If the span of distances of start / end point / foot point to the point site indicate an intersection, + // we should find one. + assert(intersections.count > 0); + if (intersections.count == 2) { + // Now decide which points fall on this Voronoi edge. + // Tangential points (single intersection) are ignored. + if (possibly_two_points) { + Vec2d v = p1 - p0; + double l2 = v.squaredNorm(); + double t0 = v.dot(intersections.pts[0] - p0); + double t1 = v.dot(intersections.pts[1] - p0); + if (t0 > t1) { + std::swap(t0, t1); + std::swap(intersections.pts[0], intersections.pts[1]); + } + // Remove points outside of the line range. + if (t0 < 0. || t0 > l2) { + if (t1 < 0. || t1 > l2) + intersections.count = 0; + else { + -- intersections.count; + t0 = t1; + intersections.pts[0] = intersections.pts[1]; + } + } else if (t1 < 0. || t1 > l2) + -- intersections.count; + } else { + // Take the point furthest from the end points of the Voronoi edge or a Voronoi parabolic arc. + double d0 = std::max((intersections.pts[0] - p0).squaredNorm(), (intersections.pts[0] - p1).squaredNorm()); + double d1 = std::max((intersections.pts[1] - p0).squaredNorm(), (intersections.pts[1] - p1).squaredNorm()); + if (d0 > d1) + intersections.pts[0] = intersections.pts[1]; + -- intersections.count; + } + assert(intersections.count > 0); + if (intersections.count == 2) { + set_edge_state_final(edge_idx, EdgeState::Active); + set_edge_state_final(edge_idx2, EdgeState::Active); + edge_offset_point[edge_idx] = intersections.pts[1]; + edge_offset_point[edge_idx2] = intersections.pts[0]; + done = true; + } else if (intersections.count == 1) { + if (d1 < d0) + std::swap(edge_idx, edge_idx2); + set_edge_state_final(edge_idx, EdgeState::Active); + set_edge_state_final(edge_idx2, EdgeState::Inactive); + edge_offset_point[edge_idx] = intersections.pts[0]; + done = true; + } + } + } + } + if (! done) { + set_edge_state_final(edge_idx, EdgeState::Inactive); + set_edge_state_final(edge_idx2, EdgeState::Inactive); + } + } + } + } + +#ifndef NDEBUG + for (const VD::edge_type &edge : vd.edges()) { + assert(edge_state[&edge - front_edge] == EdgeState::Inactive || edge_state[&edge - front_edge] == EdgeState::Active); + // None of a new edge candidate may start with null vertex. + assert(edge_state[&edge - front_edge] == EdgeState::Inactive || edge.vertex0() != nullptr); + assert(edge_state[edge.twin() - front_edge] == EdgeState::Inactive || edge.twin()->vertex0() != nullptr); + } +#endif // NDEBUG + +#ifdef VORONOI_DEBUG_OUT + { + Lines helper_lines; + for (const VD::edge_type &edge : vd.edges()) + if (edge_state[&edge - front_edge] == EdgeState::Active) + helper_lines.emplace_back(Line(Point(edge.vertex0()->x(), edge.vertex0()->y()), Point(edge_offset_point[&edge - front_edge].cast<coord_t>()))); + dump_voronoi_to_svg(debug_out_path("voronoi-offset-candidates2-%d.svg", irun).c_str(), vd, Points(), lines, Polygons(), helper_lines); + } +#endif // VORONOI_DEBUG_OUT + + auto next_offset_edge = [&edge_state, front_edge](const VD::edge_type *start_edge) -> const VD::edge_type* { + for (const VD::edge_type *edge = start_edge->next(); edge != start_edge; edge = edge->next()) + if (edge_state[edge->twin() - front_edge] == EdgeState::Active) + return edge->twin(); + // assert(false); + return nullptr; + }; + +#ifndef NDEBUG + auto dist_to_site = [&lines](const VD::cell_type &cell, const Vec2d &point) { + const Line &line = lines[cell.source_index()]; + return cell.contains_point() ? + (((cell.source_category() == boost::polygon::SOURCE_CATEGORY_SEGMENT_START_POINT) ? line.a : line.b).cast<double>() - point).norm() : + (Geometry::foot_pt<Vec2d>(line.a.cast<double>(), (line.b - line.a).cast<double>(), point) - point).norm(); + }; +#endif /* NDEBUG */ + + // Track the offset curves. + Polygons out; + double angle_step = 2. * acos((offset_distance - discretization_error) / offset_distance); + double cos_threshold = cos(angle_step); + for (size_t seed_edge_idx = 0; seed_edge_idx < vd.num_edges(); ++ seed_edge_idx) + if (edge_state[seed_edge_idx] == EdgeState::Active) { + const VD::edge_type *start_edge = &vd.edges()[seed_edge_idx]; + const VD::edge_type *edge = start_edge; + Polygon poly; + do { + // find the next edge + const VD::edge_type *next_edge = next_offset_edge(edge); +#ifdef VORONOI_DEBUG_OUT + if (next_edge == nullptr) { + Lines helper_lines; + dump_voronoi_to_svg(debug_out_path("voronoi-offset-open-loop-%d.svg", irun).c_str(), vd, Points(), lines, Polygons(), to_lines(poly)); + } +#endif // VORONOI_DEBUG_OUT + assert(next_edge); + //std::cout << "offset-output: "; print_edge(edge); std::cout << " to "; print_edge(next_edge); std::cout << "\n"; + // Interpolate a circular segment or insert a linear segment between edge and next_edge. + const VD::cell_type *cell = edge->cell(); + edge_state[next_edge - front_edge] = EdgeState::Inactive; + Vec2d p1 = edge_offset_point[edge - front_edge]; + Vec2d p2 = edge_offset_point[next_edge - front_edge]; +#ifndef NDEBUG + { + double err = dist_to_site(*cell, p1) - offset_distance; + double err2 = dist_to_site(*cell, p2) - offset_distance; +#ifdef VORONOI_DEBUG_OUT + if (std::max(err, err2) >= SCALED_EPSILON) { + Lines helper_lines; + dump_voronoi_to_svg(debug_out_path("voronoi-offset-incorrect_pt-%d.svg", irun).c_str(), vd, Points(), lines, Polygons(), to_lines(poly)); + } +#endif // VORONOI_DEBUG_OUT + assert(std::abs(err) < SCALED_EPSILON); + assert(std::abs(err2) < SCALED_EPSILON); + } +#endif /* NDEBUG */ + if (cell->contains_point()) { + // Discretize an arc from p1 to p2 with radius = offset_distance and discretization_error. + // The extracted contour is CCW oriented, extracted holes are CW oriented. + // The extracted arc will have the same orientation. As the Voronoi regions are convex, the angle covered by the arc will be convex as well. + const Line &line0 = lines[cell->source_index()]; + const Vec2d ¢er = ((cell->source_category() == boost::polygon::SOURCE_CATEGORY_SEGMENT_START_POINT) ? line0.a : line0.b).cast<double>(); + const Vec2d v1 = p1 - center; + const Vec2d v2 = p2 - center; + bool ccw = cross2(v1, v2) > 0; + double cos_a = v1.dot(v2); + double norm = v1.norm() * v2.norm(); + assert(norm > 0.); + if (cos_a < cos_threshold * norm) { + // Angle is bigger than the threshold, therefore the arc will be discretized. + cos_a /= norm; + assert(cos_a > -1. - EPSILON && cos_a < 1. + EPSILON); + double angle = acos(std::max(-1., std::min(1., cos_a))); + size_t n_steps = size_t(ceil(angle / angle_step)); + double astep = angle / n_steps; + if (! ccw) + astep *= -1.; + double a = astep; + for (size_t i = 1; i < n_steps; ++ i, a += astep) { + double c = cos(a); + double s = sin(a); + Vec2d p = center + Vec2d(c * v1.x() - s * v1.y(), s * v1.x() + c * v1.y()); + poly.points.emplace_back(Point(coord_t(p.x()), coord_t(p.y()))); + } + } + } + { + Point pt_last(coord_t(p2.x()), coord_t(p2.y())); + if (poly.empty() || poly.points.back() != pt_last) + poly.points.emplace_back(pt_last); + } + edge = next_edge; + } while (edge != start_edge); + out.emplace_back(std::move(poly)); + } + + return out; +} + +} // namespace Slic3r |