From 11b03804dcec964b7bd63cfd2efdf7ef99fe199e Mon Sep 17 00:00:00 2001 From: PavelMikus Date: Fri, 25 Nov 2022 15:57:59 +0100 Subject: [PATCH] Fix AABB tree query, add new fast query for point outside, which uses axis aligned rays --- src/libslic3r/AABBTreeLines.hpp | 167 ++++++++++++++------- src/libslic3r/GCode/ExtrusionProcessor.hpp | 36 ++++- 2 files changed, 144 insertions(+), 59 deletions(-) diff --git a/src/libslic3r/AABBTreeLines.hpp b/src/libslic3r/AABBTreeLines.hpp index 06cd082c1..cc9fabf88 100644 --- a/src/libslic3r/AABBTreeLines.hpp +++ b/src/libslic3r/AABBTreeLines.hpp @@ -8,78 +8,121 @@ #include #include -namespace Slic3r { - -namespace AABBTreeLines { +namespace Slic3r { namespace AABBTreeLines { namespace detail { -template -struct IndexedLinesDistancer { - using LineType = ALineType; - using TreeType = ATreeType; +template struct IndexedLinesDistancer +{ + using LineType = ALineType; + using TreeType = ATreeType; using VectorType = AVectorType; using ScalarType = typename VectorType::Scalar; const std::vector &lines; - const TreeType &tree; + const TreeType &tree; const VectorType origin; - inline VectorType closest_point_to_origin(size_t primitive_index, - ScalarType &squared_distance) const { + inline VectorType closest_point_to_origin(size_t primitive_index, ScalarType &squared_distance) const + { Vec nearest_point; - const LineType &line = lines[primitive_index]; + const LineType &line = lines[primitive_index]; squared_distance = line_alg::distance_to_squared(line, origin.template cast(), &nearest_point); return nearest_point.template cast(); } }; +// !!! NOTE: algorithm expects the BoundingBoxes to be snug, no epsilon is allowed +template +inline std::tuple coordinate_aligned_ray_hit_count(size_t node_idx, const TreeType &tree, const VectorType &ray_origin) +{ + static constexpr int other_coordinate = (coordinate + 1) % 2; + using Scalar = typename LineType::Scalar; + using Floating = typename std::conditional::value, Scalar, double>::type; + const auto &node = tree.node(node_idx); + assert(node.is_valid()); + if (node.is_leaf()) { + if (ray_origin[coordinate] > node.bbox.max()[coordinate]) { + return {1, 0}; + } else if (ray_origin[coordinate] < node.bbox.min()[coordinate]) { + return {0, 1}; + } else { + auto sizes = node.bbox.sizes(); + Floating t = (ray_origin[other_coordinate] - node.bbox.min()[other_coordinate]) / + (sizes[other_coordinate] > 0 ? sizes[other_coordinate] : 1); + auto intersection = node.bbox.min()[coordinate] + t * sizes[coordinate]; + if (ray_origin[coordinate] > intersection) { + return {1, 0}; + } else if (ray_origin[coordinate] < intersection) { + return {0, 1}; + } else { // ray origin is on boundary + return {1, 1}; + } + } + } else { + size_t intersections_above = 0; + size_t intersections_below = 0; + size_t left_node_idx = node_idx * 2 + 1; + size_t right_node_idx = left_node_idx + 1; + const auto &node_left = tree.node(left_node_idx); + const auto &node_right = tree.node(right_node_idx); + assert(node_left.is_valid()); + assert(node_right.is_valid()); + + if (node_left.bbox.min()[other_coordinate] <= ray_origin[other_coordinate] && + node_left.bbox.max()[other_coordinate] > ray_origin[other_coordinate]) { // sharp inequality, beacuse we do not want to count point common to two lines more than once + auto [above, below] = coordinate_aligned_ray_hit_count(left_node_idx, tree, + ray_origin); + intersections_above += above; + intersections_below += below; + } + + if (node_right.bbox.min()[other_coordinate] <= ray_origin[other_coordinate] && + node_right.bbox.max()[other_coordinate] > ray_origin[other_coordinate]) { + auto [above, below] = coordinate_aligned_ray_hit_count(right_node_idx, tree, + ray_origin); + intersections_above += above; + intersections_below += below; + } + return {intersections_above, intersections_below}; + } } +} // namespace detail + // Build a balanced AABB Tree over a vector of lines, balancing the tree // on centroids of the lines. // Epsilon is applied to the bounding boxes of the AABB Tree to cope with numeric inaccuracies // during tree traversal. template -inline AABBTreeIndirect::Tree<2, typename LineType::Scalar> build_aabb_tree_over_indexed_lines( - const std::vector &lines, - //FIXME do we want to apply an epsilon? - const double eps = 0) - { +inline AABBTreeIndirect::Tree<2, typename LineType::Scalar> build_aabb_tree_over_indexed_lines(const std::vector &lines) +{ using TreeType = AABBTreeIndirect::Tree<2, typename LineType::Scalar>; -// using CoordType = typename TreeType::CoordType; - using VectorType = typename TreeType::VectorType; + // using CoordType = typename TreeType::CoordType; + using VectorType = typename TreeType::VectorType; using BoundingBox = typename TreeType::BoundingBox; - struct InputType { - size_t idx() const { - return m_idx; - } - const BoundingBox& bbox() const { - return m_bbox; - } - const VectorType& centroid() const { - return m_centroid; - } + struct InputType + { + size_t idx() const { return m_idx; } + const BoundingBox &bbox() const { return m_bbox; } + const VectorType ¢roid() const { return m_centroid; } - size_t m_idx; + size_t m_idx; BoundingBox m_bbox; - VectorType m_centroid; + VectorType m_centroid; }; std::vector input; input.reserve(lines.size()); - const VectorType veps(eps, eps); for (size_t i = 0; i < lines.size(); ++i) { const LineType &line = lines[i]; - InputType n; - n.m_idx = i; + InputType n; + n.m_idx = i; n.m_centroid = (line.a + line.b) * 0.5; - n.m_bbox = BoundingBox(line.a, line.a); + n.m_bbox = BoundingBox(line.a, line.a); n.m_bbox.extend(line.b); - n.m_bbox.min() -= veps; - n.m_bbox.max() += veps; input.emplace_back(n); } @@ -112,7 +155,7 @@ template inline std::vector all_lines_in_radius(const std::vector &lines, const TreeType &tree, const VectorType &point, - typename VectorType::Scalar max_distance_squared) + typename VectorType::Scalar max_distance_squared) { auto distancer = detail::IndexedLinesDistancer{lines, tree, point}; @@ -123,6 +166,30 @@ inline std::vector all_lines_in_radius(const std::vector &line return found_lines; } +// return 1 if true, -1 if false, 0 if cannot be determined +template +inline int point_outside_closed_contours(const std::vector &lines, const TreeType &tree, const VectorType &point) +{ + if (tree.empty()) { return 1; } + + auto [hits_above, hits_below] = detail::coordinate_aligned_ray_hit_count(0, tree, point); + std::cout << "hits_above: " << hits_above << " hits_below: " << hits_below << std::endl; + if (hits_above % 2 == 1 && hits_below % 2 == 1) { + return -1; + } else if (hits_above % 2 == 0 && hits_below % 2 == 0) { + return 1; + } else { // this should not happen with closed contours. lets check it in Y direction + auto [hits_above, hits_below] = detail::coordinate_aligned_ray_hit_count(0, tree, point); + if (hits_above % 2 == 1 && hits_below % 2 == 1) { + return -1; + } else if (hits_above % 2 == 0 && hits_below % 2 == 0) { + return 1; + } else { // both results were unclear + return 0; + } + } +} + template class LinesDistancer { private: @@ -144,6 +211,9 @@ public: LinesDistancer() = default; + // 1 true, -1 false, 0 cannot determine + int outside(const Vec<2, Scalar> &point) const { return point_outside_closed_contours(lines, tree, point); } + // negative sign means inside std::tuple> signed_distance_from_lines_extra(const Vec<2, Scalar> &point) const { @@ -153,26 +223,9 @@ public: auto distance = AABBTreeLines::squared_distance_to_indexed_lines(lines, tree, p, nearest_line_index_out, nearest_point_out); if (distance < 0) { return {std::numeric_limits::infinity(), nearest_line_index_out, nearest_point_out}; } - distance = sqrt(distance); - const LineType &line = lines[nearest_line_index_out]; - Vec<2, Floating> v1 = (line.b - line.a).template cast(); - Vec<2, Floating> v2 = (point - line.a).template cast(); - auto d1 = (v1.x() * v2.y()) - (v1.y() * v2.x()); - - LineType second_line = line; - if ((line.a.template cast() - nearest_point_out).squaredNorm() < SCALED_EPSILON) { - second_line = lines[prev_idx_modulo(nearest_line_index_out, lines.size())]; - } else { - second_line = lines[next_idx_modulo(nearest_line_index_out, lines.size())]; - } - v1 = (second_line.b - second_line.a).template cast(); - v2 = (point - second_line.a).template cast(); - auto d2 = (v1.x() * v2.y()) - (v1.y() * v2.x()); - - auto d = abs(d1) > abs(d2) ? d1 : d2; - - if (d > 0.0) { distance *= -1.0; } - + distance = sqrt(distance); + distance *= outside(point); + return {distance, nearest_line_index_out, nearest_point_out}; } diff --git a/src/libslic3r/GCode/ExtrusionProcessor.hpp b/src/libslic3r/GCode/ExtrusionProcessor.hpp index 1bdc67668..33bf6be60 100644 --- a/src/libslic3r/GCode/ExtrusionProcessor.hpp +++ b/src/libslic3r/GCode/ExtrusionProcessor.hpp @@ -7,6 +7,10 @@ #include "../ExtrusionEntity.hpp" #include "../Layer.hpp" #include "../Point.hpp" +#include "../SVG.hpp" +#include "../BoundingBox.hpp" +#include "../Polygon.hpp" +#include "../ClipperUtils.hpp" #include #include @@ -62,7 +66,7 @@ public: class CurvatureEstimator { static const size_t sliders_count = 3; - SlidingWindowCurvatureAccumulator sliders[sliders_count] = {{2.0},{4.0}, {8.0}}; + SlidingWindowCurvatureAccumulator sliders[sliders_count] = {{2.0}, {4.0}, {8.0}}; public: void add_point(float distance, float angle) @@ -101,6 +105,34 @@ public: } prev_layer_boundary = next_layer_boundary; next_layer_boundary = AABBTreeLines::LinesDistancer{std::move(layer_lines)}; + +#if 0 // EXPORT DEBUG FILES + Lines scaled_lines; + for (const Linef &lf : layer_lines) { scaled_lines.push_back({Point::new_scale(lf.a), Point::new_scale(lf.b)}); } + BoundingBox bb = get_extents(scaled_lines); + + Points inside; + for (const Layer *layer : layers) { + if (layer == nullptr) continue; + auto in = to_points(to_polygons(offset_ex(layer->lslices, -scale_(0.4)))); + inside.insert(inside.end(), in.begin(), in.end()); + } + + ::Slic3r::SVG svg(debug_out_path(("path_jps" + std::to_string(rand() % 1000)).c_str()).c_str(), bb); + svg.draw(scaled_lines, "black", scale_(0.10)); + for (Point p : inside) { + auto [distance, line_idx, nearest_point] = next_layer_boundary.signed_distance_from_lines_extra(unscaled(p)); + if (distance > 0) { + svg.draw(p, "red", scale_(0.2)); + svg.draw(Point::new_scale(nearest_point.x(), nearest_point.y()), "blue", scale_(0.2)); + auto li = next_layer_boundary.get_line(line_idx); + Line ls{Point::new_scale(li.a), Point::new_scale(li.b)}; + svg.draw(ls, "yellow", scale_(0.2)); + + + } + } +#endif } std::vector estimate_extrusion_quality(const ExtrusionPath &path) @@ -121,7 +153,7 @@ public: double dist_from_prev_layer = prev_layer_boundary.signed_distance_from_lines(p.cast()) + flow_width * 0.5f; - float default_dist_quality = 0.5f; + float default_dist_quality = 0.3f; float distance_quality = 1.0f; if (dist_from_prev_layer < min_malformation_dist) { distance_quality = 1.0f;