diff --git a/resources/profiles/Voron.idx b/resources/profiles/Voron.idx
index a593d2fb4b..a169eb751e 100644
--- a/resources/profiles/Voron.idx
+++ b/resources/profiles/Voron.idx
@@ -1,2 +1,4 @@
+min_slic3r_version = 2.4.2
+1.0.1 Added 350mm Voron v1 variant. Updated max print heights. Removed redundant v1 volcano nozzle variants.
 min_slic3r_version = 2.4.0-beta0
 1.0.0 Initial version
diff --git a/resources/profiles/Voron.ini b/resources/profiles/Voron.ini
index cd0fbd2d9a..e34da50100 100644
--- a/resources/profiles/Voron.ini
+++ b/resources/profiles/Voron.ini
@@ -7,7 +7,7 @@
 name = Voron
 # Configuration version of this file. Config file will only be installed, if the config_version differs.
 # This means, the server may force the PrusaSlicer configuration to be downgraded.
-config_version = 1.0.0
+config_version = 1.0.1
 # Where to get the updates from?
 config_update_url = https://files.prusa3d.com/wp-content/uploads/repository/PrusaSlicer-settings-master/live/Voron/
 
@@ -72,21 +72,30 @@ default_materials = Basic PLA @VORON; Basic PLA VOLCANO @VORON; Basic PET @VORON
 
 [printer_model:Voron_v1_250_afterburner]
 name = Voron v1 250mm3
-variants = 0.4; 0.25; 0.3; 0.5; 0.6; 0.8; volcano 0.6; volcano 0.8; volcano 1.0; volcano 1.2
+variants = 0.4; 0.25; 0.3; 0.5; 0.6; 0.8
 technology = FFF
 family = Voron v1 Afterburner
 bed_model = printbed-v1-250.stl
 bed_texture = bedtexture-v1-250.png
-default_materials = Basic PLA @VORON; Basic PLA VOLCANO @VORON; Basic PET @VORON; Basic PET VOLCANO @VORON; Basic ABS @VORON; Basic ABS VOLCANO @VORON
+default_materials = Basic PLA @VORON; Basic PET @VORON; Basic ABS @VORON
 
 [printer_model:Voron_v1_300_afterburner]
 name = Voron v1 300mm3
-variants = 0.4; 0.25; 0.3; 0.5; 0.6; 0.8; volcano 0.6; volcano 0.8; volcano 1.0; volcano 1.2
+variants = 0.4; 0.25; 0.3; 0.5; 0.6; 0.8
 technology = FFF
 family = Voron v1 Afterburner
 bed_model = printbed-v1-300.stl
 bed_texture = bedtexture-v1-300.png
-default_materials = Basic PLA @VORON; Basic PLA VOLCANO @VORON; Basic PET @VORON; Basic PET VOLCANO @VORON; Basic ABS @VORON; Basic ABS VOLCANO @VORON
+default_materials = Basic PLA @VORON; Basic PET @VORON; Basic ABS @VORON
+
+[printer_model:Voron_v1_350_afterburner]
+name = Voron v1 350mm3
+variants = 0.4; 0.25; 0.3; 0.5; 0.6; 0.8
+technology = FFF
+family = Voron v1 Afterburner
+bed_model = printbed-v1-350.stl
+bed_texture = bedtexture-v2-350.png 
+default_materials = Basic PLA @VORON; Basic PET @VORON; Basic ABS @VORON
 
 [printer_model:Voron_v0_120]
 name = Voron Zero 120mm3
@@ -239,21 +248,21 @@ retract_speed = 50
 [printer:*Voron_v2_250*]
 inherits = *common*
 bed_shape = 0x0,250x0,250x250,0x250
-max_print_height = 250
+max_print_height = 230
 printer_model = Voron_v2_250
 printer_notes = Unoffical profile.\nPRINTER_HAS_BOWDEN\nE3DV6
 
 [printer:*Voron_v2_300*]
 inherits = *common*
 bed_shape = 0x0,300x0,300x300,0x300
-max_print_height = 300
+max_print_height = 280
 printer_model = Voron_v2_300
 printer_notes = Unoffical profile.\nPRINTER_HAS_BOWDEN\nE3DV6
 
 [printer:*Voron_v2_350*]
 inherits = *common*
 bed_shape = 0x0,350x0,350x350,0x350
-max_print_height = 350
+max_print_height = 330
 printer_model = Voron_v2_350
 printer_notes = Unoffical profile.\nPRINTER_HAS_BOWDEN\nE3DV6
 
@@ -282,10 +291,17 @@ printer_notes = Unoffical profile.\nE3DV6
 [printer:*Voron_v1_300_afterburner*]
 inherits = *common*; *afterburner*
 bed_shape = 0x0,300x0,300x300,0x300
-max_print_height = 230
+max_print_height = 280
 printer_model = Voron_v1_300_afterburner
 printer_notes = Unoffical profile.\nE3DV6
 
+[printer:*Voron_v1_350_afterburner*]
+inherits = *common*; *afterburner*
+bed_shape = 0x0,350x0,350x350,0x350
+max_print_height = 330
+printer_model = Voron_v1_350_afterburner
+printer_notes = Unoffical profile.\nE3DV6
+
 [printer:*Voron_v0_120*]
 inherits = *common*
 bed_shape = 0x0,120x0,120x120,0x120
@@ -455,6 +471,24 @@ inherits = *Voron_v1_300_afterburner*; *0.6nozzle*
 [printer:Voron_v1_300_afterburner 0.8 nozzle]
 inherits = *Voron_v1_300_afterburner*; *0.8nozzle*
 
+[printer:Voron_v1_350_afterburner 0.25 nozzle]
+inherits = *Voron_v1_350_afterburner*; *0.25nozzle*
+
+[printer:Voron_v1_350_afterburner 0.3 nozzle]
+inherits = *Voron_v1_350_afterburner*; *0.3nozzle*
+
+[printer:Voron_v1_350_afterburner 0.4 nozzle]
+inherits = *Voron_v1_350_afterburner*; *0.4nozzle*
+
+[printer:Voron_v1_350_afterburner 0.5 nozzle]
+inherits = *Voron_v1_350_afterburner*; *0.5nozzle*
+
+[printer:Voron_v1_350_afterburner 0.6 nozzle]
+inherits = *Voron_v1_350_afterburner*; *0.6nozzle*
+
+[printer:Voron_v1_350_afterburner 0.8 nozzle]
+inherits = *Voron_v1_350_afterburner*; *0.8nozzle*
+
 [printer:Voron_v2_250_afterburner 0.25 nozzle]
 inherits = *Voron_v2_250_afterburner*; *0.25nozzle*
 
@@ -652,7 +686,7 @@ fill_angle = 45
 fill_density = 15%
 fill_pattern = gyroid
 first_layer_acceleration = 1000
-first_layer_height = 75%
+first_layer_height = 0.2
 first_layer_speed = 30
 gap_fill_speed = 40
 gcode_comments = 0
