diff --git a/src/libslic3r/AABBTreeLines.hpp b/src/libslic3r/AABBTreeLines.hpp index cc9fabf88..f62bed3f7 100644 --- a/src/libslic3r/AABBTreeLines.hpp +++ b/src/libslic3r/AABBTreeLines.hpp @@ -1,10 +1,12 @@ #ifndef SRC_LIBSLIC3R_AABBTREELINES_HPP_ #define SRC_LIBSLIC3R_AABBTREELINES_HPP_ +#include "Point.hpp" #include "Utils.hpp" #include "libslic3r.h" #include "libslic3r/AABBTreeIndirect.hpp" #include "libslic3r/Line.hpp" +#include #include #include @@ -89,6 +91,48 @@ inline std::tuple coordinate_aligned_ray_hit_count(size_t node_i } } +template +inline std::vector get_intersections_with_line(size_t node_idx, + const TreeType &tree, + const std::vector &lines, + const LineType &line, + const typename TreeType::BoundingBox &line_bb) +{ + const auto &node = tree.node(node_idx); + assert(node.is_valid()); + if (node.is_leaf()) { + VectorType intersection_pt; + if (line_alg::intersection(line, lines[node.idx], &intersection_pt)) { + return {intersection_pt}; + } else { + return {}; + } + } else { + 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()); + + std::vector result; + + if (node_left.bbox.intersects(line_bb)) { + std::vector intersections = get_intersections_with_line(left_node_idx, tree, lines, + line, line_bb); + result.insert(result.end(), intersections.begin(), intersections.end()); + } + + if (node_right.bbox.intersects(line_bb)) { + std::vector intersections = get_intersections_with_line(right_node_idx, tree, lines, + line, line_bb); + result.insert(result.end(), intersections.begin(), intersections.end()); + } + + return result; + } +} + } // namespace detail // Build a balanced AABB Tree over a vector of lines, balancing the tree @@ -173,7 +217,6 @@ inline int point_outside_closed_contours(const std::vector &lines, con 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) { @@ -190,6 +233,32 @@ inline int point_outside_closed_contours(const std::vector &lines, con } } +template +inline std::vector get_intersections_with_line(const std::vector &lines, const TreeType &tree, const LineType &line) +{ + if (tree.empty()) { return {}; } + auto line_bb = typename TreeType::BoundingBox(line.a, line.a); + line_bb.extend(line.b); + + auto intersections = detail::get_intersections_with_line(0, tree, lines, line, line_bb); + if (sorted) { + using Floating = + typename std::conditional::value, typename LineType::Scalar, double>::type; + + std::vector> points_with_sq_distance{}; + for (const VectorType &p : intersections) { + points_with_sq_distance.emplace_back((p - line.a).template cast().squaredNorm(), p); + } + std::sort(points_with_sq_distance.begin(), points_with_sq_distance.end(), + [](const std::pair &left, std::pair &right) { + return left.first < right.first; + }); + for (size_t i = 0; i < points_with_sq_distance.size(); i++) { intersections[i] = points_with_sq_distance[i].second; } + } + + return intersections; +} + template class LinesDistancer { private: @@ -206,7 +275,7 @@ public: explicit LinesDistancer(std::vector &&lines) : lines(lines) { - tree = AABBTreeLines::build_aabb_tree_over_indexed_lines(this->lines); + tree = AABBTreeLines::build_aabb_tree_over_indexed_lines(std::move(this->lines)); } LinesDistancer() = default; @@ -240,6 +309,11 @@ public: return all_lines_in_radius(this->lines, this->tree, point, radius * radius); } + template std::vector> intersections_with_line(const LineType &line) const + { + return get_intersections_with_line>(lines, tree, line); + } + const LineType &get_line(size_t line_idx) const { return lines[line_idx]; } const std::vector &get_lines() const { return lines; } diff --git a/src/libslic3r/Line.cpp b/src/libslic3r/Line.cpp index 68a7449c7..7e75d5632 100644 --- a/src/libslic3r/Line.cpp +++ b/src/libslic3r/Line.cpp @@ -91,28 +91,7 @@ bool Line::perpendicular_to(const Line& line) const bool Line::intersection(const Line &l2, Point *intersection) const { - const Line &l1 = *this; - const Vec2d v1 = (l1.b - l1.a).cast(); - const Vec2d v2 = (l2.b - l2.a).cast(); - double denom = cross2(v1, v2); - if (fabs(denom) < EPSILON) -#if 0 - // Lines are collinear. Return true if they are coincident (overlappign). - return ! (fabs(nume_a) < EPSILON && fabs(nume_b) < EPSILON); -#else - return false; -#endif - const Vec2d v12 = (l1.a - l2.a).cast(); - double nume_a = cross2(v2, v12); - double nume_b = cross2(v1, v12); - double t1 = nume_a / denom; - double t2 = nume_b / denom; - if (t1 >= 0 && t1 <= 1.0f && t2 >= 0 && t2 <= 1.0f) { - // Get the intersection point. - (*intersection) = (l1.a.cast() + t1 * v1).cast(); - return true; - } - return false; // not intersecting + return line_alg::intersection(*this, l2, intersection); } bool Line::clip_with_bbox(const BoundingBox &bbox) diff --git a/src/libslic3r/Line.hpp b/src/libslic3r/Line.hpp index 947c7fcba..90f564898 100644 --- a/src/libslic3r/Line.hpp +++ b/src/libslic3r/Line.hpp @@ -120,6 +120,33 @@ double distance_to_infinite(const L &line, const Vec, Scalar> &point) return std::sqrt(distance_to_infinite_squared(line, point)); } +template bool intersection(const L &l1, const L &l2, Vec, Scalar> *intersection_pt) +{ + using Floating = typename std::conditional>::value, Scalar, double>::type; + using VecType = const Vec, Floating>; + const VecType v1 = (l1.b - l1.a).template cast(); + const VecType v2 = (l2.b - l2.a).template cast(); + Floating denom = cross2(v1, v2); + if (fabs(denom) < EPSILON) +#if 0 + // Lines are collinear. Return true if they are coincident (overlappign). + return ! (fabs(nume_a) < EPSILON && fabs(nume_b) < EPSILON); +#else + return false; +#endif + const VecType v12 = (l1.a - l2.a).template cast(); + Floating nume_a = cross2(v2, v12); + Floating nume_b = cross2(v1, v12); + Floating t1 = nume_a / denom; + Floating t2 = nume_b / denom; + if (t1 >= 0 && t1 <= 1.0f && t2 >= 0 && t2 <= 1.0f) { + // Get the intersection point. + (*intersection_pt) = (l1.a.template cast() + t1 * v1).template cast>(); + return true; + } + return false; // not intersecting +} + } // namespace line_alg class Line