From 55e0f2dd83ac5e5f66ef639d7f3c6bc7bdcad6d7 Mon Sep 17 00:00:00 2001 From: PavelMikus Date: Tue, 1 Feb 2022 16:17:32 +0100 Subject: [PATCH] refactoring raycaster, gathering only hitpoints and their normals. --- src/libslic3r/GCode.cpp | 10 +- src/libslic3r/GCode/SeamPlacerNG.cpp | 345 +++++++++++++++------------ src/libslic3r/GCode/SeamPlacerNG.hpp | 80 ++++--- src/libslic3r/KDTreeIndirect.hpp | 53 +++- 4 files changed, 286 insertions(+), 202 deletions(-) diff --git a/src/libslic3r/GCode.cpp b/src/libslic3r/GCode.cpp index 3d0ca5069b..5f312183b7 100644 --- a/src/libslic3r/GCode.cpp +++ b/src/libslic3r/GCode.cpp @@ -2586,7 +2586,7 @@ std::string GCode::extrude_loop(ExtrusionLoop loop, std::string description, dou loop.split_at(last_pos, false); } else - m_seam_placer.place_seam(loop, this->last_pos(), m_config.external_perimeters_first, + m_seam_placer.place_seam(m_layer->object(), loop, m_layer->slice_z, this->last_pos(), m_config.external_perimeters_first, EXTRUDER_CONFIG(nozzle_diameter), lower_layer_edge_grid ? lower_layer_edge_grid->get() : nullptr); // clip the path to avoid the extruder to get exactly on the first point of the loop; @@ -2715,10 +2715,10 @@ std::string GCode::extrude_perimeters(const Print &print, const std::vectorlower_layer && ! lower_layer_edge_grid) lower_layer_edge_grid = calculate_layer_edge_grid(*m_layer->lower_layer); - m_seam_placer.plan_perimeters(std::vector(region.perimeters.begin(), region.perimeters.end()), - *m_layer, m_config.seam_position, this->last_pos(), EXTRUDER_CONFIG(nozzle_diameter), - (m_layer == NULL ? nullptr : m_layer->object()), - (lower_layer_edge_grid ? lower_layer_edge_grid.get() : nullptr)); +// m_seam_placer.plan_perimeters(std::vector(region.perimeters.begin(), region.perimeters.end()), +// *m_layer, m_config.seam_position, this->last_pos(), EXTRUDER_CONFIG(nozzle_diameter), +// (m_layer == NULL ? nullptr : m_layer->object()), +// (lower_layer_edge_grid ? lower_layer_edge_grid.get() : nullptr)); for (const ExtrusionEntity* ee : region.perimeters) gcode += this->extrude_entity(*ee, "perimeter", -1., &lower_layer_edge_grid); diff --git a/src/libslic3r/GCode/SeamPlacerNG.cpp b/src/libslic3r/GCode/SeamPlacerNG.cpp index 0b179797f7..a93ac299ab 100644 --- a/src/libslic3r/GCode/SeamPlacerNG.cpp +++ b/src/libslic3r/GCode/SeamPlacerNG.cpp @@ -6,7 +6,6 @@ #include #include #include -#include #include "libslic3r/ExtrusionEntity.hpp" #include "libslic3r/Print.hpp" @@ -16,7 +15,7 @@ #include "libslic3r/SVG.hpp" #include "libslic3r/Layer.hpp" -//TODO remove +// TODO remove #include namespace Slic3r { @@ -24,60 +23,58 @@ namespace Slic3r { namespace SeamPlacerImpl { /// Coordinate frame -class Frame -{ +class Frame { public: - Frame() - { + Frame() { mX = Vec3d(1, 0, 0); mY = Vec3d(0, 1, 0); mZ = Vec3d(0, 0, 1); } - Frame(const Vec3d &x, const Vec3d &y, const Vec3d &z) - : mX(x), mY(y), mZ(z) - {} - - void set_from_z(const Vec3d &z) - { - mZ = z.normalized(); - Vec3d tmpZ = mZ; - Vec3d tmpX = (std::abs(tmpZ.x()) > 0.99f) ? Vec3d(0, 1, 0) : - Vec3d(1, 0, 0); - mY = (tmpZ.cross(tmpX)).normalized(); - mX = mY.cross(tmpZ); + Frame(const Vec3d &x, const Vec3d &y, const Vec3d &z) : + mX(x), mY(y), mZ(z) { } - Vec3d to_world(const Vec3d &a) const - { + void set_from_z(const Vec3d &z) { + mZ = z.normalized(); + Vec3d tmpZ = mZ; + Vec3d tmpX = (std::abs(tmpZ.x()) > 0.99f) ? Vec3d(0, 1, 0) : Vec3d(1, 0, 0); + mY = (tmpZ.cross(tmpX)).normalized(); + mX = mY.cross(tmpZ); + } + + Vec3d to_world(const Vec3d &a) const { return a.x() * mX + a.y() * mY + a.z() * mZ; } - Vec3d to_local(const Vec3d &a) const - { + Vec3d to_local(const Vec3d &a) const { return Vec3d(mX.dot(a), mY.dot(a), mZ.dot(a)); } - const Vec3d &binormal() const { return mX; } + const Vec3d& binormal() const { + return mX; + } - const Vec3d &tangent() const { return mY; } + const Vec3d& tangent() const { + return mY; + } - const Vec3d &normal() const { return mZ; } + const Vec3d& normal() const { + return mZ; + } private: Vec3d mX, mY, mZ; }; -Vec3d sample_sphere_uniform(const Vec2f &samples) -{ +Vec3d sample_sphere_uniform(const Vec2f &samples) { float term_one = 2.0f * M_PIf32 * samples.x(); float term_two = 2.0f * sqrt(samples.y() - samples.y() * samples.y()); return {cos(term_one) * term_two, sin(term_one) * term_two, - 1.0f - 2.0f * samples.y()}; + 1.0f - 2.0f * samples.y()}; } -Vec3d sample_power_cosine_hemisphere(const Vec2f &samples, float power) -{ +Vec3d sample_power_cosine_hemisphere(const Vec2f &samples, float power) { const float term1 = 2.f * M_PIf32 * samples.x(); const float term2 = pow(samples.y(), 1.f / (power + 1.f)); const float term3 = sqrt(1.f - term2 * term2); @@ -85,182 +82,220 @@ Vec3d sample_power_cosine_hemisphere(const Vec2f &samples, float power) return Vec3d(cos(term1) * term3, sin(term1) * term3, term2); } -void raycast_visibility( - size_t ray_count, - const AABBTreeIndirect::Tree<3, float> &raycasting_tree, - const indexed_triangle_set &triangles, - const KDTreeIndirect<3, coordf_t, KDTreeCoordinateFunctor> - &perimeter_points_tree, - std::vector &perimeter_points) -{ - auto bbox = raycasting_tree.node(0).bbox; +void resolve_geometry_hit(const igl::Hit &hitpoint, + const indexed_triangle_set &triangles, + const KDTreeIndirect<3, coordf_t, KDTreeCoordinateFunctor> &perimeter_points_tree, + const std::vector &perimeter_points, + std::vector> &visibility_counters) { + auto face = triangles.indices[hitpoint.id]; + auto edge1 = triangles.vertices[face[1]] - triangles.vertices[face[0]]; + auto edge2 = triangles.vertices[face[2]] - triangles.vertices[face[0]]; + + Vec3d hit_pos = (triangles.vertices[face[0]] + edge1 * hitpoint.u + edge2 * hitpoint.v).cast(); + auto perimeter_point_index = find_closest_point(perimeter_points_tree, hit_pos); + auto perimeter_point_pos = perimeter_points[perimeter_point_index].position; + auto dist_squared = (perimeter_point_pos - hit_pos).squaredNorm(); + + visibility_counters[perimeter_point_index].fetch_add(1.0 / dist_squared, std::memory_order_relaxed); +} + +std::vector raycast_visibility(size_t ray_count, + const AABBTreeIndirect::Tree<3, float> &raycasting_tree, + const indexed_triangle_set &triangles) { + auto bbox = raycasting_tree.node(0).bbox; Vec3d vision_sphere_center = bbox.center().cast(); - float vision_sphere_raidus = (bbox.sizes().maxCoeff() * - 0.55); // 0.5 (half) covers whole object, - // 0.05 added to avoid corner cases + float vision_sphere_raidus = (bbox.sizes().maxCoeff() * 0.55); // 0.5 (half) covers whole object, + // 0.05 added to avoid corner cases // Prepare random samples per ray - std::random_device rnd_device; - std::mt19937 mersenne_engine{rnd_device()}; - std::uniform_real_distribution dist{0, 1}; + std::random_device rnd_device; + std::mt19937 mersenne_engine { rnd_device() }; + std::uniform_real_distribution dist { 0, 1 }; auto gen = [&dist, &mersenne_engine]() { return Vec2f(dist(mersenne_engine), dist(mersenne_engine)); }; - BOOST_LOG_TRIVIAL(debug) << "PM: generate random samples: start"; + BOOST_LOG_TRIVIAL(debug) + << "PM: generate random samples: start"; std::vector global_dir_random_samples(ray_count); - generate(begin(global_dir_random_samples), end(global_dir_random_samples), - gen); + generate(begin(global_dir_random_samples), end(global_dir_random_samples), gen); std::vector local_dir_random_samples(ray_count); - generate(begin(local_dir_random_samples), end(local_dir_random_samples), - gen); - - BOOST_LOG_TRIVIAL(debug) << "PM: generate random samples: end"; - - std::vector> visibility_counters( - perimeter_points.size()); + generate(begin(local_dir_random_samples), end(local_dir_random_samples), gen); BOOST_LOG_TRIVIAL(debug) - << "PM: raycast visibility for " << ray_count << " rays: start"; + << "PM: generate random samples: end"; + + BOOST_LOG_TRIVIAL(debug) + << "PM: raycast visibility for " << ray_count << " rays: start"; // raycast visibility - tbb::parallel_for( - tbb::blocked_range(0, ray_count), - [&](tbb::blocked_range r) { - for (size_t index = r.begin(); index < r.end(); ++index) { - Vec3d global_ray_dir = sample_sphere_uniform( - global_dir_random_samples[index]); - Vec3d ray_origin = (vision_sphere_center - - global_ray_dir * vision_sphere_raidus); - Vec3d local_dir = sample_power_cosine_hemisphere( - local_dir_random_samples[index], 1.0); + std::vector hit_points = tbb::parallel_reduce(tbb::blocked_range(0, ray_count), + std::vector { }, + [&](tbb::blocked_range r, std::vector init) { + for (size_t index = r.begin(); index < r.end(); ++index) { + Vec3d global_ray_dir = sample_sphere_uniform(global_dir_random_samples[index]); + Vec3d ray_origin = (vision_sphere_center - global_ray_dir * vision_sphere_raidus); + Vec3d local_dir = sample_power_cosine_hemisphere(local_dir_random_samples[index], 1.5); - Frame f; - f.set_from_z(global_ray_dir); - Vec3d final_ray_dir = (f.to_world(local_dir)); + Frame f; + f.set_from_z(global_ray_dir); + Vec3d final_ray_dir = (f.to_world(local_dir)); - igl::Hit hitpoint; - // FIXME: This query will not compile for float ray origin and - // direction for some reason - auto hit = AABBTreeIndirect::intersect_ray_first_hit( - triangles.vertices, triangles.indices, raycasting_tree, - ray_origin, final_ray_dir, hitpoint); + igl::Hit hitpoint; + // FIXME: This query will not compile for float ray origin and + // direction for some reason + auto hit = AABBTreeIndirect::intersect_ray_first_hit(triangles.vertices, + triangles.indices, raycasting_tree, ray_origin, final_ray_dir, hitpoint); - if (hit) { - auto face = triangles.indices[hitpoint.id]; - auto edge1 = triangles.vertices[face[1]] - - triangles.vertices[face[0]]; - auto edge2 = triangles.vertices[face[2]] - - triangles.vertices[face[0]]; + if (hit) { + auto face = triangles.indices[hitpoint.id]; + auto edge1 = triangles.vertices[face[1]] - triangles.vertices[face[0]]; + auto edge2 = triangles.vertices[face[2]] - triangles.vertices[face[0]]; - Vec3d hit_pos = (triangles.vertices[face[0]] + - edge1 * hitpoint.u + edge2 * hitpoint.v) - .cast(); + Vec3d hit_pos = (triangles.vertices[face[0]] + edge1 * hitpoint.u + edge2 * hitpoint.v).cast(); + Vec3d surface_normal = edge1.cross(edge2).cast(); - auto perimeter_point_index = - find_closest_point(perimeter_points_tree, hit_pos); - - visibility_counters[perimeter_point_index] - .fetch_add(1, std::memory_order_relaxed); + init.push_back(HitInfo { hit_pos, surface_normal }); + } } + return init; + }, + [](std::vector left, std::vector right) { + left.insert(left.end(), right.begin(), right.end()); + return left; } - }); + ); BOOST_LOG_TRIVIAL(debug) - << "PM: raycast visibility for " << ray_count << " rays: end"; - - BOOST_LOG_TRIVIAL(debug) - << "PM: assign visibility to perimenter points : start"; - - tbb::parallel_for(tbb::blocked_range(0, perimeter_points.size()), - [&](tbb::blocked_range r) { - for (size_t index = r.begin(); index < r.end(); - ++index) { - perimeter_points[index].visibility = - visibility_counters[index]; - } - }); - BOOST_LOG_TRIVIAL(debug) - << "PM: assign visibility to perimenter points : end"; + << "PM: raycast visibility for " << ray_count << " rays: end"; +//TODO disable, only debug code +//#ifdef 0 its_write_obj(triangles, "triangles.obj"); Slic3r::CNumericLocalesSetter locales_setter; FILE *fp = boost::nowide::fopen("perimeter.obj", "w"); if (fp == nullptr) { - BOOST_LOG_TRIVIAL(error) << "Couldn't open " - << "perimeter.obj" - << " for writing"; + BOOST_LOG_TRIVIAL(error) + << "Couldn't open " << "perimeter.obj" << " for writing"; } - for (size_t i = 0; i < perimeter_points.size(); ++i) - fprintf(fp, "v %f %f %f %zu\n", perimeter_points[i].position[0], - perimeter_points[i].position[1], - perimeter_points[i].position[2], - perimeter_points[i].visibility); + for (size_t i = 0; i < hit_points.size(); ++i) + fprintf(fp, "v %f %f %f \n", hit_points[i].m_position[0], hit_points[i].m_position[1], + hit_points[i].m_position[2]); fclose(fp); +//#endif + + return hit_points; } } // namespace SeamPlacerImpl -void SeamPlacer::init(const Print &print) -{ +void SeamPlacer::init(const Print &print) { using namespace SeamPlacerImpl; - seam_candidates_trees.clear(); + m_perimeter_points_trees_per_object.clear(); + m_perimeter_points_per_object.clear(); for (const PrintObject *po : print.objects()) { BOOST_LOG_TRIVIAL(debug) - << "PM: build AABB tree for raycasting: start"; + << "PM: build AABB tree for raycasting: start"; // Build AABB tree for raycasting auto obj_transform = po->trafo_centered(); - auto triangle_set = po->model_object()->raw_indexed_triangle_set(); + auto triangle_set = po->model_object()->raw_indexed_triangle_set(); its_transform(triangle_set, obj_transform); - auto raycasting_tree = - AABBTreeIndirect::build_aabb_tree_over_indexed_triangle_set( - triangle_set.vertices, triangle_set.indices); - - BOOST_LOG_TRIVIAL(debug) << "PM: build AABB tree for raycasting: end"; + auto raycasting_tree = AABBTreeIndirect::build_aabb_tree_over_indexed_triangle_set(triangle_set.vertices, + triangle_set.indices); BOOST_LOG_TRIVIAL(debug) - << "PM: gather and build KD tree with seam candidates: start"; + << "PM: build AABB tree for raycasting: end"; + + std::vector hit_points = raycast_visibility(ray_count_per_object, raycasting_tree, triangle_set); + HitInfoCoordinateFunctor hit_points_functor { &hit_points }; + KDTreeIndirect<3, coordf_t, HitInfoCoordinateFunctor> hit_points_tree { hit_points_functor, hit_points.size() }; + + BOOST_LOG_TRIVIAL(debug) + << "PM: gather and build KD tree with seam candidates: start"; // gather seam candidates (perimeter points) - auto seam_candidates = tbb::parallel_reduce( - tbb::blocked_range(0, po->layers().size()), - std::vector{}, - [&](tbb::blocked_range r, - std::vector init) { - for (size_t index = r.begin(); index < r.end(); ++index) { - const auto layer = po->layers()[index]; - auto unscaled_z = layer->slice_z; - for (Points points : layer->lslices) { - for (Point point : points) { - auto unscaled_p = unscale(point); - auto unscaled_position = Vec3d{unscaled_p.x(), - unscaled_p.y(), - unscaled_z}; - init.emplace_back(unscaled_position); + m_perimeter_points_per_object[po] = tbb::parallel_reduce(tbb::blocked_range(0, po->layers().size()), + std::vector { }, [&](tbb::blocked_range r, std::vector init) { + for (size_t index = r.begin(); index < r.end(); ++index) { + const auto layer = po->layers()[index]; + auto unscaled_z = layer->slice_z; + for (const ExPolygon &expoly : layer->lslices) { + // Contour - insert first point marked as Polygon + // start, then insert rest sequentially. + { + auto unscaled_p = unscale(expoly.contour[0]); + init.emplace_back(Vec3d { unscaled_p.x(), unscaled_p.y(), unscaled_z }, true); + } + for (size_t index = 1; index < expoly.contour.size(); ++index) { + auto unscaled_p = unscale(expoly.contour[index]); + init.emplace_back(Vec3d { unscaled_p.x(), unscaled_p.y(), unscaled_z }); + } + + for (const Polygon &hole : expoly.holes) { + // Perform the same for each hole + { + auto unscaled_p = unscale(hole[0]); + init.emplace_back(Vec3d { unscaled_p.x(), unscaled_p.y(), unscaled_z }, true); + } + for (size_t index = 1; index < hole.size(); ++index) { + auto unscaled_p = unscale(hole[index]); + init.emplace_back(Vec3d { unscaled_p.x(), unscaled_p.y(), unscaled_z }); + } + } } } - } - return init; - }, - [](std::vector prev, - std::vector next) { - prev.insert(prev.end(), next.begin(), next.end()); - return prev; - }); + return init; + }, + [](std::vector prev, std::vector next) { + prev.insert(prev.end(), next.begin(), next.end()); + return prev; + }); + auto &perimeter_points = m_perimeter_points_per_object[po]; // Build KD tree with seam candidates - auto functor = KDTreeCoordinateFunctor{&seam_candidates}; - auto perimeter_points_tree = KDTreeIndirect< - 3, coordf_t, KDTreeCoordinateFunctor>{functor, - seam_candidates.size()}; + auto functor = KDTreeCoordinateFunctor { &perimeter_points }; + m_perimeter_points_trees_per_object.emplace(std::piecewise_construct, std::forward_as_tuple(po), + std::forward_as_tuple(functor, m_perimeter_points_per_object[po].size())); + SeamPlacer::PointTree &perimeter_points_tree = m_perimeter_points_trees_per_object.find(po)->second; BOOST_LOG_TRIVIAL(debug) - << "PM: gather and build KD tree with seam candidates: end"; - - raycast_visibility(100000, raycasting_tree, triangle_set, - perimeter_points_tree, seam_candidates); + << "PM: gather and build KD tree with seam candidates: end"; } } + +void SeamPlacer::place_seam(const PrintObject *po, ExtrusionLoop &loop, coordf_t unscaled_z, const Point &last_pos, + bool external_first, + double nozzle_diameter, const EdgeGrid::Grid *lower_layer_edge_grid) { + assert(m_perimeter_points_trees_per_object.find(po) != nullptr); + assert(m_perimeter_points_per_object.find(po) != nullptr); + const auto &perimeter_points_tree = m_perimeter_points_trees_per_object.find(po)->second; + const auto &perimeter_points = m_perimeter_points_per_object.find(po)->second; + + Points loop_points { }; + loop.collect_points(loop_points); + + // vector of pairs: first-> index into perimeter points, second-> index into loop points + auto closest_perimeter_point_indices = std::vector>(loop_points.size()); + for (size_t p_index = 0; p_index < loop_points.size(); ++p_index) { + auto unscaled_p = unscale(loop_points[p_index]); + closest_perimeter_point_indices.emplace_back(find_closest_point(perimeter_points_tree, + Vec3d { unscaled_p.x(), unscaled_p.y(), unscaled_z }), p_index); + } + + std::sort(closest_perimeter_point_indices.begin(), closest_perimeter_point_indices.end(), + [&](const std::pair left, const std::pair right) { + return perimeter_points[left.first].visibility < perimeter_points[right.first].visibility; + }); + + loop.split_at_vertex(loop_points[closest_perimeter_point_indices[0].second]); +// +// Point seam = last_pos; +// if (!loop.split_at_vertex(seam)) +// // The point is not in the original loop. +// // Insert it. +// loop.split_at(seam, true); +} + } // namespace Slic3r diff --git a/src/libslic3r/GCode/SeamPlacerNG.hpp b/src/libslic3r/GCode/SeamPlacerNG.hpp index 75c394213d..feeedaec8b 100644 --- a/src/libslic3r/GCode/SeamPlacerNG.hpp +++ b/src/libslic3r/GCode/SeamPlacerNG.hpp @@ -24,57 +24,59 @@ class Grid; namespace SeamPlacerImpl { -struct SeamCandidate -{ - SeamCandidate(const Vec3d &pos) : position(pos), visibility(0) {} - // TODO is there some equivalent for Vec with coordf_t type? - Vec3d position; - size_t visibility; +struct SeamCandidate { + explicit SeamCandidate(const Vec3d &pos) : + position(pos), visibility(0.0), polygon_start(false) { + } + SeamCandidate(const Vec3d &pos, bool polygon_start) : + position(pos), visibility(0.0), polygon_start(polygon_start) { + } + Vec3d position; + float visibility; + bool polygon_start; }; -struct KDTreeCoordinateFunctor -{ - KDTreeCoordinateFunctor(std::vector *seam_candidates) - : seam_candidates(seam_candidates) - {} +struct HitInfo { + Vec3d m_position; + Vec3d m_surface_normal; +}; + + +struct KDTreeCoordinateFunctor { + KDTreeCoordinateFunctor(std::vector *seam_candidates) : + seam_candidates(seam_candidates) { + } std::vector *seam_candidates; - float operator()(size_t index, size_t dim) const - { + float operator()(size_t index, size_t dim) const { return seam_candidates->operator[](index).position[dim]; } }; + +struct HitInfoCoordinateFunctor { + HitInfoCoordinateFunctor(std::vector *hit_points) : + m_hit_points(hit_points) { + } + std::vector *m_hit_points; + float operator()(size_t index, size_t dim) const { + return m_hit_points->operator[](index).m_position[dim]; + } +}; } // namespace SeamPlacerImpl -class SeamPlacer -{ +class SeamPlacer { + using PointTree = + KDTreeIndirect<3, coordf_t, SeamPlacerImpl::KDTreeCoordinateFunctor>; + const size_t ray_count_per_object = 100000; + public: - std::vector< - KDTreeIndirect<3, coordf_t, SeamPlacerImpl::KDTreeCoordinateFunctor>> - seam_candidates_trees; + std::unordered_map m_perimeter_points_trees_per_object; + std::unordered_map> m_perimeter_points_per_object; void init(const Print &print); - void plan_perimeters(const std::vector perimeters, - const Layer &layer, - SeamPosition seam_position, - Point last_pos, - coordf_t nozzle_dmr, - const PrintObject *po, - const EdgeGrid::Grid *lower_layer_edge_grid) - {} - - void place_seam(ExtrusionLoop &loop, - const Point &last_pos, - bool external_first, - double nozzle_diameter, - const EdgeGrid::Grid *lower_layer_edge_grid) - { - Point seam = last_pos; - if (!loop.split_at_vertex(seam)) - // The point is not in the original loop. - // Insert it. - loop.split_at(seam, true); - } + void place_seam(const PrintObject *po, ExtrusionLoop &loop, coordf_t unscaled_z, const Point &last_pos, + bool external_first, + double nozzle_diameter, const EdgeGrid::Grid *lower_layer_edge_grid); }; } // namespace Slic3r diff --git a/src/libslic3r/KDTreeIndirect.hpp b/src/libslic3r/KDTreeIndirect.hpp index 12e462569e..38e020e1fb 100644 --- a/src/libslic3r/KDTreeIndirect.hpp +++ b/src/libslic3r/KDTreeIndirect.hpp @@ -59,7 +59,7 @@ public: CONTINUE_RIGHT = 2, STOP = 4, }; - template + template unsigned int descent_mask(const CoordType &point_coord, const CoordType &search_radius, size_t idx, size_t dimension) const { CoordType dist = point_coord - this->coordinate(idx, dimension); @@ -110,8 +110,8 @@ private: // Partition the input m_nodes at "k" and "dimension" using the QuickSelect method: // https://en.wikipedia.org/wiki/Quickselect - // Items left of the k'th item are lower than the k'th item in the "dimension", - // items right of the k'th item are higher than the k'th item in the "dimension", + // Items left of the k'th item are lower than the k'th item in the "dimension", + // items right of the k'th item are higher than the k'th item in the "dimension", void partition_input(std::vector &input, const size_t dimension, size_t left, size_t right, const size_t k) const { while (left < right) { @@ -231,6 +231,53 @@ size_t find_closest_point(const KDTreeIndirectType& kdtree, const PointType& poi return find_closest_point(kdtree, point, [](size_t) { return true; }); } +// Find a nearby points (spherical neighbourhood) using Euclidian metrics. +template +std::vector find_nearby_points(const KDTreeIndirectType &kdtree, const PointType ¢er, + typename KDTreeIndirectType::CoordType& max_distance, FilterFn filter) + { + using CoordType = typename KDTreeIndirectType::CoordType; + + struct Visitor { + const KDTreeIndirectType &kdtree; + const PointType ¢er; + const CoordType &max_distance_squared; + const FilterFn filter; + std::vector result; + + Visitor(const KDTreeIndirectType &kdtree, const PointType ¢er, const CoordType &max_distance, + FilterFn filter) : + kdtree(kdtree), center(center), max_distance_squared(max_distance*max_distance), filter(filter) { + } + unsigned int operator()(size_t idx, size_t dimension) { + if (this->filter(idx)) { + auto dist = CoordType(0); + for (size_t i = 0; i < KDTreeIndirectType::NumDimensions; ++i) { + CoordType d = center[i] - kdtree.coordinate(idx, i); + dist += d * d; + } + if (dist < max_distance_squared) { + result.push_back(idx); + } + } + return kdtree.descent_mask(center[dimension], max_distance_squared, idx, dimension); + } + } visitor(kdtree, center, max_distance, filter); + + kdtree.visit(visitor); + return visitor.result; +} + +template +std::vector find_nearby_points(const KDTreeIndirectType &kdtree, const PointType ¢er, + typename KDTreeIndirectType::CoordType& max_distance) + { + return find_nearby_points(kdtree, center, max_distance, [](size_t) { + return true; + }); +} + + } // namespace Slic3r #endif /* slic3r_KDTreeIndirect_hpp_ */