extended the lines AABB tree with intersections of line with all lines in the tree

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 <algorithm>
 #include <type_traits>
 #include <vector>
 
@@ -89,6 +91,48 @@ inline std::tuple<size_t, size_t> coordinate_aligned_ray_hit_count(size_t node_i
     }
 }
 
+template<typename LineType, typename TreeType, typename VectorType>
+inline std::vector<VectorType> get_intersections_with_line(size_t                                node_idx,
+                                                           const TreeType                       &tree,
+                                                           const std::vector<LineType>          &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<VectorType> result;
+
+        if (node_left.bbox.intersects(line_bb)) {
+            std::vector<VectorType> intersections = get_intersections_with_line<LineType, TreeType, VectorType>(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<VectorType> intersections = get_intersections_with_line<LineType, TreeType, VectorType>(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<LineType> &lines, con
     if (tree.empty()) { return 1; }
 
     auto [hits_above, hits_below] = detail::coordinate_aligned_ray_hit_count<LineType, TreeType, VectorType, 0>(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<LineType> &lines, con
     }
 }
 
+template<bool sorted, typename VectorType, typename LineType, typename TreeType>
+inline std::vector<VectorType> get_intersections_with_line(const std::vector<LineType> &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<LineType, TreeType, VectorType>(0, tree, lines, line, line_bb);
+    if (sorted) {
+        using Floating =
+            typename std::conditional<std::is_floating_point<typename LineType::Scalar>::value, typename LineType::Scalar, double>::type;
+
+        std::vector<std::pair<Floating, VectorType>> points_with_sq_distance{};
+        for (const VectorType &p : intersections) {
+            points_with_sq_distance.emplace_back((p - line.a).template cast<Floating>().squaredNorm(), p);
+        }
+        std::sort(points_with_sq_distance.begin(), points_with_sq_distance.end(),
+                  [](const std::pair<Floating, VectorType> &left, std::pair<Floating, VectorType> &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<typename LineType> class LinesDistancer
 {
 private:
@@ -206,7 +275,7 @@ public:
 
     explicit LinesDistancer(std::vector<LineType> &&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<bool sorted> std::vector<Vec<2, Scalar>> intersections_with_line(const LineType &line) const
+    {
+        return get_intersections_with_line<sorted, Vec<2, Scalar>>(lines, tree, line);
+    }
+
     const LineType &get_line(size_t line_idx) const { return lines[line_idx]; }
 
     const std::vector<LineType> &get_lines() const { return lines; }

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;
+    return line_alg::intersection(*this, l2, intersection);
-    const Vec2d  v1  = (l1.b - l1.a).cast<double>();
-    const Vec2d  v2  = (l2.b - l2.a).cast<double>();
-    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>();
-    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<double>() + t1 * v1).cast<coord_t>();
-        return true;
-    }
-    return false;  // not intersecting
 }
 
 bool Line::clip_with_bbox(const BoundingBox &bbox)

src/libslic3r/Line.hpp

@@ -120,6 +120,33 @@ double distance_to_infinite(const L &line, const Vec<Dim<L>, Scalar<L>> &point)
     return std::sqrt(distance_to_infinite_squared(line, point));
 }
 
+template<class L> bool intersection(const L &l1, const L &l2, Vec<Dim<L>, Scalar<L>> *intersection_pt)
+{
+    using Floating      = typename std::conditional<std::is_floating_point<Scalar<L>>::value, Scalar<L>, double>::type;
+    using VecType       = const Vec<Dim<L>, Floating>;
+    const VecType v1    = (l1.b - l1.a).template cast<Floating>();
+    const VecType v2    = (l2.b - l2.a).template cast<Floating>();
+    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>();
+    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<Floating>() + t1 * v1).template cast<Scalar<L>>();
+        return true;
+    }
+    return false; // not intersecting
+}
+
 } // namespace line_alg
 
 class Line