From d0c08ec5c19e33d13ba8723393e8188207959406 Mon Sep 17 00:00:00 2001 From: PavelMikus Date: Thu, 12 May 2022 12:55:10 +0200 Subject: [PATCH] Feature: AABB tree for lines Small refactoring of AABB tree distance query function, to allow different primitives (apart from triangles) Implemented Distancer and functions to create AABB tree from lines and use closest point query Added test for the AABBTree with lines Added Benchmark comparing EdgeGrid with AABBTree on line contours (Inside AABBTree test file, disabled under compilation flag) --- src/libslic3r/AABBTreeIndirect.hpp | 143 +++++++------ src/libslic3r/AABBTreeLines.hpp | 112 ++++++++++ tests/libslic3r/test_aabbindirect.cpp | 285 ++++++++++++++++++++++++++ 3 files changed, 473 insertions(+), 67 deletions(-) create mode 100644 src/libslic3r/AABBTreeLines.hpp diff --git a/src/libslic3r/AABBTreeIndirect.hpp b/src/libslic3r/AABBTreeIndirect.hpp index 014b49af1..f2847579c 100644 --- a/src/libslic3r/AABBTreeIndirect.hpp +++ b/src/libslic3r/AABBTreeIndirect.hpp @@ -446,6 +446,57 @@ namespace detail { } } + // Real-time collision detection, Ericson, Chapter 5 + template + static inline Vector closest_point_to_triangle(const Vector &p, const Vector &a, const Vector &b, const Vector &c) + { + using Scalar = typename Vector::Scalar; + // Check if P in vertex region outside A + Vector ab = b - a; + Vector ac = c - a; + Vector ap = p - a; + Scalar d1 = ab.dot(ap); + Scalar d2 = ac.dot(ap); + if (d1 <= 0 && d2 <= 0) + return a; + // Check if P in vertex region outside B + Vector bp = p - b; + Scalar d3 = ab.dot(bp); + Scalar d4 = ac.dot(bp); + if (d3 >= 0 && d4 <= d3) + return b; + // Check if P in edge region of AB, if so return projection of P onto AB + Scalar vc = d1*d4 - d3*d2; + if (a != b && vc <= 0 && d1 >= 0 && d3 <= 0) { + Scalar v = d1 / (d1 - d3); + return a + v * ab; + } + // Check if P in vertex region outside C + Vector cp = p - c; + Scalar d5 = ab.dot(cp); + Scalar d6 = ac.dot(cp); + if (d6 >= 0 && d5 <= d6) + return c; + // Check if P in edge region of AC, if so return projection of P onto AC + Scalar vb = d5*d2 - d1*d6; + if (vb <= 0 && d2 >= 0 && d6 <= 0) { + Scalar w = d2 / (d2 - d6); + return a + w * ac; + } + // Check if P in edge region of BC, if so return projection of P onto BC + Scalar va = d3*d6 - d5*d4; + if (va <= 0 && (d4 - d3) >= 0 && (d5 - d6) >= 0) { + Scalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); + return b + w * (c - b); + } + // P inside face region. Compute Q through its barycentric coordinates (u,v,w) + Scalar denom = Scalar(1.0) / (va + vb + vc); + Scalar v = vb * denom; + Scalar w = vc * denom; + return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = 1.0-v-w + }; + + // Nothing to do with COVID-19 social distancing. template struct IndexedTriangleSetDistancer { @@ -453,74 +504,36 @@ namespace detail { using IndexedFaceType = AIndexedFaceType; using TreeType = ATreeType; using VectorType = AVectorType; + using ScalarType = typename VectorType::Scalar; const std::vector &vertices; const std::vector &faces; const TreeType &tree; const VectorType origin; + + inline VectorType closest_point_to_origin(size_t primitive_index, + ScalarType& squared_distance){ + const auto &triangle = this->faces[primitive_index]; + VectorType closest_point = closest_point_to_triangle(origin, + this->vertices[triangle(0)].template cast(), + this->vertices[triangle(1)].template cast(), + this->vertices[triangle(2)].template cast()); + squared_distance = (origin - closest_point).squaredNorm(); + return closest_point; + } }; - // Real-time collision detection, Ericson, Chapter 5 - template - static inline Vector closest_point_to_triangle(const Vector &p, const Vector &a, const Vector &b, const Vector &c) - { - using Scalar = typename Vector::Scalar; - // Check if P in vertex region outside A - Vector ab = b - a; - Vector ac = c - a; - Vector ap = p - a; - Scalar d1 = ab.dot(ap); - Scalar d2 = ac.dot(ap); - if (d1 <= 0 && d2 <= 0) - return a; - // Check if P in vertex region outside B - Vector bp = p - b; - Scalar d3 = ab.dot(bp); - Scalar d4 = ac.dot(bp); - if (d3 >= 0 && d4 <= d3) - return b; - // Check if P in edge region of AB, if so return projection of P onto AB - Scalar vc = d1*d4 - d3*d2; - if (a != b && vc <= 0 && d1 >= 0 && d3 <= 0) { - Scalar v = d1 / (d1 - d3); - return a + v * ab; - } - // Check if P in vertex region outside C - Vector cp = p - c; - Scalar d5 = ab.dot(cp); - Scalar d6 = ac.dot(cp); - if (d6 >= 0 && d5 <= d6) - return c; - // Check if P in edge region of AC, if so return projection of P onto AC - Scalar vb = d5*d2 - d1*d6; - if (vb <= 0 && d2 >= 0 && d6 <= 0) { - Scalar w = d2 / (d2 - d6); - return a + w * ac; - } - // Check if P in edge region of BC, if so return projection of P onto BC - Scalar va = d3*d6 - d5*d4; - if (va <= 0 && (d4 - d3) >= 0 && (d5 - d6) >= 0) { - Scalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); - return b + w * (c - b); - } - // P inside face region. Compute Q through its barycentric coordinates (u,v,w) - Scalar denom = Scalar(1.0) / (va + vb + vc); - Scalar v = vb * denom; - Scalar w = vc * denom; - return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = 1.0-v-w - }; - - template - static inline Scalar squared_distance_to_indexed_triangle_set_recursive( - IndexedTriangleSetDistancerType &distancer, + template + static inline Scalar squared_distance_to_indexed_primitives_recursive( + IndexedPrimitivesDistancerType &distancer, size_t node_idx, Scalar low_sqr_d, Scalar up_sqr_d, size_t &i, - Eigen::PlainObjectBase &c) + Eigen::PlainObjectBase &c) { - using Vector = typename IndexedTriangleSetDistancerType::VectorType; + using Vector = typename IndexedPrimitivesDistancerType::VectorType; if (low_sqr_d > up_sqr_d) return low_sqr_d; @@ -538,13 +551,9 @@ namespace detail { assert(node.is_valid()); if (node.is_leaf()) { - const auto &triangle = distancer.faces[node.idx]; - Vector c_candidate = closest_point_to_triangle( - distancer.origin, - distancer.vertices[triangle(0)].template cast(), - distancer.vertices[triangle(1)].template cast(), - distancer.vertices[triangle(2)].template cast()); - set_min((c_candidate - distancer.origin).squaredNorm(), node.idx, c_candidate); + Scalar sqr_dist; + Vector c_candidate = distancer.closest_point_to_origin(node.idx, sqr_dist); + set_min(sqr_dist, node.idx, c_candidate); } else { @@ -561,7 +570,7 @@ namespace detail { { size_t i_left; Vector c_left = c; - Scalar sqr_d_left = squared_distance_to_indexed_triangle_set_recursive(distancer, left_node_idx, low_sqr_d, up_sqr_d, i_left, c_left); + Scalar sqr_d_left = squared_distance_to_indexed_primitives_recursive(distancer, left_node_idx, low_sqr_d, up_sqr_d, i_left, c_left); set_min(sqr_d_left, i_left, c_left); looked_left = true; }; @@ -569,13 +578,13 @@ namespace detail { { size_t i_right; Vector c_right = c; - Scalar sqr_d_right = squared_distance_to_indexed_triangle_set_recursive(distancer, right_node_idx, low_sqr_d, up_sqr_d, i_right, c_right); + Scalar sqr_d_right = squared_distance_to_indexed_primitives_recursive(distancer, right_node_idx, low_sqr_d, up_sqr_d, i_right, c_right); set_min(sqr_d_right, i_right, c_right); looked_right = true; }; // must look left or right if in box - using BBoxScalar = typename IndexedTriangleSetDistancerType::TreeType::BoundingBox::Scalar; + using BBoxScalar = typename IndexedPrimitivesDistancerType::TreeType::BoundingBox::Scalar; if (node_left.bbox.contains(distancer.origin.template cast())) look_left(); if (node_right.bbox.contains(distancer.origin.template cast())) @@ -747,7 +756,7 @@ inline typename VectorType::Scalar squared_distance_to_indexed_triangle_set( auto distancer = detail::IndexedTriangleSetDistancer { vertices, faces, tree, point }; return tree.empty() ? Scalar(-1) : - detail::squared_distance_to_indexed_triangle_set_recursive(distancer, size_t(0), Scalar(0), std::numeric_limits::infinity(), hit_idx_out, hit_point_out); + detail::squared_distance_to_indexed_primitives_recursive(distancer, size_t(0), Scalar(0), std::numeric_limits::infinity(), hit_idx_out, hit_point_out); } // Decides if exists some triangle in defined radius on a 3D indexed triangle set using a pre-built AABBTreeIndirect::Tree. @@ -779,7 +788,7 @@ inline bool is_any_triangle_in_radius( return false; } - detail::squared_distance_to_indexed_triangle_set_recursive(distancer, size_t(0), Scalar(0), max_distance_squared, hit_idx, hit_point); + detail::squared_distance_to_indexed_primitives_recursive(distancer, size_t(0), Scalar(0), max_distance_squared, hit_idx, hit_point); return hit_point.allFinite(); } diff --git a/src/libslic3r/AABBTreeLines.hpp b/src/libslic3r/AABBTreeLines.hpp new file mode 100644 index 000000000..7b9595419 --- /dev/null +++ b/src/libslic3r/AABBTreeLines.hpp @@ -0,0 +1,112 @@ +#ifndef SRC_LIBSLIC3R_AABBTREELINES_HPP_ +#define SRC_LIBSLIC3R_AABBTREELINES_HPP_ + +#include "libslic3r/Point.hpp" +#include "libslic3r/EdgeGrid.hpp" +#include "libslic3r/AABBTreeIndirect.hpp" +#include "libslic3r/Line.hpp" + +namespace Slic3r { + +namespace AABBTreeLines { + +namespace detail { + +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 VectorType origin; + + inline VectorType closest_point_to_origin(size_t primitive_index, + ScalarType &squared_distance) { + VectorType nearest_point; + const LineType &line = lines[primitive_index]; + squared_distance = line_alg::distance_to_squared(line, origin, &nearest_point); + return nearest_point; + } +}; + +} + +// Build a balanced AABB Tree over a vector of float 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 float eps = 0) + { + using TreeType = AABBTreeIndirect::Tree<2, typename LineType::Scalar>; +// 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; + } + + size_t m_idx; + BoundingBox m_bbox; + 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; + n.m_centroid = (line.a + line.b) * 0.5; + 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); + } + + TreeType out; + out.build(std::move(input)); + return out; +} + +// Finding a closest line, its closest point and squared distance to the closest point +// Returns squared distance to the closest point or -1 if the input is empty. +template +inline typename VectorType::Scalar squared_distance_to_indexed_lines( + const std::vector &lines, + const TreeType &tree, + const VectorType &point, + size_t &hit_idx_out, + Eigen::PlainObjectBase &hit_point_out) + { + using Scalar = typename VectorType::Scalar; + auto distancer = detail::IndexedLinesDistancer + { lines, tree, point }; + return tree.empty() ? + Scalar(-1) : + AABBTreeIndirect::detail::squared_distance_to_indexed_primitives_recursive(distancer, size_t(0), Scalar(0), + std::numeric_limits::infinity(), hit_idx_out, hit_point_out); +} + +} + +} + +#endif /* SRC_LIBSLIC3R_AABBTREELINES_HPP_ */ diff --git a/tests/libslic3r/test_aabbindirect.cpp b/tests/libslic3r/test_aabbindirect.cpp index 3b834c442..87cf80943 100644 --- a/tests/libslic3r/test_aabbindirect.cpp +++ b/tests/libslic3r/test_aabbindirect.cpp @@ -3,6 +3,7 @@ #include #include +#include using namespace Slic3r; @@ -58,3 +59,287 @@ TEST_CASE("Building a tree over a box, ray caster and closest query", "[AABBIndi REQUIRE(closest_point.y() == Approx(0.5)); REQUIRE(closest_point.z() == Approx(1.)); } + +TEST_CASE("Creating a several 2d lines, testing closest point query", "[AABBIndirect]") +{ + std::vector lines { }; + lines.push_back(Linef(Vec2d(0.0, 0.0), Vec2d(1.0, 0.0))); + lines.push_back(Linef(Vec2d(1.0, 0.0), Vec2d(1.0, 1.0))); + lines.push_back(Linef(Vec2d(1.0, 1.0), Vec2d(0.0, 1.0))); + lines.push_back(Linef(Vec2d(0.0, 1.0), Vec2d(0.0, 0.0))); + + auto tree = AABBTreeLines::build_aabb_tree_over_indexed_lines(lines); + + size_t hit_idx_out; + Vec2d hit_point_out; + auto sqr_dist = AABBTreeLines::squared_distance_to_indexed_lines(lines, tree, Vec2d(0.0, 0.0), hit_idx_out, + hit_point_out); + REQUIRE(sqr_dist == Approx(0.0)); + REQUIRE((hit_idx_out == 0 || hit_idx_out == 3)); + REQUIRE(hit_point_out.x() == Approx(0.0)); + REQUIRE(hit_point_out.y() == Approx(0.0)); + + sqr_dist = AABBTreeLines::squared_distance_to_indexed_lines(lines, tree, Vec2d(1.5, 0.5), hit_idx_out, + hit_point_out); + REQUIRE(sqr_dist == Approx(0.25)); + REQUIRE(hit_idx_out == 1); + REQUIRE(hit_point_out.x() == Approx(1.0)); + REQUIRE(hit_point_out.y() == Approx(0.5)); +} + +#if 0 +#include "libslic3r/EdgeGrid.hpp" +#include +#include +#include +#include +TEST_CASE("AABBTreeLines vs SignedDistanceGrid time Benchmark", "[AABBIndirect]") +{ + std::vector lines { Points { } }; + std::vector linesf { }; + Vec2d prevf { }; + + // NOTE: max coord value of the lines is approx 83 mm + for (int r = 1; r < 1000; ++r) { + lines[0].push_back(Point::new_scale(Vec2d(exp(0.005f * r) * cos(r), exp(0.005f * r) * cos(r)))); + linesf.emplace_back(prevf, Vec2d(exp(0.005f * r) * cos(r), exp(0.005f * r) * cos(r))); + prevf = linesf.back().b; + } + + int build_num = 10000; + using namespace std::chrono; + { + std::cout << "building the tree " << build_num << " times..." << std::endl; + high_resolution_clock::time_point t1 = high_resolution_clock::now(); + for (int i = 0; i < build_num; ++i) { + volatile auto tree = AABBTreeLines::build_aabb_tree_over_indexed_lines(linesf); + } + high_resolution_clock::time_point t2 = high_resolution_clock::now(); + + duration time_span = duration_cast>(t2 - t1); + std::cout << "It took " << time_span.count() << " seconds." << std::endl << std::endl; + + } + { + std::cout << "building the grid res 1mm ONLY " << build_num/100 << " !!! times..." << std::endl; + high_resolution_clock::time_point t1 = high_resolution_clock::now(); + for (int i = 0; i < build_num/100; ++i) { + EdgeGrid::Grid grid { }; + grid.create(lines, scaled(1.0), true); + grid.calculate_sdf(); + } + high_resolution_clock::time_point t2 = high_resolution_clock::now(); + duration time_span = duration_cast>(t2 - t1); + std::cout << "It took " << time_span.count() << " seconds." << std::endl << std::endl; + } + { + std::cout << "building the grid res 10mm " << build_num << " times..." << std::endl; + high_resolution_clock::time_point t1 = high_resolution_clock::now(); + for (int i = 0; i < build_num; ++i) { + EdgeGrid::Grid grid { }; + grid.create(lines, scaled(10.0), true); + grid.calculate_sdf(); + } + high_resolution_clock::time_point t2 = high_resolution_clock::now(); + duration time_span = duration_cast>(t2 - t1); + std::cout << "It took " << time_span.count() << " seconds." << std::endl << std::endl; + } + + EdgeGrid::Grid grid10 { }; + grid10.create(lines, scaled(10.0), true); + coord_t query10_res = scaled(10.0); + grid10.calculate_sdf(); + + EdgeGrid::Grid grid1 { }; + grid1.create(lines, scaled(1.0), true); + coord_t query1_res = scaled(1.0); + grid1.calculate_sdf(); + + auto tree = AABBTreeLines::build_aabb_tree_over_indexed_lines(linesf); + + int query_num = 10000; + Points query_points { }; + std::vector query_pointsf { }; + for (int x = 0; x < query_num; ++x) { + Vec2d qp { rand() / (double(RAND_MAX) + 1.0f) * 200.0 - 100.0, rand() / (double(RAND_MAX) + 1.0f) * 200.0 + - 100.0 }; + query_pointsf.push_back(qp); + query_points.push_back(Point::new_scale(qp)); + } + + { + std::cout << "querying tree " << query_num << " times..." << std::endl; + high_resolution_clock::time_point t1 = high_resolution_clock::now(); + for (const Vec2d &qp : query_pointsf) { + size_t hit_idx_out; + Vec2d hit_point_out; + AABBTreeLines::squared_distance_to_indexed_lines(linesf, tree, qp, hit_idx_out, hit_point_out); + } + high_resolution_clock::time_point t2 = high_resolution_clock::now(); + duration time_span = duration_cast>(t2 - t1); + std::cout << "It took " << time_span.count() << " seconds." << std::endl << std::endl; + } + + { + std::cout << "querying grid res 1mm " << query_num << " times..." << std::endl; + high_resolution_clock::time_point t1 = high_resolution_clock::now(); + for (const Point &qp : query_points) { + volatile auto dist = grid1.closest_point_signed_distance(qp, query1_res); + } + high_resolution_clock::time_point t2 = high_resolution_clock::now(); + duration time_span = duration_cast>(t2 - t1); + std::cout << "It took " << time_span.count() << " seconds." << std::endl << std::endl; + } + + { + std::cout << "querying grid res 10mm " << query_num << " times..." << std::endl; + high_resolution_clock::time_point t1 = high_resolution_clock::now(); + for (const Point &qp : query_points) { + volatile auto dist = grid10.closest_point_signed_distance(qp, query10_res); + } + high_resolution_clock::time_point t2 = high_resolution_clock::now(); + duration time_span = duration_cast>(t2 - t1); + std::cout << "It took " << time_span.count() << " seconds." << std::endl << std::endl; + } + + std::cout << "Test build and queries together - same number of contour points and query points" << std::endl << std::endl; + + std::vector point_counts { 100, 300, 500, 1000, 3000 }; + for (auto count : point_counts) { + + std::vector lines { Points { } }; + std::vector linesf { }; + Vec2d prevf { }; + Points query_points { }; + std::vector query_pointsf { }; + + for (int x = 0; x < count; ++x) { + Vec2d cp { rand() / (double(RAND_MAX) + 1.0f) * 200.0 - 100.0, rand() / (double(RAND_MAX) + 1.0f) * 200.0 + - 100.0 }; + lines[0].push_back(Point::new_scale(cp)); + linesf.emplace_back(prevf, cp); + prevf = linesf.back().b; + + Vec2d qp { rand() / (double(RAND_MAX) + 1.0f) * 200.0 - 100.0, rand() / (double(RAND_MAX) + 1.0f) * 200.0 + - 100.0 }; + query_pointsf.push_back(qp); + query_points.push_back(Point::new_scale(qp)); + } + + std::cout << "Test for point count: " << count << std::endl; + { + high_resolution_clock::time_point t1 = high_resolution_clock::now(); + auto tree = AABBTreeLines::build_aabb_tree_over_indexed_lines(linesf); + for (const Vec2d &qp : query_pointsf) { + size_t hit_idx_out; + Vec2d hit_point_out; + AABBTreeLines::squared_distance_to_indexed_lines(linesf, tree, qp, hit_idx_out, hit_point_out); + } + high_resolution_clock::time_point t2 = high_resolution_clock::now(); + duration time_span = duration_cast>(t2 - t1); + std::cout << " Tree took " << time_span.count() << " seconds." << std::endl; + } + + { + high_resolution_clock::time_point t1 = high_resolution_clock::now(); + EdgeGrid::Grid grid1 { }; + grid1.create(lines, scaled(1.0), true); + coord_t query1_res = scaled(1.0); + grid1.calculate_sdf(); + for (const Point &qp : query_points) { + volatile auto dist = grid1.closest_point_signed_distance(qp, query1_res); + } + high_resolution_clock::time_point t2 = high_resolution_clock::now(); + duration time_span = duration_cast>(t2 - t1); + std::cout << " Grid 1mm took " << time_span.count() << " seconds." << std::endl; + } + + { + high_resolution_clock::time_point t1 = high_resolution_clock::now(); + EdgeGrid::Grid grid10 { }; + grid10.create(lines, scaled(10.0), true); + coord_t query10_res = scaled(10.0); + grid10.calculate_sdf(); + for (const Point &qp : query_points) { + volatile auto dist = grid10.closest_point_signed_distance(qp, query10_res); + } + high_resolution_clock::time_point t2 = high_resolution_clock::now(); + duration time_span = duration_cast>(t2 - t1); + std::cout << " Grid 10mm took " << time_span.count() << " seconds." << std::endl; + } + + } + + + + std::cout << "Test build and queries together - same number of contour points and query points" << std::endl << + "And with limited contour edge length to 4mm " << std::endl; + for (auto count : point_counts) { + + std::vector lines { Points { } }; + std::vector linesf { }; + Vec2d prevf { }; + Points query_points { }; + std::vector query_pointsf { }; + + for (int x = 0; x < count; ++x) { + Vec2d cp { rand() / (double(RAND_MAX) + 1.0f) * 200.0 - 100.0, rand() / (double(RAND_MAX) + 1.0f) * 200.0 + - 100.0 }; + Vec2d contour = prevf + cp.normalized()*4.0; // limits the cnotour edge len to 4mm + lines[0].push_back(Point::new_scale(contour)); + linesf.emplace_back(prevf, contour); + prevf = linesf.back().b; + + Vec2d qp { rand() / (double(RAND_MAX) + 1.0f) * 200.0 - 100.0, rand() / (double(RAND_MAX) + 1.0f) * 200.0 + - 100.0 }; + query_pointsf.push_back(qp); + query_points.push_back(Point::new_scale(qp)); + } + + std::cout << "Test for point count: " << count << std::endl; + { + high_resolution_clock::time_point t1 = high_resolution_clock::now(); + auto tree = AABBTreeLines::build_aabb_tree_over_indexed_lines(linesf); + for (const Vec2d &qp : query_pointsf) { + size_t hit_idx_out; + Vec2d hit_point_out; + AABBTreeLines::squared_distance_to_indexed_lines(linesf, tree, qp, hit_idx_out, hit_point_out); + } + high_resolution_clock::time_point t2 = high_resolution_clock::now(); + duration time_span = duration_cast>(t2 - t1); + std::cout << " Tree took " << time_span.count() << " seconds." << std::endl; + } + + { + high_resolution_clock::time_point t1 = high_resolution_clock::now(); + EdgeGrid::Grid grid1 { }; + grid1.create(lines, scaled(1.0), true); + coord_t query1_res = scaled(1.0); + grid1.calculate_sdf(); + for (const Point &qp : query_points) { + volatile auto dist = grid1.closest_point_signed_distance(qp, query1_res); + } + high_resolution_clock::time_point t2 = high_resolution_clock::now(); + duration time_span = duration_cast>(t2 - t1); + std::cout << " Grid 1mm took " << time_span.count() << " seconds." << std::endl; + } + + { + high_resolution_clock::time_point t1 = high_resolution_clock::now(); + EdgeGrid::Grid grid10 { }; + grid10.create(lines, scaled(10.0), true); + coord_t query10_res = scaled(10.0); + grid10.calculate_sdf(); + for (const Point &qp : query_points) { + volatile auto dist = grid10.closest_point_signed_distance(qp, query10_res); + } + high_resolution_clock::time_point t2 = high_resolution_clock::now(); + duration time_span = duration_cast>(t2 - t1); + std::cout << " Grid 10mm took " << time_span.count() << " seconds." << std::endl; + } + + } + +} +#endif +