Speed up searching for intersections

FIX of O(n^2) complexity!
Remove unneccesary creation empty vectors and testing on empty
Try to remove recursion but result was slower

src/libslic3r/AABBTreeLines.hpp

@@ -121,45 +121,77 @@ inline std::tuple<int, int> coordinate_aligned_ray_hit_count(size_t
 }
 
 template<typename LineType, typename TreeType, typename VectorType>
-inline std::vector<std::pair<VectorType, size_t>> get_intersections_with_line(size_t                                node_idx,
+inline void insert_intersections_with_line(std::vector<std::pair<VectorType, size_t>> &result,
-                                                                              const TreeType                       &tree,
+                                           size_t                                      node_idx,
-                                                                              const std::vector<LineType>          &lines,
+                                           const TreeType                             &tree,
-                                                                              const LineType                       &line,
+                                           const std::vector<LineType>                &lines,
-                                                                              const typename TreeType::BoundingBox &line_bb)
+                                           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 {std::pair<VectorType, size_t>(intersection_pt, node.idx)};
+            result.emplace_back(intersection_pt, node.idx);
-        } else {
-            return {};
         }
-    } else {
+        return;
-        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<std::pair<VectorType, size_t>> result;
-
-        if (node_left.bbox.intersects(line_bb)) {
-            std::vector<std::pair<VectorType, size_t>> 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<std::pair<VectorType, size_t>> 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;
     }
+
+    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.intersects(line_bb)) {
+        insert_intersections_with_line<LineType, TreeType, VectorType>(result, left_node_idx, tree, lines, line, line_bb);
+    }
+
+    if (node_right.bbox.intersects(line_bb)) {
+        insert_intersections_with_line<LineType, TreeType, VectorType>(result, right_node_idx, tree, lines, line, line_bb);
+    }
+
+    //// NOTE: Non recursive implementation - for my case was slower ;-(     
+    // std::vector<size_t> node_indicies_for_check; // evaluation queue
+    // size_t approx_size = static_cast<size_t>(std::ceil(std::sqrt(tree.nodes().size())));
+    // node_indicies_for_check.reserve(approx_size);
+    // do {
+    //     const auto &node = tree.node(node_index);
+    //     assert(node.is_valid());
+    //     if (node.is_leaf()) {
+    //         VectorType intersection_pt;
+    //         if (line_alg::intersection(line, lines[node.idx], &intersection_pt))
+    //             result.emplace_back(intersection_pt, node.idx);
+    //         node_index = 0;// clear next node
+    //     } else {
+    //         size_t      left_node_idx  = node_index * 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());
+
+    //        // Set next node index
+    //        node_index = 0; // clear next node
+    //        if (node_left.bbox.intersects(line_bb))
+    //            node_index = left_node_idx;
+
+    //        if (node_right.bbox.intersects(line_bb)) {
+    //            if (node_index == 0)
+    //                node_index = right_node_idx;
+    //            else
+    //                node_indicies_for_check.push_back(right_node_idx); // store for later evaluation
+    //        }
+    //    }
+
+    //    if (node_index == 0 && !node_indicies_for_check.empty()) {
+    //        // no direct next node take one from queue
+    //        node_index = node_indicies_for_check.back();
+    //        node_indicies_for_check.pop_back();
+    //    }
+    //} while (node_index != 0);
 }
 
 } // namespace detail
@@ -277,7 +309,9 @@ inline std::vector<std::pair<VectorType, size_t>> get_intersections_with_line(co
     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);
+    std::vector<std::pair<VectorType, size_t>> intersections; // result
+    detail::insert_intersections_with_line(intersections, 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;
@@ -293,7 +327,6 @@ inline std::vector<std::pair<VectorType, size_t>> get_intersections_with_line(co
             intersections[i] = points_with_sq_distance[i].second;
         }
     }
-
     return intersections;
 }
 

src/libslic3r/IntersectionPoints.cpp

@@ -5,6 +5,10 @@
 #include "IntersectionPoints.hpp"
 
 //#define USE_CGAL_SWEEP_LINE
+#define USE_AABB_TREE
+//#define USE_AABB_TREE_FLOAT
+//#define USE_LINE_TO_LINE
+
 #ifdef USE_CGAL_SWEEP_LINE
 
 #include <CGAL/Cartesian.h>
@@ -109,8 +113,64 @@ Slic3r::Pointfs Slic3r::intersection_points(const ExPolygons &expolygons)
 // use bounding boxes
 #include <libslic3r/BoundingBox.hpp>
 