diff --git a/resources/profiles/Voron/Voron_v1_250_afterburner_thumbnail.png b/resources/profiles/Voron/Voron_v1_250_afterburner_thumbnail.png
index 07c3202cac..2b93390975 100644
Binary files a/resources/profiles/Voron/Voron_v1_250_afterburner_thumbnail.png and b/resources/profiles/Voron/Voron_v1_250_afterburner_thumbnail.png differ
diff --git a/resources/profiles/Voron/Voron_v1_300_afterburner_thumbnail.png b/resources/profiles/Voron/Voron_v1_300_afterburner_thumbnail.png
index 07c3202cac..2b93390975 100644
Binary files a/resources/profiles/Voron/Voron_v1_300_afterburner_thumbnail.png and b/resources/profiles/Voron/Voron_v1_300_afterburner_thumbnail.png differ
diff --git a/resources/profiles/Voron/Voron_v1_350_afterburner_thumbnail.png b/resources/profiles/Voron/Voron_v1_350_afterburner_thumbnail.png
index 07c3202cac..2b93390975 100644
Binary files a/resources/profiles/Voron/Voron_v1_350_afterburner_thumbnail.png and b/resources/profiles/Voron/Voron_v1_350_afterburner_thumbnail.png differ
diff --git a/src/libslic3r/AABBTreeIndirect.hpp b/src/libslic3r/AABBTreeIndirect.hpp
index 3ea18de8dc..014b49af11 100644
--- a/src/libslic3r/AABBTreeIndirect.hpp
+++ b/src/libslic3r/AABBTreeIndirect.hpp
@@ -771,8 +771,8 @@ inline bool is_any_triangle_in_radius(
     auto distancer = detail::IndexedTriangleSetDistancer<VertexType, IndexedFaceType, TreeType, VectorType>
             { vertices, faces, tree, point };
 
-	size_t hit_idx;
-	VectorType hit_point = VectorType::Ones() * (std::nan(""));
+    size_t hit_idx;
+    VectorType hit_point = VectorType::Ones() * (NaN<typename VectorType::Scalar>);
 
 	if(tree.empty())
 	{
@@ -828,22 +828,22 @@ struct Intersecting<Eigen::AlignedBox<CoordType, NumD>> {
 
 template<class G> auto intersecting(const G &g) { return Intersecting<G>{g}; }
 
-template<class G> struct Containing {};
+template<class G> struct Within {};
 
 // Intersection predicate specialization for box-box intersections
 template<class CoordType, int NumD>
-struct Containing<Eigen::AlignedBox<CoordType, NumD>> {
+struct Within<Eigen::AlignedBox<CoordType, NumD>> {
     Eigen::AlignedBox<CoordType, NumD> box;
 
-    Containing(const Eigen::AlignedBox<CoordType, NumD> &bb): box{bb} {}
+    Within(const Eigen::AlignedBox<CoordType, NumD> &bb): box{bb} {}
 
     bool operator() (const typename Tree<NumD, CoordType>::Node &node) const
     {
-        return box.contains(node.bbox);
+        return node.is_leaf() ? box.contains(node.bbox) : box.intersects(node.bbox);
     }
 };
 
-template<class G> auto containing(const G &g) { return Containing<G>{g}; }
+template<class G> auto within(const G &g) { return Within<G>{g}; }
 
 namespace detail {
 
@@ -858,7 +858,7 @@ void traverse_recurse(const Tree<Dims, T> &tree,
     if (!pred(tree.node(idx))) return;
 
     if (tree.node(idx).is_leaf()) {
-        callback(tree.node(idx).idx);
+        callback(tree.node(idx));
     } else {
 
         // call this with left and right node idx:
diff --git a/src/libslic3r/AStar.hpp b/src/libslic3r/AStar.hpp
new file mode 100644
index 0000000000..052d0e814d
--- /dev/null
+++ b/src/libslic3r/AStar.hpp
@@ -0,0 +1,182 @@
+#ifndef ASTAR_HPP
+#define ASTAR_HPP
+
+#include "libslic3r/Point.hpp"
+#include "libslic3r/MutablePriorityQueue.hpp"
+
+#include <unordered_map>
+
+namespace Slic3r { namespace astar {
+
+// Input interface for the Astar algorithm. Specialize this struct for a
+// particular type and implement all the 4 methods and specify the Node type
+// to register the new type for the astar implementation.
+template<class T> struct TracerTraits_
+{
+    // The type of a node used by this tracer. Usually a point in space.
+    using Node = typename T::Node;
+
+    // Call fn for every new node reachable from node 'src'. fn should have the
+    // candidate node as its only argument.
+    template<class Fn>
+    static void foreach_reachable(const T &tracer, const Node &src, Fn &&fn)
+    {
+        tracer.foreach_reachable(src, fn);
+    }
+
+    // Get the distance from node 'a' to node 'b'. This is sometimes referred
+    // to as the g value of a node in AStar context.
+    static float distance(const T &tracer, const Node &a, const Node &b)
+    {
+        return tracer.distance(a, b);
+    }
+
+    // Get the estimated distance heuristic from node 'n' to the destination.
+    // This is referred to as the h value in AStar context.
+    // If node 'n' is the goal, this function should return a negative value.
+    static float goal_heuristic(const T &tracer, const Node &n)
+    {
+        return tracer.goal_heuristic(n);
+    }
+
+    // Return a unique identifier (hash) for node 'n'.
+    static size_t unique_id(const T &tracer, const Node &n)
+    {
+        return tracer.unique_id(n);
+    }
+};
+
+// Helper definition to get the node type of a tracer
+template<class T>
+using TracerNodeT = typename TracerTraits_<remove_cvref_t<T>>::Node;
+
+namespace detail {
+// Helper functions dispatching calls through the TracerTraits_ interface
+
+template<class T> using TracerTraits = TracerTraits_<remove_cvref_t<T>>;
+
+template<class T, class Fn>
+void foreach_reachable(const T &tracer, const TracerNodeT<T> &from, Fn &&fn)
+{
+    TracerTraits<T>::foreach_reachable(tracer, from, fn);
+}
+
+template<class T>
+float trace_distance(const T &tracer, const TracerNodeT<T> &a, const TracerNodeT<T> &b)
+{
+    return TracerTraits<T>::distance(tracer, a, b);
+}
+
+template<class T>
+float goal_heuristic(const T &tracer, const TracerNodeT<T> &n)
+{
+    return TracerTraits<T>::goal_heuristic(tracer, n);
+}
+
+template<class T>
+size_t unique_id(const T &tracer, const TracerNodeT<T> &n)
+{
+    return TracerTraits<T>::unique_id(tracer, n);
+}
+
+} // namespace astar_detail
+
+// Run the AStar algorithm on a tracer implementation.
+// The 'tracer' argument encapsulates the domain (grid, point cloud, etc...)
+// The 'source' argument is the starting node.
+// The 'out' argument is the output iterator into which the output nodes are
+// written.
+// Note that no destination node is given. The tracer's goal_heuristic() method
+// should return a negative value if a node is a destination node.
+template<class Tracer, class It>
+bool search_route(const Tracer &tracer, const TracerNodeT<Tracer> &source, It out)
+{
+    using namespace detail;
+
+    using Node = TracerNodeT<Tracer>;
+    enum  class QueueType { Open, Closed, None };
+
+    struct QNode // Queue node. Keeps track of scores g, and h
+    {
+        Node      node;                    // The actual node itself
+        QueueType qtype = QueueType::None; // Which queue holds this node
+
+        float  g = 0.f, h = 0.f;
+        float f() const { return g + h; }
+    };
+
+       // TODO: apply a linear memory allocator
+    using QMap = std::unordered_map<size_t, QNode>;
+
+       // The traversed nodes are stored here encapsulated in QNodes
+    QMap cached_nodes;
+
+    struct LessPred { // Comparison functor needed by MutablePriorityQueue
+        QMap &m;
+        bool operator ()(size_t node_a, size_t node_b) {
+            auto ait = m.find(node_a);
+            auto bit = m.find(node_b);
+            assert (ait != m.end() && bit != m.end());
+
+            return ait->second.f() < bit->second.f();
+        }
+    };
+
+    auto qopen =
+        make_mutable_priority_queue<size_t, false>([](size_t, size_t){},
+                                                   LessPred{cached_nodes});
+
+    auto qclosed =
+        make_mutable_priority_queue<size_t, false>([](size_t, size_t){},
+                                                   LessPred{cached_nodes});
+
+    QNode initial{source, QueueType::Open};
+    cached_nodes.insert({unique_id(tracer, source), initial});
+    qopen.push(unique_id(tracer, source));
+
+    bool goal_reached = false;
+
+    while (!goal_reached && !qopen.empty()) {
+        size_t q_id = qopen.top();
+        qopen.pop();
+        QNode q = cached_nodes.at(q_id);
+
+        foreach_reachable(tracer, q.node, [&](const Node &nd) {
+            if (goal_reached) return goal_reached;
+
+            float h = goal_heuristic(tracer, nd);
+            if (h < 0.f) {
+                goal_reached = true;
+            } else {
+                float  dst = trace_distance(tracer, q.node, nd);
+                QNode  qnd{nd, QueueType::None, q.g + dst, h};
+                size_t qnd_id = unique_id(tracer, nd);
+
+                auto it = cached_nodes.find(qnd_id);
+
+                if (it == cached_nodes.end() ||
+                    (it->second.qtype != QueueType::None && qnd.f() < it->second.f())) {
+                    qnd.qtype = QueueType::Open;
+                    cached_nodes.insert_or_assign(qnd_id, qnd);
+                    qopen.push(qnd_id);
+                }
+            }
+
+            return goal_reached;
+        });
+
+        q.qtype = QueueType::Closed;
+        cached_nodes.insert_or_assign(q_id, q);
+        qclosed.push(q_id);
+
+           // write the output
+        *out = q.node;
+        ++out;
+    }
+
+    return goal_reached;
+}
+
+}} // namespace Slic3r::astar
+
+#endif // ASTAR_HPP
diff --git a/src/libslic3r/Arrange.hpp b/src/libslic3r/Arrange.hpp
index 0ff87c88d5..47171007ac 100644
--- a/src/libslic3r/Arrange.hpp
+++ b/src/libslic3r/Arrange.hpp
@@ -15,7 +15,7 @@ class CircleBed {
     double radius_;
 public:
 
-    inline CircleBed(): center_(0, 0), radius_(std::nan("")) {}
+    inline CircleBed(): center_(0, 0), radius_(NaNd) {}
     explicit inline CircleBed(const Point& c, double r): center_(c), radius_(r) {}
 
     inline double radius() const { return radius_; }
diff --git a/src/libslic3r/CMakeLists.txt b/src/libslic3r/CMakeLists.txt
index d2ecefb9c7..c9d8aa4fa8 100644
--- a/src/libslic3r/CMakeLists.txt
+++ b/src/libslic3r/CMakeLists.txt
@@ -17,6 +17,7 @@ endif()
 set(SLIC3R_SOURCES
     pchheader.cpp
     pchheader.hpp
+    AStar.hpp
     BoundingBox.cpp
     BoundingBox.hpp
     BridgeDetector.cpp
@@ -212,6 +213,7 @@ set(SLIC3R_SOURCES
     PrintObject.cpp
     PrintObjectSlice.cpp
     PrintRegion.cpp
+    PointGrid.hpp
     PNGReadWrite.hpp
     PNGReadWrite.cpp
     QuadricEdgeCollapse.cpp
diff --git a/src/libslic3r/Execution/Execution.hpp b/src/libslic3r/Execution/Execution.hpp
index dcfd86bde8..57ad4b41b9 100644
--- a/src/libslic3r/Execution/Execution.hpp
+++ b/src/libslic3r/Execution/Execution.hpp
@@ -30,8 +30,8 @@ template<class EP> using AsTraits = Traits<remove_cvref_t<EP>>;
 
 // Each execution policy should declare two types of mutexes. A a spin lock and
 // a blocking mutex. These types should satisfy the BasicLockable concept.
-template<class EP> using SpinningMutex = typename Traits<EP>::SpinningMutex;
-template<class EP> using BlockingMutex = typename Traits<EP>::BlockingMutex;
+template<class EP> using SpinningMutex = typename AsTraits<EP>::SpinningMutex;
+template<class EP> using BlockingMutex = typename AsTraits<EP>::BlockingMutex;
 
 // Query the available threads for concurrency.
 template<class EP, class = ExecutionPolicyOnly<EP> >
diff --git a/src/libslic3r/ExtrusionEntity.hpp b/src/libslic3r/ExtrusionEntity.hpp
index 52a8a563ce..e72a7b9140 100644
--- a/src/libslic3r/ExtrusionEntity.hpp
+++ b/src/libslic3r/ExtrusionEntity.hpp
@@ -85,7 +85,7 @@ public:
     virtual ExtrusionEntity* clone() const = 0;
     // Create a new object, initialize it with this object using the move semantics.
     virtual ExtrusionEntity* clone_move() = 0;
-    virtual ~ExtrusionEntity() {}
+    virtual ~ExtrusionEntity() = default;
     virtual void reverse() = 0;
     virtual const Point& first_point() const = 0;
     virtual const Point& last_point() const = 0;
diff --git a/src/libslic3r/ExtrusionEntityCollection.hpp b/src/libslic3r/ExtrusionEntityCollection.hpp
index 6e62a45fd6..1ecda7dd53 100644
--- a/src/libslic3r/ExtrusionEntityCollection.hpp
+++ b/src/libslic3r/ExtrusionEntityCollection.hpp
@@ -36,9 +36,13 @@ public:
     ExtrusionEntityCollection(ExtrusionEntityCollection &&other) : entities(std::move(other.entities)), no_sort(other.no_sort) {}
     explicit ExtrusionEntityCollection(const ExtrusionPaths &paths);
     ExtrusionEntityCollection& operator=(const ExtrusionEntityCollection &other);
-    ExtrusionEntityCollection& operator=(ExtrusionEntityCollection &&other)
-        { this->entities = std::move(other.entities); this->no_sort = other.no_sort; return *this; }
-    ~ExtrusionEntityCollection() { clear(); }
+    ExtrusionEntityCollection& operator=(ExtrusionEntityCollection &&other) {
+        this->clear();
+        this->entities = std::move(other.entities);
+        this->no_sort  = other.no_sort;
+        return *this;
+    }
+    ~ExtrusionEntityCollection() override { clear(); }
     explicit operator ExtrusionPaths() const;
     
     bool is_collection() const override { return true; }
diff --git a/src/libslic3r/KDTreeIndirect.hpp b/src/libslic3r/KDTreeIndirect.hpp
index 36a157456f..37c10827b1 100644
--- a/src/libslic3r/KDTreeIndirect.hpp
+++ b/src/libslic3r/KDTreeIndirect.hpp
@@ -11,231 +11,276 @@
 
 namespace Slic3r {
 
+enum class VisitorReturnMask : unsigned int {
+    CONTINUE_LEFT  = 1,
+    CONTINUE_RIGHT = 2,
+    STOP           = 4,
+};
+
 // KD tree for N-dimensional closest point search.
 template<size_t ANumDimensions, typename ACoordType, typename ACoordinateFn>
 class KDTreeIndirect
 {
 public:
-	static constexpr size_t NumDimensions = ANumDimensions;
-	using					CoordinateFn  = ACoordinateFn;
-	using					CoordType     = ACoordType;
+    static constexpr size_t NumDimensions = ANumDimensions;
+    using					CoordinateFn  = ACoordinateFn;
+    using					CoordType     = ACoordType;
     // Following could be static constexpr size_t, but that would not link in C++11
     enum : size_t {
         npos = size_t(-1)
     };
 
-	KDTreeIndirect(CoordinateFn coordinate) : coordinate(coordinate) {}
-	KDTreeIndirect(CoordinateFn coordinate, std::vector<size_t>   indices) : coordinate(coordinate) { this->build(std::move(indices)); }
-	KDTreeIndirect(CoordinateFn coordinate, std::vector<size_t> &&indices) : coordinate(coordinate) { this->build(std::move(indices)); }
-	KDTreeIndirect(CoordinateFn coordinate, size_t num_indices) : coordinate(coordinate) { this->build(num_indices); }
-	KDTreeIndirect(KDTreeIndirect &&rhs) : m_nodes(std::move(rhs.m_nodes)), coordinate(std::move(rhs.coordinate)) {}
-	KDTreeIndirect& operator=(KDTreeIndirect &&rhs) { m_nodes = std::move(rhs.m_nodes); coordinate = std::move(rhs.coordinate); return *this; }
-	void clear() { m_nodes.clear(); }
+    KDTreeIndirect(CoordinateFn coordinate) : coordinate(coordinate) {}
+    KDTreeIndirect(CoordinateFn coordinate, std::vector<size_t> indices) : coordinate(coordinate) { this->build(indices); }
+    KDTreeIndirect(CoordinateFn coordinate, size_t num_indices) : coordinate(coordinate) { this->build(num_indices); }
+    KDTreeIndirect(KDTreeIndirect &&rhs) : m_nodes(std::move(rhs.m_nodes)), coordinate(std::move(rhs.coordinate)) {}
+    KDTreeIndirect& operator=(KDTreeIndirect &&rhs) { m_nodes = std::move(rhs.m_nodes); coordinate = std::move(rhs.coordinate); return *this; }
+    void clear() { m_nodes.clear(); }
 
-	void build(size_t num_indices)
-	{
-		std::vector<size_t> indices;
-		indices.reserve(num_indices);
-		for (size_t i = 0; i < num_indices; ++ i)
-			indices.emplace_back(i);
-		this->build(std::move(indices));
-	}
+    void build(size_t num_indices)
+    {
+        std::vector<size_t> indices;
+        indices.reserve(num_indices);
+        for (size_t i = 0; i < num_indices; ++ i)
+            indices.emplace_back(i);
+        this->build(indices);
+    }
 
-	void build(std::vector<size_t> &&indices)
-	{
-		if (indices.empty())
-			clear();
-		else {
-			// Allocate enough memory for a full binary tree.
-			m_nodes.assign(next_highest_power_of_2(indices.size() + 1), npos);
-			build_recursive(indices, 0, 0, 0, indices.size() - 1);
-		}
-		indices.clear();
-	}
+    void build(std::vector<size_t> &indices)
+    {
+        if (indices.empty())
+            clear();
+        else {
+            // Allocate enough memory for a full binary tree.
+            m_nodes.assign(next_highest_power_of_2(indices.size() + 1), npos);
+            build_recursive(indices, 0, 0, 0, indices.size() - 1);
+        }
+        indices.clear();
+    }
 
-	enum class VisitorReturnMask : unsigned int
-	{
-		CONTINUE_LEFT   = 1,
-		CONTINUE_RIGHT  = 2,
-		STOP 			= 4,
-	};
-	template<typename CoordType>
-	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);
-		return (dist * dist < search_radius + CoordType(EPSILON)) ?
-			// The plane intersects a hypersphere centered at point_coord of search_radius.
-			((unsigned int)(VisitorReturnMask::CONTINUE_LEFT) | (unsigned int)(VisitorReturnMask::CONTINUE_RIGHT)) :
-			// The plane does not intersect the hypersphere.
-			(dist > CoordType(0)) ? (unsigned int)(VisitorReturnMask::CONTINUE_RIGHT) : (unsigned int)(VisitorReturnMask::CONTINUE_LEFT);
-	}
+    template<typename CoordType>
+    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);
+        return (dist * dist < search_radius + CoordType(EPSILON)) ?
+                                                                    // The plane intersects a hypersphere centered at point_coord of search_radius.
+                   ((unsigned int)(VisitorReturnMask::CONTINUE_LEFT) | (unsigned int)(VisitorReturnMask::CONTINUE_RIGHT)) :
+                   // The plane does not intersect the hypersphere.
+                   (dist > CoordType(0)) ? (unsigned int)(VisitorReturnMask::CONTINUE_RIGHT) : (unsigned int)(VisitorReturnMask::CONTINUE_LEFT);
+    }
 
-	// Visitor is supposed to return a bit mask of VisitorReturnMask.
-	template<typename Visitor>
-	void visit(Visitor &visitor) const
-	{
+       // Visitor is supposed to return a bit mask of VisitorReturnMask.
+    template<typename Visitor>
+    void visit(Visitor &visitor) const
+    {
         visit_recursive(0, 0, visitor);
-	}
+    }
 
-	CoordinateFn coordinate;
+    CoordinateFn coordinate;
 
 private:
-	// Build a balanced tree by splitting the input sequence by an axis aligned plane at a dimension.
-	void build_recursive(std::vector<size_t> &input, size_t node, const size_t dimension, const size_t left, const size_t right)
-	{
-		if (left > right)
-			return;
+    // Build a balanced tree by splitting the input sequence by an axis aligned plane at a dimension.
+    void build_recursive(std::vector<size_t> &input, size_t node, const size_t dimension, const size_t left, const size_t right)
+    {
+        if (left > right)
+            return;
 
-		assert(node < m_nodes.size());
+        assert(node < m_nodes.size());
 
-		if (left == right) {
-			// Insert a node into the balanced tree.
-			m_nodes[node] = input[left];
-			return;
-		}
+        if (left == right) {
+            // Insert a node into the balanced tree.
+            m_nodes[node] = input[left];
+            return;
+        }
 
-		// Partition the input to left / right pieces of the same length to produce a balanced tree.
-		size_t center = (left + right) / 2;
-		partition_input(input, dimension, left, right, center);
-		// Insert a node into the tree.
-		m_nodes[node] = input[center];
-		// Build up the left / right subtrees.
-		size_t next_dimension = dimension;
-		if (++ next_dimension == NumDimensions)
-			next_dimension = 0;
-		if (center > left)
-			build_recursive(input, node * 2 + 1, next_dimension, left, center - 1);
-		build_recursive(input, node * 2 + 2, next_dimension, center + 1, right);
-	}
+           // Partition the input to left / right pieces of the same length to produce a balanced tree.
+        size_t center = (left + right) / 2;
+        partition_input(input, dimension, left, right, center);
+        // Insert a node into the tree.
+        m_nodes[node] = input[center];
+        // Build up the left / right subtrees.
+        size_t next_dimension = dimension;
+        if (++ next_dimension == NumDimensions)
+            next_dimension = 0;
+        if (center > left)
+            build_recursive(input, node * 2 + 1, next_dimension, left, center - 1);
+        build_recursive(input, node * 2 + 2, next_dimension, center + 1, right);
+    }
 
-	// Partition the input m_nodes <left, right> 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",
-	void partition_input(std::vector<size_t> &input, const size_t dimension, size_t left, size_t right, const size_t k) const
-	{
-		while (left < right) {
-			size_t center = (left + right) / 2;
-			CoordType pivot;
-			{
-				// Bubble sort the input[left], input[center], input[right], so that a median of the three values
-				// will end up in input[center].
-				CoordType left_value   = this->coordinate(input[left],   dimension);
-				CoordType center_value = this->coordinate(input[center], dimension);
-				CoordType right_value  = this->coordinate(input[right],  dimension);
-				if (left_value > center_value) {
-					std::swap(input[left], input[center]);
-					std::swap(left_value,  center_value);
-				}
-				if (left_value > right_value) {
-					std::swap(input[left], input[right]);
-					right_value = left_value;
-				}
-				if (center_value > right_value) {
-					std::swap(input[center], input[right]);
-					center_value = right_value;
-				}
-				pivot = center_value;
-			}
-			if (right <= left + 2)
-				// The <left, right> interval is already sorted.
-				break;
-			size_t i = left;
-			size_t j = right - 1;
-			std::swap(input[center], input[j]);
-			// Partition the set based on the pivot.
-			for (;;) {
-				// Skip left points that are already at correct positions.
-				// Search will certainly stop at position (right - 1), which stores the pivot.
-				while (this->coordinate(input[++ i], dimension) < pivot) ;
-				// Skip right points that are already at correct positions.
-				while (this->coordinate(input[-- j], dimension) > pivot && i < j) ;
-				if (i >= j)
-					break;
-				std::swap(input[i], input[j]);
-			}
-			// Restore pivot to the center of the sequence.
-			std::swap(input[i], input[right - 1]);
-			// Which side the kth element is in?
-			if (k < i)
-				right = i - 1;
-			else if (k == i)
-				// Sequence is partitioned, kth element is at its place.
-				break;
-			else
-				left = i + 1;
-		}
-	}
+       // Partition the input m_nodes <left, right> 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",
+    void partition_input(std::vector<size_t> &input, const size_t dimension, size_t left, size_t right, const size_t k) const
+    {
+        while (left < right) {
+            size_t center = (left + right) / 2;
+            CoordType pivot;
+            {
+                // Bubble sort the input[left], input[center], input[right], so that a median of the three values
+                // will end up in input[center].
+                CoordType left_value   = this->coordinate(input[left],   dimension);
+                CoordType center_value = this->coordinate(input[center], dimension);
+                CoordType right_value  = this->coordinate(input[right],  dimension);
+                if (left_value > center_value) {
+                    std::swap(input[left], input[center]);
+                    std::swap(left_value,  center_value);
+                }
+                if (left_value > right_value) {
+                    std::swap(input[left], input[right]);
+                    right_value = left_value;
+                }
+                if (center_value > right_value) {
+                    std::swap(input[center], input[right]);
+                    center_value = right_value;
+                }
+                pivot = center_value;
+            }
+            if (right <= left + 2)
+                // The <left, right> interval is already sorted.
+                break;
+            size_t i = left;
+            size_t j = right - 1;
+            std::swap(input[center], input[j]);
+            // Partition the set based on the pivot.
+            for (;;) {
+                // Skip left points that are already at correct positions.
+                // Search will certainly stop at position (right - 1), which stores the pivot.
+                while (this->coordinate(input[++ i], dimension) < pivot) ;
+                // Skip right points that are already at correct positions.
+                while (this->coordinate(input[-- j], dimension) > pivot && i < j) ;
+                if (i >= j)
+                    break;
+                std::swap(input[i], input[j]);
+            }
+            // Restore pivot to the center of the sequence.
+            std::swap(input[i], input[right - 1]);
+            // Which side the kth element is in?
+            if (k < i)
+                right = i - 1;
+            else if (k == i)
+                // Sequence is partitioned, kth element is at its place.
+                break;
+            else
+                left = i + 1;
+        }
+    }
 
-	template<typename Visitor>
-	void visit_recursive(size_t node, size_t dimension, Visitor &visitor) const
-	{
-		assert(! m_nodes.empty());
-		if (node >= m_nodes.size() || m_nodes[node] == npos)
-			return;
+    template<typename Visitor>
+    void visit_recursive(size_t node, size_t dimension, Visitor &visitor) const
+    {
+        assert(! m_nodes.empty());
+        if (node >= m_nodes.size() || m_nodes[node] == npos)
+            return;
 
-		// Left / right child node index.
-		size_t left  = node * 2 + 1;
-		size_t right = left + 1;
-		unsigned int mask = visitor(m_nodes[node], dimension);
-		if ((mask & (unsigned int)VisitorReturnMask::STOP) == 0) {
-			size_t next_dimension = (++ dimension == NumDimensions) ? 0 : dimension;
-			if (mask & (unsigned int)VisitorReturnMask::CONTINUE_LEFT)
-				visit_recursive(left,  next_dimension, visitor);
-			if (mask & (unsigned int)VisitorReturnMask::CONTINUE_RIGHT)
-				visit_recursive(right, next_dimension, visitor);
-		}
-	}
+           // Left / right child node index.
+        size_t left  = node * 2 + 1;
+        size_t right = left + 1;
+        unsigned int mask = visitor(m_nodes[node], dimension);
+        if ((mask & (unsigned int)VisitorReturnMask::STOP) == 0) {
+            size_t next_dimension = (++ dimension == NumDimensions) ? 0 : dimension;
+            if (mask & (unsigned int)VisitorReturnMask::CONTINUE_LEFT)
+                visit_recursive(left,  next_dimension, visitor);
+            if (mask & (unsigned int)VisitorReturnMask::CONTINUE_RIGHT)
+                visit_recursive(right, next_dimension, visitor);
+        }
+    }
 
-	std::vector<size_t> m_nodes;
+    std::vector<size_t> m_nodes;
 };
 
 // Find a closest point using Euclidian metrics.
 // Returns npos if not found.
-template<typename KDTreeIndirectType, typename PointType, typename FilterFn>
-size_t find_closest_point(const KDTreeIndirectType &kdtree, const PointType &point, FilterFn filter)
+template<size_t K,
+         typename PointType,
+         typename FilterFn,
+         size_t D,
+         typename CoordT,
+         typename CoordFn>
+std::array<size_t, K> find_closest_points(
+    const KDTreeIndirect<D, CoordT, CoordFn> &kdtree,
+    const PointType                          &point,
+    FilterFn                                  filter)
 {
-	using CoordType = typename KDTreeIndirectType::CoordType;
+    using Tree = KDTreeIndirect<D, CoordT, CoordFn>;
 
-	struct Visitor {
-		const KDTreeIndirectType   &kdtree;
-		const PointType    		   &point;
-		const FilterFn				filter;
-		size_t 						min_idx  = KDTreeIndirectType::npos;
-		CoordType					min_dist = std::numeric_limits<CoordType>::max();
+    struct Visitor
+    {
+        const Tree      &kdtree;
+        const PointType &point;
+        const FilterFn   filter;
 
-		Visitor(const KDTreeIndirectType &kdtree, const PointType &point, FilterFn filter) : kdtree(kdtree), point(point), 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 = point[i] - kdtree.coordinate(idx, i);
-					dist += d * d;
-				}
-				if (dist < min_dist) {
-					min_dist = dist;
-					min_idx  = idx;
-				}
-			}
-			return kdtree.descent_mask(point[dimension], min_dist, idx, dimension);
-		}
-	} visitor(kdtree, point, filter);
+        std::array<std::pair<size_t, CoordT>, K> results;
 
-	kdtree.visit(visitor);
-	return visitor.min_idx;
+        Visitor(const Tree &kdtree, const PointType &point, FilterFn filter)
+            : kdtree(kdtree), point(point), filter(filter)
+        {
+            results.fill(std::make_pair(Tree::npos,
+                                        std::numeric_limits<CoordT>::max()));
+        }
+        unsigned int operator()(size_t idx, size_t dimension)
+        {
+            if (this->filter(idx)) {
+                auto dist = CoordT(0);
+                for (size_t i = 0; i < D; ++i) {
+                    CoordT d = point[i] - kdtree.coordinate(idx, i);
+                    dist += d * d;
+                }
+
+                auto res = std::make_pair(idx, dist);
+                auto it  = std::lower_bound(results.begin(), results.end(),
+                                            res, [](auto &r1, auto &r2) {
+                                               return r1.second < r2.second;
+                                            });
+
+                if (it != results.end()) {
+                    std::rotate(it, std::prev(results.end()), results.end());
+                    *it = res;
+                }
+            }
+            return kdtree.descent_mask(point[dimension],
+                                       results.front().second, idx,
+                                       dimension);
+        }
+    } visitor(kdtree, point, filter);
+
+    kdtree.visit(visitor);
+    std::array<size_t, K> ret;
+    for (size_t i = 0; i < K; i++) ret[i] = visitor.results[i].first;
+
+    return ret;
+}
+
+template<size_t K, typename PointType, size_t D, typename CoordT, typename CoordFn>
+std::array<size_t, K> find_closest_points(
+    const KDTreeIndirect<D, CoordT, CoordFn> &kdtree, const PointType &point)
+{
+    return find_closest_points<K>(kdtree, point, [](size_t) { return true; });
+}
+
+template<typename PointType,
+         typename FilterFn,
+         size_t D,
+         typename CoordT,
+         typename CoordFn>
+size_t find_closest_point(const KDTreeIndirect<D, CoordT, CoordFn> &kdtree,
+                          const PointType                          &point,
+                          FilterFn                                  filter)
+{
+    return find_closest_points<1>(kdtree, point, filter)[0];
 }
 
 template<typename KDTreeIndirectType, typename PointType>
 size_t find_closest_point(const KDTreeIndirectType& kdtree, const PointType& point)
 {
-	return find_closest_point(kdtree, point, [](size_t) { return true; });
+    return find_closest_point(kdtree, point, [](size_t) { return true; });
 }
 
 // Find nearby points (spherical neighbourhood) using Euclidian metrics.
 template<typename KDTreeIndirectType, typename PointType, typename FilterFn>
 std::vector<size_t> find_nearby_points(const KDTreeIndirectType &kdtree, const PointType &center,
-       const typename KDTreeIndirectType::CoordType& max_distance, FilterFn filter)
-        {
+                                       const typename KDTreeIndirectType::CoordType& max_distance, FilterFn filter)
+{
     using CoordType = typename KDTreeIndirectType::CoordType;
 
     struct Visitor {
@@ -247,7 +292,7 @@ std::vector<size_t> find_nearby_points(const KDTreeIndirectType &kdtree, const P
 
         Visitor(const KDTreeIndirectType &kdtree, const PointType& center, const CoordType &max_distance,
                 FilterFn filter) :
-                kdtree(kdtree), center(center), max_distance_squared(max_distance*max_distance), filter(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)) {
@@ -260,7 +305,7 @@ std::vector<size_t> find_nearby_points(const KDTreeIndirectType &kdtree, const P
                     result.push_back(idx);
                 }
             }
-           return kdtree.descent_mask(center[dimension], max_distance_squared, idx, dimension);
+            return kdtree.descent_mask(center[dimension], max_distance_squared, idx, dimension);
         }
     } visitor(kdtree, center, max_distance, filter);
 
@@ -270,13 +315,59 @@ std::vector<size_t> find_nearby_points(const KDTreeIndirectType &kdtree, const P
 
 template<typename KDTreeIndirectType, typename PointType>
 std::vector<size_t> find_nearby_points(const KDTreeIndirectType &kdtree, const PointType &center,
-       const typename KDTreeIndirectType::CoordType& max_distance)
-        {
+                                       const typename KDTreeIndirectType::CoordType& max_distance)
+{
     return find_nearby_points(kdtree, center, max_distance, [](size_t) {
         return true;
     });
 }
 
+// Find nearby points (spherical neighbourhood) using Euclidian metrics.
+template<typename KDTreeIndirectType, typename PointType, typename FilterFn>
+std::vector<size_t> find_nearby_points(const KDTreeIndirectType &kdtree,
+                                       const PointType          &bb_min,
+                                       const PointType          &bb_max,
+                                       FilterFn                  filter)
+{
+    struct Visitor {
+        const KDTreeIndirectType &kdtree;
+        const PointType &bb_min, &bb_max;
+        const FilterFn filter;
+        std::vector<size_t> result;
+
+        Visitor(const KDTreeIndirectType &kdtree, const PointType& bbmin, const PointType& bbmax,
+                FilterFn filter) :
+            kdtree(kdtree), bb_min{bbmin}, bb_max{bbmax}, filter(filter) {
+        }
+        unsigned int operator()(size_t idx, size_t dimension) {
+            unsigned int ret =
+                static_cast<unsigned int>(VisitorReturnMask::CONTINUE_LEFT) |
+                static_cast<unsigned int>(VisitorReturnMask::CONTINUE_RIGHT);
+
+            if (this->filter(idx)) {
+                PointType p;
+                bool contains = true;
+                for (size_t i = 0; i < KDTreeIndirectType::NumDimensions; ++i) {
+                    p(i) = kdtree.coordinate(idx, i);
+                    contains = contains && bb_min(i) <= p(i) && p(i) <= bb_max(i);
+                }
+
+                if (p(dimension) < bb_min(dimension))
+                    ret = static_cast<unsigned int>(VisitorReturnMask::CONTINUE_RIGHT);
+                if (p(dimension) > bb_max(dimension))
+                    ret = static_cast<unsigned int>(VisitorReturnMask::CONTINUE_LEFT);
+
+                if (contains)
+                    result.emplace_back(idx);
+            }
+
+            return ret;
+        }
+    } visitor(kdtree, bb_min, bb_max, filter);
+
+    kdtree.visit(visitor);
+    return visitor.result;
+}
 
 } // namespace Slic3r
 
diff --git a/src/libslic3r/MutablePriorityQueue.hpp b/src/libslic3r/MutablePriorityQueue.hpp
index cc1cae68c9..418d8deffd 100644
--- a/src/libslic3r/MutablePriorityQueue.hpp
+++ b/src/libslic3r/MutablePriorityQueue.hpp
@@ -17,6 +17,14 @@ public:
 		{}
 	~MutablePriorityQueue()	{ clear(); }
 
+	MutablePriorityQueue(MutablePriorityQueue &&) = default;
+	MutablePriorityQueue& operator=(MutablePriorityQueue &&) = default;
+
+	// This class modifies the outside data through the m_index_setter
+	// and thus it should not be copied. The semantics are similar to std::unique_ptr
+	MutablePriorityQueue(const MutablePriorityQueue &) = delete;
+	MutablePriorityQueue& operator=(const MutablePriorityQueue &) = delete;
+
 	void		clear();
 	void		reserve(size_t cnt) 				{ m_heap.reserve(cnt); }
 	void		push(const T &item);
diff --git a/src/libslic3r/Optimize/Optimizer.hpp b/src/libslic3r/Optimize/Optimizer.hpp
index 8ae55c61c5..bf95d9ee07 100644
--- a/src/libslic3r/Optimize/Optimizer.hpp
+++ b/src/libslic3r/Optimize/Optimizer.hpp
@@ -41,13 +41,13 @@ template<size_t N> using Bounds = std::array<Bound, N>;
 class StopCriteria {
 
     // If the absolute value difference between two scores.
-    double m_abs_score_diff = std::nan("");
+    double m_abs_score_diff = NaNd;
 
     // If the relative value difference between two scores.
-    double m_rel_score_diff = std::nan("");
+    double m_rel_score_diff = NaNd;
 
     // Stop if this value or better is found.
-    double m_stop_score = std::nan("");
+    double m_stop_score = NaNd;
 
     // A predicate that if evaluates to true, the optimization should terminate
     // and the best result found prior to termination should be returned.
diff --git a/src/libslic3r/PointGrid.hpp b/src/libslic3r/PointGrid.hpp
new file mode 100644
index 0000000000..259a2950eb
--- /dev/null
+++ b/src/libslic3r/PointGrid.hpp
@@ -0,0 +1,74 @@
+#ifndef POINTGRID_HPP
+#define POINTGRID_HPP
+
+#include <libslic3r/Execution/Execution.hpp>
+#include <libslic3r/Point.hpp>
+#include <libslic3r/BoundingBox.hpp>
+
+namespace Slic3r {
+
+template<class T>
+class PointGrid {
+    Vec3i m_size;
+    std::vector<Vec<3, T>> m_data;
+    const int XY;
+
+public:
+    explicit PointGrid(std::vector<Vec<3, T>> data, const Vec3i &size)
+        : m_data(std::move(data)), m_size{size}, XY{m_size.x() * m_size.y()}
+    {}
+
+    const Vec<3, T> & get(size_t idx) const { return m_data[idx]; }
+    const Vec<3, T> & get(const Vec3i &coord) const
+    {
+        return m_data[get_idx(coord)];
+    }
+
+    size_t get_idx(const Vec3i &coord) const
+    {
+        size_t ret = coord.z() * XY + coord.y() * m_size.x() + coord.x();
+
+        return ret;
+    }
+
+    Vec3i get_coord(size_t idx) const {
+        int iz = idx / XY;
+        int iy = (idx / m_size.x()) % m_size.y();
+        int ix = idx % m_size.x();
+
+        return {ix, iy, iz};
+    }
+
+    const std::vector<Vec<3, T>> & data() const { return m_data; }
+    size_t point_count() const { return m_data.size(); }
+    bool empty() const { return m_data.empty(); }
+};
+
+template<class Ex, class CoordT>
+PointGrid<CoordT> point_grid(Ex                                      policy,
+                             const BoundingBox3Base<Vec<3, CoordT>> &bounds,
+                             const Vec<3, CoordT>                   &stride)
+{
+    Vec3i numpts = Vec3i::Zero();
+
+    for (int n = 0; n < 3; ++n)
+        numpts(n) = (bounds.max(n) - bounds.min(n)) / stride(n);
+
+    std::vector<Vec<3, CoordT>> out(numpts.x() * numpts.y() * numpts.z());
+
+    size_t XY = numpts[X] * numpts[Y];
+
+    execution::for_each(policy, size_t(0), out.size(), [&](size_t i) {
+        int iz = i / XY;
+        int iy = (i / numpts[X]) % numpts[Y];
+        int ix = i % numpts[X];
+
+        out[i] = Vec<3, CoordT>(ix * stride.x(), iy * stride.y(), iz * stride.z());
+    });
+
+    return PointGrid{std::move(out), numpts};
+}
+
+} // namespace Slic3r
+
+#endif // POINTGRID_HPP
diff --git a/src/libslic3r/SLA/Rotfinder.cpp b/src/libslic3r/SLA/Rotfinder.cpp
index 196646dc9e..847e638e6f 100644
--- a/src/libslic3r/SLA/Rotfinder.cpp
+++ b/src/libslic3r/SLA/Rotfinder.cpp
@@ -76,7 +76,7 @@ struct Facestats {
 // Try to guess the number of support points needed to support a mesh
 double get_misalginment_score(const TriangleMesh &mesh, const Transform3f &tr)
 {
-    if (mesh.its.vertices.empty()) return std::nan("");
+    if (mesh.its.vertices.empty()) return NaNd;
 
     auto accessfn = [&mesh, &tr](size_t fi) {
         Facestats fc{get_transformed_triangle(mesh, tr, fi)};
@@ -117,7 +117,7 @@ inline double get_supportedness_score(const Facestats &fc)
 // Try to guess the number of support points needed to support a mesh
 double get_supportedness_score(const TriangleMesh &mesh, const Transform3f &tr)
 {
-    if (mesh.its.vertices.empty()) return std::nan("");
+    if (mesh.its.vertices.empty()) return NaNd;
 
     auto accessfn = [&mesh, &tr](size_t fi) {
         Facestats fc{get_transformed_triangle(mesh, tr, fi)};
@@ -149,10 +149,10 @@ float find_ground_level(const TriangleMesh &mesh,
     return execution::reduce(ex_tbb, size_t(0), vsize, zmin, minfn, accessfn, granularity);
 }
 
-float get_supportedness_onfloor_score(const TriangleMesh &mesh,
-                                      const Transform3f & tr)
+double get_supportedness_onfloor_score(const TriangleMesh &mesh,
+                                       const Transform3f  &tr)
 {
-    if (mesh.its.vertices.empty()) return std::nan("");
+    if (mesh.its.vertices.empty()) return NaNd;
 
     size_t Nthreads = std::thread::hardware_concurrency();
 
diff --git a/src/libslic3r/SLA/SupportTreeBuildsteps.cpp b/src/libslic3r/SLA/SupportTreeBuildsteps.cpp
index aa69fdc777..e4ca3f59cd 100644
--- a/src/libslic3r/SLA/SupportTreeBuildsteps.cpp
+++ b/src/libslic3r/SLA/SupportTreeBuildsteps.cpp
@@ -654,7 +654,7 @@ void SupportTreeBuildsteps::filter()
     for (const SupportPoint &sp : m_support_pts) {
         m_thr();
         heads.emplace_back(
-            std::nan(""),
+            NaNd,
             sp.head_front_radius,
             0.,
             m_cfg.head_penetration_mm,
diff --git a/src/libslic3r/SLAPrintSteps.cpp b/src/libslic3r/SLAPrintSteps.cpp
index de3e3ae96d..c3ec3e3997 100644
--- a/src/libslic3r/SLAPrintSteps.cpp
+++ b/src/libslic3r/SLAPrintSteps.cpp
@@ -408,9 +408,9 @@ void SLAPrint::Steps::drill_holes(SLAPrintObject &po)
         AABBTreeIndirect::traverse(
                     tree,
                     AABBTreeIndirect::intersecting(ebb),
-                    [&part_to_drill, &hollowed_mesh](size_t faceid)
+                    [&part_to_drill, &hollowed_mesh](const auto& node)
         {
-            part_to_drill.indices.emplace_back(hollowed_mesh.its.indices[faceid]);
+            part_to_drill.indices.emplace_back(hollowed_mesh.its.indices[node.idx]);
         });
 
         auto cgal_meshpart = MeshBoolean::cgal::triangle_mesh_to_cgal(
@@ -1036,7 +1036,7 @@ void SLAPrint::Steps::merge_slices_and_eval_stats() {
     // Estimated printing time
     // A layers count o the highest object
     if (printer_input.size() == 0)
-        print_statistics.estimated_print_time = std::nan("");
+        print_statistics.estimated_print_time = NaNd;
     else {
         print_statistics.estimated_print_time = estim_time;
         print_statistics.layers_times = layers_times;
diff --git a/src/libslic3r/libslic3r.h b/src/libslic3r/libslic3r.h
index 2131a9233a..06cad840e5 100644
--- a/src/libslic3r/libslic3r.h
+++ b/src/libslic3r/libslic3r.h
@@ -331,6 +331,12 @@ public:
     inline bool   empty() const { return size() == 0; }
 };
 
+template<class T, class = FloatingOnly<T>>
+constexpr T NaN = std::numeric_limits<T>::quiet_NaN();
+
+constexpr float NaNf = NaN<float>;
+constexpr double NaNd = NaN<double>;
+
 } // namespace Slic3r
 
 #endif
diff --git a/src/slic3r/GUI/DoubleSlider.cpp b/src/slic3r/GUI/DoubleSlider.cpp
index 20a3d83960..dda50ec053 100644
--- a/src/slic3r/GUI/DoubleSlider.cpp
+++ b/src/slic3r/GUI/DoubleSlider.cpp
@@ -1163,7 +1163,7 @@ void Control::draw_ruler(wxDC& dc)
         }
     };
 
-    double short_tick = std::nan("");
+    double short_tick = NaNd;
     int tick = 0;
     double value = 0.0;
     size_t sequence = 0;
diff --git a/src/slic3r/GUI/GLCanvas3D.hpp b/src/slic3r/GUI/GLCanvas3D.hpp
index 09659faea1..1876500e23 100644
--- a/src/slic3r/GUI/GLCanvas3D.hpp
+++ b/src/slic3r/GUI/GLCanvas3D.hpp
@@ -822,7 +822,7 @@ public:
     
     class WipeTowerInfo {
     protected:
-        Vec2d m_pos = {std::nan(""), std::nan("")};
+        Vec2d m_pos = {NaNd, NaNd};
         double m_rotation = 0.;
         BoundingBoxf m_bb;
         friend class GLCanvas3D;
diff --git a/tests/libslic3r/CMakeLists.txt b/tests/libslic3r/CMakeLists.txt
index a3984b34b1..cf89b2246f 100644
--- a/tests/libslic3r/CMakeLists.txt
+++ b/tests/libslic3r/CMakeLists.txt
@@ -4,6 +4,7 @@ add_executable(${_TEST_NAME}_tests
 	${_TEST_NAME}_tests.cpp
 	test_3mf.cpp
 	test_aabbindirect.cpp
+	test_kdtreeindirect.cpp
 	test_clipper_offset.cpp
 	test_clipper_utils.cpp
 	test_color.cpp
@@ -25,6 +26,7 @@ add_executable(${_TEST_NAME}_tests
     test_png_io.cpp
     test_timeutils.cpp
     test_indexed_triangle_set.cpp
+    test_astar.cpp
     ../libnest2d/printer_parts.cpp
 	)
 
diff --git a/tests/libslic3r/test_astar.cpp b/tests/libslic3r/test_astar.cpp
new file mode 100644
index 0000000000..176d7694ab
--- /dev/null
+++ b/tests/libslic3r/test_astar.cpp
@@ -0,0 +1,71 @@
+#include <catch2/catch.hpp>
+
+#include "libslic3r/BoundingBox.hpp"
+#include "libslic3r/AStar.hpp"
+#include "libslic3r/Execution/ExecutionSeq.hpp"
+#include "libslic3r/PointGrid.hpp"
+
+using namespace Slic3r;
+
+struct PointGridTracer {
+    using Node = size_t;
+    const PointGrid<float> &grid;
+    size_t final;
+
+    PointGridTracer(const PointGrid<float> &g, size_t goal) :
+        grid{g}, final{goal} {}
+
+    template<class Fn>
+    void foreach_reachable(size_t from, Fn &&fn) const
+    {
+        Vec3i from_crd = grid.get_coord(from);
+        REQUIRE(grid.get_idx(from_crd) == from);
+
+        if (size_t i = grid.get_idx(from_crd + Vec3i{ 1,  0,  0}); i < grid.point_count()) fn(i);
+        if (size_t i = grid.get_idx(from_crd + Vec3i{ 0,  1,  0}); i < grid.point_count()) fn(i);
+        if (size_t i = grid.get_idx(from_crd + Vec3i{ 0,  0,  1}); i < grid.point_count()) fn(i);
+        if (size_t i = grid.get_idx(from_crd + Vec3i{ 1,  1,  0}); i < grid.point_count()) fn(i);
+        if (size_t i = grid.get_idx(from_crd + Vec3i{ 0,  1,  1}); i < grid.point_count()) fn(i);
+        if (size_t i = grid.get_idx(from_crd + Vec3i{ 1,  1,  1}); i < grid.point_count()) fn(i);
+        if (size_t i = grid.get_idx(from_crd + Vec3i{-1,  0,  0}); from_crd.x() > 0 && i < grid.point_count()) fn(i);
+        if (size_t i = grid.get_idx(from_crd + Vec3i{ 0, -1,  0}); from_crd.y() > 0 && i < grid.point_count()) fn(i);
+        if (size_t i = grid.get_idx(from_crd + Vec3i{ 0,  0, -1}); from_crd.z() > 0 && i < grid.point_count()) fn(i);
+        if (size_t i = grid.get_idx(from_crd + Vec3i{-1, -1,  0}); from_crd.x() > 0 && from_crd.y() > 0 && i < grid.point_count()) fn(i);
+        if (size_t i = grid.get_idx(from_crd + Vec3i{ 0, -1, -1}); from_crd.y() > 0 && from_crd.z() && i < grid.point_count()) fn(i);
+        if (size_t i = grid.get_idx(from_crd + Vec3i{-1, -1, -1}); from_crd.x() > 0 && from_crd.y() > 0 && from_crd.z() && i < grid.point_count()) fn(i);
+
+    }
+
+    float distance(size_t a, size_t b) const
+    {
+        return (grid.get(a) - grid.get(b)).squaredNorm();
+    }
+
+    float goal_heuristic(size_t n) const
+    {
+        return n == final ? -1.f : (grid.get(n) - grid.get(final)).squaredNorm();
+    }
+
+    size_t unique_id(size_t n) const { return n; }
+};
+
+TEST_CASE("astar algorithm test over 3D point grid", "[AStar]") {
+    auto vol = BoundingBox3Base<Vec3f>{{0.f, 0.f, 0.f}, {1.f, 1.f, 1.f}};
+
+    auto pgrid = point_grid(ex_seq, vol, {0.1f, 0.1f, 0.1f});
+
+    size_t target = pgrid.point_count() - 1;
+
+    std::cout << "Tracing route to " << pgrid.get_coord(target).transpose() << std::endl;
+    PointGridTracer pgt{pgrid, pgrid.point_count() - 1};
+    std::vector<size_t> out;
+    bool found = astar::search_route(pgt, size_t(0), std::back_inserter(out));
+
+    std::cout << "Route taken: ";
+    for (size_t i : out) {
+        std::cout << "(" << pgrid.get_coord(i).transpose() << ") ";
+    }
+    std::cout << std::endl;
+
+    REQUIRE(found);
+}
diff --git a/tests/libslic3r/test_kdtreeindirect.cpp b/tests/libslic3r/test_kdtreeindirect.cpp
new file mode 100644
index 0000000000..28d409bbbf
--- /dev/null
+++ b/tests/libslic3r/test_kdtreeindirect.cpp
@@ -0,0 +1,142 @@
+#include <catch2/catch.hpp>
+
+#include "libslic3r/KDTreeIndirect.hpp"
+#include "libslic3r/Execution/ExecutionSeq.hpp"
+#include "libslic3r/BoundingBox.hpp"
+#include "libslic3r/PointGrid.hpp"
+
+using namespace Slic3r;
+
+//template<class G>
+//struct Within { // Wrapper for the `within` predicate that counts calls.
+
+//    kdtree::Within<G> pred;
+
+//    Within(G box): pred{box} {}
+
+//       // Number of times the predicate was called
+//    mutable size_t call_count = 0;
+
+//    std::pair<bool, unsigned int> operator() (const Vec3f &p, size_t dim)
+//    {
+//        ++call_count;
+
+//        return pred(p, dim);
+//    }
+//};
+
+static double volume(const BoundingBox3Base<Vec3f> &box)
+{
+    auto sz = box.size();
+    return sz.x() * sz.y() * sz.z();
+}
+
+static double volume(const Eigen::AlignedBox<float, 3> &box)
+{
+    return box.volume();
+}
+
+TEST_CASE("Test kdtree query for a Box", "[KDTreeIndirect]")
+{
+    auto vol = BoundingBox3Base<Vec3f>{{0.f, 0.f, 0.f}, {10.f, 10.f, 10.f}};
+
+    auto pgrid = point_grid(ex_seq, vol, Vec3f{0.1f, 0.1f, 0.1f});
+
+    REQUIRE(!pgrid.empty());
+
+    auto coordfn = [&pgrid] (size_t i, size_t D) { return pgrid.get(i)(int(D)); };
+    KDTreeIndirect<3, float, decltype(coordfn)> tree{coordfn, pgrid.point_count()};
+
+    std::vector<size_t> out;
+
+    auto qbox = BoundingBox3Base{Vec3f{0.f, 0.f, 0.f}, Vec3f{.5f, .5f, .5f}};
+
+    size_t call_count = 0;
+    out = find_nearby_points(tree, qbox.min, qbox.max, [&call_count](size_t) {
+        call_count++;
+        return true;
+    });
+
+    // Output shall be non-empty
+    REQUIRE(!out.empty());
+
+    std::sort(out.begin(), out.end());
+
+    // No duplicates allowed in the output
+    auto it = std::unique(out.begin(), out.end());
+    REQUIRE(it == out.end());
+
+    // Test if inside points are in the output and outside points are not.
+    bool succ = true;
+    for (size_t i = 0; i < pgrid.point_count(); ++i) {
+        auto foundit = std::find(out.begin(), out.end(), i);
+        bool contains = qbox.contains(pgrid.get(i));
+        succ = succ && contains ? foundit != out.end() : foundit == out.end();
+
+        if (!succ) {
+            std::cout << "invalid point: " << i << " " << pgrid.get(i).transpose()
+                      << std::endl;
+            break;
+        }
+    }
+
+    REQUIRE(succ);
+
+    // Test for the expected cost of the query.
+    double gridvolume = volume(vol);
+    double queryvolume = volume(qbox);
+    double volratio = (queryvolume / gridvolume);
+    REQUIRE(call_count < 3 * volratio * pgrid.point_count());
+    REQUIRE(call_count < pgrid.point_count());
+}
+
+//TEST_CASE("Test kdtree query for a Sphere", "[KDTreeIndirect]") {
+//    auto vol = BoundingBox3Base<Vec3f>{{0.f, 0.f, 0.f}, {10.f, 10.f, 10.f}};
+
+//    auto pgrid = point_grid(ex_seq, vol, Vec3f{0.1f, 0.1f, 0.1f});
+
+//    REQUIRE(!pgrid.empty());
+
+//    auto coordfn = [&pgrid] (size_t i, size_t D) { return pgrid.get(i)(int(D)); };
+//    kdtree::KDTreeIndirect<3, float, decltype(coordfn)> tree{coordfn, pgrid.point_count()};
+
+//    std::vector<size_t> out;
+
+//    auto querysphere = kdtree::Sphere{Vec3f{5.f, 5.f, 5.f}, 2.f};
+
+//    auto pred = Within(querysphere);
+
+//    kdtree::query(tree, pred, std::back_inserter(out));
+
+//    // Output shall be non-empty
+//    REQUIRE(!out.empty());
+
+//    std::sort(out.begin(), out.end());
+
+//    // No duplicates allowed in the output
+//    auto it = std::unique(out.begin(), out.end());
+//    REQUIRE(it == out.end());
+
+//    // Test if inside points are in the output and outside points are not.
+//    bool succ = true;
+//    for (size_t i = 0; i < pgrid.point_count(); ++i) {
+//        auto foundit = std::find(out.begin(), out.end(), i);
+//        bool contains = (querysphere.center - pgrid.get(i)).squaredNorm() < pred.pred.r2;
+//        succ = succ && contains ? foundit != out.end() : foundit == out.end();
+
+//        if (!succ) {
+//            std::cout << "invalid point: " << i << " " << pgrid.get(i).transpose()
+//                      << std::endl;
+//            break;
+//        }
+//    }
+
+//    REQUIRE(succ);
+
+//    // Test for the expected cost of the query.
+//    double gridvolume = volume(vol);
+//    double queryvolume = volume(querysphere);
+//    double volratio = (queryvolume / gridvolume);
+//    REQUIRE(pred.call_count < 3 * volratio * pgrid.point_count());
+//    REQUIRE(pred.call_count < pgrid.point_count());
+//}
diff --git a/tests/sla_print/sla_test_utils.cpp b/tests/sla_print/sla_test_utils.cpp
index d98a920372..a58b33ca0f 100644
--- a/tests/sla_print/sla_test_utils.cpp
+++ b/tests/sla_print/sla_test_utils.cpp
@@ -386,7 +386,7 @@ long raster_pxsum(const sla::RasterGrayscaleAA &raster)
 
 double raster_white_area(const sla::RasterGrayscaleAA &raster)
 {
-    if (raster.resolution().pixels() == 0) return std::nan("");
+    if (raster.resolution().pixels() == 0) return NaNd;
     
     auto res = raster.resolution();
     double a = 0;