-namespace priv {
+#include <libslic3r/AABBTreeLines.hpp>
-//FIXME O(n^2) complexity!
+namespace{  
+#ifdef USE_AABB_TREE
+// NOTE: it is about 18% slower than USE_LINE_TO_LINE on 'contour_ALIENATO.TTF_glyph_i'
+using namespace Slic3r;
+Pointfs compute_intersections(const Lines &lines)
+{
+    if (lines.size() < 3)
+        return {};    
+
+    auto tree = AABBTreeLines::build_aabb_tree_over_indexed_lines(lines);
+    Pointfs result;
+    for (size_t li = 0; li < lines.size()-1; ++li) {
+        const Line &l = lines[li];
+        auto intersections = AABBTreeLines::get_intersections_with_line<false, Point, Line>(lines, tree, l);
+        for (const auto &[p, node_index] : intersections) {
+            if (node_index - 1 <= li)
+                continue;
+            if (p == l.a || p == l.b)
+                continue;
+            result.push_back(p.cast<double>());
+        }
+    }
+    return result;
+}
+#endif // USE_AABB_TREE
+
+#ifdef USE_AABB_TREE_FLOAT
+// NOTE: It is slower than int tree, but has floating point for intersection
+using namespace Slic3r;
+Pointfs compute_intersections(const Lines &lines)
+{
+    if (lines.size() < 3)
+        return {};
+    Linesf input;
+    input.reserve(lines.size());
+    for (const Line &line : lines)
+        input.emplace_back(line.a.cast<double>(), line.b.cast<double>());
+
+    auto    tree = AABBTreeLines::build_aabb_tree_over_indexed_lines(input);
+    Pointfs result;
+    for (size_t li = 0; li < lines.size() - 1; ++li) {
+        const Linef &l             = input[li];
+        auto         intersections = AABBTreeLines::get_intersections_with_line<false, Vec2d, Linef>(input, tree, l);
+        for (const auto &[p, node_index] : intersections) {
+            if (node_index - 1 <= li)
+                continue;
+            if (p == l.a || p == l.b)
+                continue;
+            result.push_back(p.cast<double>());
+        }
+    }
+    return result;
+}
+#endif // USE_AABB_TREE_FLOAT
+
+#ifdef USE_LINE_TO_LINE
+// FIXME O(n^2) complexity!
 Slic3r::Pointfs compute_intersections(const Slic3r::Lines &lines)
 {
     using namespace Slic3r;
@@ -123,53 +183,51 @@ Slic3r::Pointfs compute_intersections(const Slic3r::Lines &lines)
     Pointfs pts;
     Point   i;
     for (size_t li = 0; li < lines.size(); ++li) {
-        const Line  &l = lines[li];
+        const Line  &l  = lines[li];
-        const Point &a = l.a;
+        const Point &a  = l.a;
-        const Point &b = l.b;
+        const Point &b  = l.b;
-        Point min(std::min(a.x(), b.x()), std::min(a.y(), b.y()));
+        BoundingBox  bb({a, b});
-        Point max(std::max(a.x(), b.x()), std::max(a.y(), b.y()));
-        BoundingBox  bb(min, max);
         for (size_t li_ = li + 1; li_ < lines.size(); ++li_) {
             const Line  &l_ = lines[li_];
             const Point &a_ = l_.a;
             const Point &b_ = l_.b;
-            if (a == b_ || b == a_ || a == a_ || b == b_) continue;
+            // NOTE: Be Carefull - Not only neighbor has same point
-            Point min_(std::min(a_.x(), b_.x()), std::min(a_.y(), b_.y()));
+            if (a == b_ || b == a_ || a == a_ || b == b_)
-            Point max_(std::max(a_.x(), b_.x()), std::max(a_.y(), b_.y()));
+                continue;
-            BoundingBox bb_(min_, max_);
+            BoundingBox bb_({a_, b_});
             // intersect of BB compare min max
-            if (bb.overlap(bb_) &&
+            if (bb.overlap(bb_) && l.intersection(l_, &i))
-                l.intersection(l_, &i))
                 pts.push_back(i.cast<double>());
         }
     }
     return pts;
 }
-} // namespace priv
+#endif // USE_LINE_TO_LINE
+} // namespace
 
 Slic3r::Pointfs Slic3r::intersection_points(const Lines &lines)
 {
-    return priv::compute_intersections(lines);
+    return compute_intersections(lines);
 }
 
 Slic3r::Pointfs Slic3r::intersection_points(const Polygon &polygon)
 {
-    return priv::compute_intersections(to_lines(polygon));
+    return compute_intersections(to_lines(polygon));
 }
 
 Slic3r::Pointfs Slic3r::intersection_points(const Polygons &polygons)
 {
-    return priv::compute_intersections(to_lines(polygons));
+    return compute_intersections(to_lines(polygons));
 }
 
 Slic3r::Pointfs Slic3r::intersection_points(const ExPolygon &expolygon)
 {
-    return priv::compute_intersections(to_lines(expolygon));
+    return compute_intersections(to_lines(expolygon));
 }
 
 Slic3r::Pointfs Slic3r::intersection_points(const ExPolygons &expolygons)
 {
-    return priv::compute_intersections(to_lines(expolygons));
+    return compute_intersections(to_lines(expolygons));
 }
 
 #endif // USE_CGAL_SWEEP_LINE