diff --git a/src/libslic3r/CMakeLists.txt b/src/libslic3r/CMakeLists.txt
index 9f566b405..58b74402e 100644
--- a/src/libslic3r/CMakeLists.txt
+++ b/src/libslic3r/CMakeLists.txt
@@ -203,12 +203,13 @@ add_library(libslic3r STATIC
     SimplifyMeshImpl.hpp
     SimplifyMesh.cpp
     MarchingSquares.hpp
+    Optimizer.hpp
     ${OpenVDBUtils_SOURCES}
-    SLA/Common.hpp
-    SLA/Common.cpp
     SLA/Pad.hpp
     SLA/Pad.cpp
     SLA/SupportTreeBuilder.hpp
+    SLA/SupportTreeMesher.hpp
+    SLA/SupportTreeMesher.cpp
     SLA/SupportTreeBuildsteps.hpp
     SLA/SupportTreeBuildsteps.cpp
     SLA/SupportTreeBuilder.cpp
@@ -220,6 +221,7 @@ add_library(libslic3r STATIC
     SLA/Rotfinder.cpp
     SLA/BoostAdapter.hpp
     SLA/SpatIndex.hpp
+    SLA/SpatIndex.cpp
     SLA/RasterBase.hpp
     SLA/RasterBase.cpp
     SLA/AGGRaster.hpp
@@ -235,8 +237,10 @@ add_library(libslic3r STATIC
     SLA/SupportPointGenerator.cpp
     SLA/Contour3D.hpp
     SLA/Contour3D.cpp
-    SLA/EigenMesh3D.hpp
+    SLA/IndexedMesh.hpp
+    SLA/IndexedMesh.cpp
     SLA/Clustering.hpp
+    SLA/Clustering.cpp
     SLA/ReprojectPointsOnMesh.hpp
 )
 
diff --git a/src/libslic3r/OpenVDBUtils.hpp b/src/libslic3r/OpenVDBUtils.hpp
index c493845a1..e35231d35 100644
--- a/src/libslic3r/OpenVDBUtils.hpp
+++ b/src/libslic3r/OpenVDBUtils.hpp
@@ -2,7 +2,6 @@
 #define OPENVDBUTILS_HPP
 
 #include <libslic3r/TriangleMesh.hpp>
-#include <libslic3r/SLA/Common.hpp>
 #include <libslic3r/SLA/Contour3D.hpp>
 #include <openvdb/openvdb.h>
 
diff --git a/src/libslic3r/Optimizer.hpp b/src/libslic3r/Optimizer.hpp
new file mode 100644
index 000000000..6495ae7ff
--- /dev/null
+++ b/src/libslic3r/Optimizer.hpp
@@ -0,0 +1,380 @@
+#ifndef NLOPTOPTIMIZER_HPP
+#define NLOPTOPTIMIZER_HPP
+
+#ifdef _MSC_VER
+#pragma warning(push)
+#pragma warning(disable: 4244)
+#pragma warning(disable: 4267)
+#endif
+#include <nlopt.h>
+#ifdef _MSC_VER
+#pragma warning(pop)
+#endif
+
+#include <utility>
+#include <tuple>
+#include <array>
+#include <cmath>
+#include <functional>
+#include <limits>
+#include <cassert>
+
+namespace Slic3r { namespace opt {
+
+// A type to hold the complete result of the optimization.
+template<size_t N> struct Result {
+    int resultcode;
+    std::array<double, N> optimum;
+    double score;
+};
+
+// An interval of possible input values for optimization
+class Bound {
+    double m_min, m_max;
+
+public:
+    Bound(double min = std::numeric_limits<double>::min(),
+          double max = std::numeric_limits<double>::max())
+        : m_min(min), m_max(max)
+    {}
+
+    double min() const noexcept { return m_min; }
+    double max() const noexcept { return m_max; }
+};
+
+// Helper types for optimization function input and bounds
+template<size_t N> using Input = std::array<double, N>;
+template<size_t N> using Bounds = std::array<Bound, N>;
+
+// A type for specifying the stop criteria. Setter methods can be concatenated
+class StopCriteria {
+
+    // If the absolute value difference between two scores.
+    double m_abs_score_diff = std::nan("");
+
+    // If the relative value difference between two scores.
+    double m_rel_score_diff = std::nan("");
+
+    // Stop if this value or better is found.
+    double m_stop_score = std::nan("");
+
+    // A predicate that if evaluates to true, the optimization should terminate
+    // and the best result found prior to termination should be returned.
+    std::function<bool()> m_stop_condition = [] { return false; };
+
+    // The max allowed number of iterations.
+    unsigned m_max_iterations = 0;
+
+public:
+
+    StopCriteria & abs_score_diff(double val)
+    {
+        m_abs_score_diff = val; return *this;
+    }
+
+    double abs_score_diff() const { return m_abs_score_diff; }
+
+    StopCriteria & rel_score_diff(double val)
+    {
+        m_rel_score_diff = val; return *this;
+    }
+
+    double rel_score_diff() const { return m_rel_score_diff; }
+
+    StopCriteria & stop_score(double val)
+    {
+        m_stop_score = val; return *this;
+    }
+
+    double stop_score() const { return m_stop_score; }
+
+    StopCriteria & max_iterations(double val)
+    {
+        m_max_iterations = val; return *this;
+    }
+
+    double max_iterations() const { return m_max_iterations; }
+
+    template<class Fn> StopCriteria & stop_condition(Fn &&cond)
+    {
+        m_stop_condition = cond; return *this;
+    }
+
+    bool stop_condition() { return m_stop_condition(); }
+};
+
+// Helper class to use optimization methods involving gradient.
+template<size_t N> struct ScoreGradient {
+    double score;
+    std::optional<std::array<double, N>> gradient;
+
+    ScoreGradient(double s, const std::array<double, N> &grad)
+        : score{s}, gradient{grad}
+    {}
+};
+
+// Helper to be used in static_assert.
+template<class T> struct always_false { enum { value = false }; };
+
+// Basic interface to optimizer object
+template<class Method, class Enable = void> class Optimizer {
+public:
+
+    Optimizer(const StopCriteria &)
+    {
+        static_assert (always_false<Method>::value,
+                       "Optimizer unimplemented for given method!");
+    }
+
+    Optimizer<Method> &to_min() { return *this; }
+    Optimizer<Method> &to_max() { return *this; }
+    Optimizer<Method> &set_criteria(const StopCriteria &) { return *this; }
+    StopCriteria get_criteria() const { return {}; };
+
+    template<class Func, size_t N>
+    Result<N> optimize(Func&& func,
+                       const Input<N> &initvals,
+                       const Bounds<N>& bounds) { return {}; }
+
+    // optional for randomized methods:
+    void seed(long /*s*/) {}
+};
+
+namespace detail {
+
+// Helper types for NLopt algorithm selection in template contexts
+template<nlopt_algorithm alg> struct NLoptAlg {};
+
+// NLopt can combine multiple algorithms if one is global an other is a local
+// method. This is how template specializations can be informed about this fact.
+template<nlopt_algorithm gl_alg, nlopt_algorithm lc_alg = NLOPT_LN_NELDERMEAD>
+struct NLoptAlgComb {};
+
+template<class M> struct IsNLoptAlg {
+    static const constexpr bool value = false;
+};
+
+template<nlopt_algorithm a> struct IsNLoptAlg<NLoptAlg<a>> {
+    static const constexpr bool value = true;
+};
+
+template<nlopt_algorithm a1, nlopt_algorithm a2>
+struct IsNLoptAlg<NLoptAlgComb<a1, a2>> {
+    static const constexpr bool value = true;
+};
+
+template<class M, class T = void>
+using NLoptOnly = std::enable_if_t<IsNLoptAlg<M>::value, T>;
+
+// Helper to convert C style array to std::array. The copy should be optimized
+// away with modern compilers.
+template<size_t N, class T> auto to_arr(const T *a)
+{
+    std::array<T, N> r;
+    std::copy(a, a + N, std::begin(r));
+    return r;
+}
+
+template<size_t N, class T> auto to_arr(const T (&a) [N])
+{
+    return to_arr<N>(static_cast<const T *>(a));
+}
+
+enum class OptDir { MIN, MAX }; // Where to optimize
+
+struct NLopt { // Helper RAII class for nlopt_opt
+    nlopt_opt ptr = nullptr;
+
+    template<class...A> explicit NLopt(A&&...a)
+    {
+        ptr = nlopt_create(std::forward<A>(a)...);
+    }
+
+    NLopt(const NLopt&) = delete;
+    NLopt(NLopt&&) = delete;
+    NLopt& operator=(const NLopt&) = delete;
+    NLopt& operator=(NLopt&&) = delete;
+
+    ~NLopt() { nlopt_destroy(ptr); }
+};
+
+template<class Method> class NLoptOpt {};
+
+// Optimizers based on NLopt.
+template<nlopt_algorithm alg> class NLoptOpt<NLoptAlg<alg>> {
+protected:
+    StopCriteria m_stopcr;
+    OptDir m_dir;
+
+    template<class Fn> using TOptData =
+        std::tuple<std::remove_reference_t<Fn>*, NLoptOpt*, nlopt_opt>;
+
+    template<class Fn, size_t N>
+    static double optfunc(unsigned n, const double *params,
+                          double *gradient,
+                          void *data)
+    {
+        assert(n >= N);
+
+        auto tdata = static_cast<TOptData<Fn>*>(data);
+
+        if (std::get<1>(*tdata)->m_stopcr.stop_condition())
+            nlopt_force_stop(std::get<2>(*tdata));
+
+        auto fnptr  = std::get<0>(*tdata);
+        auto funval = to_arr<N>(params);
+
+        double scoreval = 0.;
+        using RetT = decltype((*fnptr)(funval));
+        if constexpr (std::is_convertible_v<RetT, ScoreGradient<N>>) {
+            ScoreGradient<N> score = (*fnptr)(funval);
+            for (size_t i = 0; i < n; ++i) gradient[i] = (*score.gradient)[i];
+            scoreval = score.score;
+        } else {
+            scoreval = (*fnptr)(funval);
+        }
+
+        return scoreval;
+    }
+
+    template<size_t N>
+    void set_up(NLopt &nl, const Bounds<N>& bounds)
+    {
+        std::array<double, N> lb, ub;
+
+        for (size_t i = 0; i < N; ++i) {
+            lb[i] = bounds[i].min();
+            ub[i] = bounds[i].max();
+        }
+
+        nlopt_set_lower_bounds(nl.ptr, lb.data());
+        nlopt_set_upper_bounds(nl.ptr, ub.data());
+
+        double abs_diff = m_stopcr.abs_score_diff();
+        double rel_diff = m_stopcr.rel_score_diff();
+        double stopval = m_stopcr.stop_score();
+        if(!std::isnan(abs_diff)) nlopt_set_ftol_abs(nl.ptr, abs_diff);
+        if(!std::isnan(rel_diff)) nlopt_set_ftol_rel(nl.ptr, rel_diff);
+        if(!std::isnan(stopval))  nlopt_set_stopval(nl.ptr, stopval);
+
+        if(this->m_stopcr.max_iterations() > 0)
+            nlopt_set_maxeval(nl.ptr, this->m_stopcr.max_iterations());
+    }
+
+    template<class Fn, size_t N>
+    Result<N> optimize(NLopt &nl, Fn &&fn, const Input<N> &initvals)
+    {
+        Result<N> r;
+
+        TOptData<Fn> data = std::make_tuple(&fn, this, nl.ptr);
+
+        switch(m_dir) {
+        case OptDir::MIN:
+            nlopt_set_min_objective(nl.ptr, optfunc<Fn, N>, &data); break;
+        case OptDir::MAX:
+            nlopt_set_max_objective(nl.ptr, optfunc<Fn, N>, &data); break;
+        }
+
+        r.optimum = initvals;
+        r.resultcode = nlopt_optimize(nl.ptr, r.optimum.data(), &r.score);
+
+        return r;
+    }
+
+public:
+
+    template<class Func, size_t N>
+    Result<N> optimize(Func&& func,
+                       const Input<N> &initvals,
+                       const Bounds<N>& bounds)
+    {
+        NLopt nl{alg, N};
+        set_up(nl, bounds);
+
+        return optimize(nl, std::forward<Func>(func), initvals);
+    }
+
+    explicit NLoptOpt(StopCriteria stopcr = {}) : m_stopcr(stopcr) {}
+
+    void set_criteria(const StopCriteria &cr) { m_stopcr = cr; }
+    const StopCriteria &get_criteria() const noexcept { return m_stopcr; }
+    void set_dir(OptDir dir) noexcept { m_dir = dir; }
+
+    void seed(long s) { nlopt_srand(s); }
+};
+
+template<nlopt_algorithm glob, nlopt_algorithm loc>
+class NLoptOpt<NLoptAlgComb<glob, loc>>: public NLoptOpt<NLoptAlg<glob>>
+{
+    using Base = NLoptOpt<NLoptAlg<glob>>;
+public:
+
+    template<class Fn, size_t N>
+    Result<N> optimize(Fn&& f,
+                       const Input<N> &initvals,
+                       const Bounds<N>& bounds)
+    {
+        NLopt nl_glob{glob, N}, nl_loc{loc, N};
+
+        Base::set_up(nl_glob, bounds);
+        Base::set_up(nl_loc, bounds);
+        nlopt_set_local_optimizer(nl_glob.ptr, nl_loc.ptr);
+
+        return Base::optimize(nl_glob, std::forward<Fn>(f), initvals);
+    }
+
+    explicit NLoptOpt(StopCriteria stopcr = {}) : Base{stopcr} {}
+};
+
+} // namespace detail;
+
+// Optimizers based on NLopt.
+template<class M> class Optimizer<M, detail::NLoptOnly<M>> {
+    detail::NLoptOpt<M> m_opt;
+
+public:
+
+    Optimizer& to_max() { m_opt.set_dir(detail::OptDir::MAX); return *this; }
+    Optimizer& to_min() { m_opt.set_dir(detail::OptDir::MIN); return *this; }
+
+    template<class Func, size_t N>
+    Result<N> optimize(Func&& func,
+                       const Input<N> &initvals,
+                       const Bounds<N>& bounds)
+    {
+        return m_opt.optimize(std::forward<Func>(func), initvals, bounds);
+    }
+
+    explicit Optimizer(StopCriteria stopcr = {}) : m_opt(stopcr) {}
+
+    Optimizer &set_criteria(const StopCriteria &cr)
+    {
+        m_opt.set_criteria(cr); return *this;
+    }
+
+    const StopCriteria &get_criteria() const { return m_opt.get_criteria(); }
+
+    void seed(long s) { m_opt.seed(s); }
+};
+
+template<size_t N> Bounds<N> bounds(const Bound (&b) [N]) { return detail::to_arr(b); }
+template<size_t N> Input<N> initvals(const double (&a) [N]) { return detail::to_arr(a); }
+template<size_t N> auto score_gradient(double s, const double (&grad)[N])
+{
+    return ScoreGradient<N>(s, detail::to_arr(grad));
+}
+
+// Predefinded NLopt algorithms that are used in the codebase
+using AlgNLoptGenetic = detail::NLoptAlgComb<NLOPT_GN_ESCH>;
+using AlgNLoptSubplex = detail::NLoptAlg<NLOPT_LN_SBPLX>;
+using AlgNLoptSimplex = detail::NLoptAlg<NLOPT_LN_NELDERMEAD>;
+
+// TODO: define others if needed...
+
+// Helper defs for pre-crafted global and local optimizers that work well.
+using DefaultGlobalOptimizer = Optimizer<AlgNLoptGenetic>;
+using DefaultLocalOptimizer  = Optimizer<AlgNLoptSubplex>;
+
+}} // namespace Slic3r::opt
+
+#endif // NLOPTOPTIMIZER_HPP
diff --git a/src/libslic3r/Point.hpp b/src/libslic3r/Point.hpp
index b818cd8be..8c1c69fde 100644
--- a/src/libslic3r/Point.hpp
+++ b/src/libslic3r/Point.hpp
@@ -60,10 +60,13 @@ inline int64_t cross2(const Vec2i64 &v1, const Vec2i64 &v2) { return v1(0) * v2(
 inline float   cross2(const Vec2f   &v1, const Vec2f   &v2) { return v1(0) * v2(1) - v1(1) * v2(0); }
 inline double  cross2(const Vec2d   &v1, const Vec2d   &v2) { return v1(0) * v2(1) - v1(1) * v2(0); }
 
-inline Vec2i32 to_2d(const Vec2i32 &pt3) { return Vec2i32(pt3(0), pt3(1)); }
-inline Vec2i64 to_2d(const Vec3i64 &pt3) { return Vec2i64(pt3(0), pt3(1)); }
-inline Vec2f   to_2d(const Vec3f   &pt3) { return Vec2f  (pt3(0), pt3(1)); }
-inline Vec2d   to_2d(const Vec3d   &pt3) { return Vec2d  (pt3(0), pt3(1)); }
+template<class T, int N> Eigen::Matrix<T,  2, 1, Eigen::DontAlign>
+to_2d(const Eigen::Matrix<T,  N, 1, Eigen::DontAlign> &ptN) { return {ptN(0), ptN(1)}; }
+
+//inline Vec2i32 to_2d(const Vec3i32 &pt3) { return Vec2i32(pt3(0), pt3(1)); }
+//inline Vec2i64 to_2d(const Vec3i64 &pt3) { return Vec2i64(pt3(0), pt3(1)); }
+//inline Vec2f   to_2d(const Vec3f   &pt3) { return Vec2f  (pt3(0), pt3(1)); }
+//inline Vec2d   to_2d(const Vec3d   &pt3) { return Vec2d  (pt3(0), pt3(1)); }
 
 inline Vec3d   to_3d(const Vec2d &v, double z) { return Vec3d(v(0), v(1), z); }
 inline Vec3f   to_3d(const Vec2f &v, float z) { return Vec3f(v(0), v(1), z); }
diff --git a/src/libslic3r/PrintConfig.cpp b/src/libslic3r/PrintConfig.cpp
index a25292298..5c1ce4b7f 100644
--- a/src/libslic3r/PrintConfig.cpp
+++ b/src/libslic3r/PrintConfig.cpp
@@ -2715,7 +2715,7 @@ void PrintConfigDef::init_sla_params()
     def->set_default_value(new ConfigOptionBool(true));
 
     def = this->add("support_head_front_diameter", coFloat);
-    def->label = L("Support head front diameter");
+    def->label = L("Pinhead front diameter");
     def->category = L("Supports");
     def->tooltip = L("Diameter of the pointing side of the head");
     def->sidetext = L("mm");
@@ -2724,7 +2724,7 @@ void PrintConfigDef::init_sla_params()
     def->set_default_value(new ConfigOptionFloat(0.4));
 
     def = this->add("support_head_penetration", coFloat);
-    def->label = L("Support head penetration");
+    def->label = L("Head penetration");
     def->category = L("Supports");
     def->tooltip = L("How much the pinhead has to penetrate the model surface");
     def->sidetext = L("mm");
@@ -2733,7 +2733,7 @@ void PrintConfigDef::init_sla_params()
     def->set_default_value(new ConfigOptionFloat(0.2));
 
     def = this->add("support_head_width", coFloat);
-    def->label = L("Support head width");
+    def->label = L("Pinhead width");
     def->category = L("Supports");
     def->tooltip = L("Width from the back sphere center to the front sphere center");
     def->sidetext = L("mm");
@@ -2743,7 +2743,7 @@ void PrintConfigDef::init_sla_params()
     def->set_default_value(new ConfigOptionFloat(1.0));
 
     def = this->add("support_pillar_diameter", coFloat);
-    def->label = L("Support pillar diameter");
+    def->label = L("Pillar diameter");
     def->category = L("Supports");
     def->tooltip = L("Diameter in mm of the support pillars");
     def->sidetext = L("mm");
@@ -2751,6 +2751,17 @@ void PrintConfigDef::init_sla_params()
     def->max = 15;
     def->mode = comSimple;
     def->set_default_value(new ConfigOptionFloat(1.0));
+
+    def = this->add("support_small_pillar_diameter_percent", coPercent);
+    def->label = L("Small pillar diameter percent");
+    def->category = L("Supports");
+    def->tooltip = L("The percentage of smaller pillars compared to the normal pillar diameter "
+                     "which are used in problematic areas where a normal pilla cannot fit.");
+    def->sidetext = L("%");
+    def->min = 1;
+    def->max = 100;
+    def->mode = comExpert;
+    def->set_default_value(new ConfigOptionPercent(50));
     
     def = this->add("support_max_bridges_on_pillar", coInt);
     def->label = L("Max bridges on a pillar");
@@ -2763,7 +2774,7 @@ void PrintConfigDef::init_sla_params()
     def->set_default_value(new ConfigOptionInt(3));
 
     def = this->add("support_pillar_connection_mode", coEnum);
-    def->label = L("Support pillar connection mode");
+    def->label = L("Pillar connection mode");
     def->tooltip = L("Controls the bridge type between two neighboring pillars."
                      " Can be zig-zag, cross (double zig-zag) or dynamic which"
                      " will automatically switch between the first two depending"
diff --git a/src/libslic3r/PrintConfig.hpp b/src/libslic3r/PrintConfig.hpp
index f28ef2a22..0213a6d6b 100644
--- a/src/libslic3r/PrintConfig.hpp
+++ b/src/libslic3r/PrintConfig.hpp
@@ -1018,6 +1018,10 @@ public:
 
     // Radius in mm of the support pillars.
     ConfigOptionFloat support_pillar_diameter /*= 0.8*/;
+
+    // The percentage of smaller pillars compared to the normal pillar diameter
+    // which are used in problematic areas where a normal pilla cannot fit.
+    ConfigOptionPercent support_small_pillar_diameter_percent;
     
     // How much bridge (supporting another pinhead) can be placed on a pillar.
     ConfigOptionInt   support_max_bridges_on_pillar;
@@ -1142,6 +1146,7 @@ protected:
         OPT_PTR(support_head_penetration);
         OPT_PTR(support_head_width);
         OPT_PTR(support_pillar_diameter);
+        OPT_PTR(support_small_pillar_diameter_percent);
         OPT_PTR(support_max_bridges_on_pillar);
         OPT_PTR(support_pillar_connection_mode);
         OPT_PTR(support_buildplate_only);
diff --git a/src/libslic3r/SLA/BoostAdapter.hpp b/src/libslic3r/SLA/BoostAdapter.hpp
index b7b3c63a6..13e0465b1 100644
--- a/src/libslic3r/SLA/BoostAdapter.hpp
+++ b/src/libslic3r/SLA/BoostAdapter.hpp
@@ -1,7 +1,9 @@
 #ifndef SLA_BOOSTADAPTER_HPP
 #define SLA_BOOSTADAPTER_HPP
 
-#include <libslic3r/SLA/Common.hpp>
+#include <libslic3r/Point.hpp>
+#include <libslic3r/BoundingBox.hpp>
+
 #include <boost/geometry.hpp>
 
 namespace boost {
diff --git a/src/libslic3r/SLA/Clustering.cpp b/src/libslic3r/SLA/Clustering.cpp
new file mode 100644
index 000000000..41ff1d4f0
--- /dev/null
+++ b/src/libslic3r/SLA/Clustering.cpp
@@ -0,0 +1,152 @@
+#include "Clustering.hpp"
+#include "boost/geometry/index/rtree.hpp"
+
+#include <libslic3r/SLA/SpatIndex.hpp>
+#include <libslic3r/SLA/BoostAdapter.hpp>
+
+namespace Slic3r { namespace sla {
+
+namespace bgi = boost::geometry::index;
+using Index3D = bgi::rtree< PointIndexEl, bgi::rstar<16, 4> /* ? */ >;
+
+namespace {
+
+bool cmp_ptidx_elements(const PointIndexEl& e1, const PointIndexEl& e2)
+{
+    return e1.second < e2.second;
+};
+
+ClusteredPoints cluster(Index3D &sindex,
+                        unsigned max_points,
+                        std::function<std::vector<PointIndexEl>(
+                            const Index3D &, const PointIndexEl &)> qfn)
+{
+    using Elems = std::vector<PointIndexEl>;
+
+    // Recursive function for visiting all the points in a given distance to
+    // each other
+    std::function<void(Elems&, Elems&)> group =
+        [&sindex, &group, max_points, qfn](Elems& pts, Elems& cluster)
+    {
+        for(auto& p : pts) {
+            std::vector<PointIndexEl> tmp = qfn(sindex, p);
+
+            std::sort(tmp.begin(), tmp.end(), cmp_ptidx_elements);
+
+            Elems newpts;
+            std::set_difference(tmp.begin(), tmp.end(),
+                                cluster.begin(), cluster.end(),
+                                std::back_inserter(newpts), cmp_ptidx_elements);
+
+            int c = max_points && newpts.size() + cluster.size() > max_points?
+                        int(max_points - cluster.size()) : int(newpts.size());
+
+            cluster.insert(cluster.end(), newpts.begin(), newpts.begin() + c);
+            std::sort(cluster.begin(), cluster.end(), cmp_ptidx_elements);
+
+            if(!newpts.empty() && (!max_points || cluster.size() < max_points))
+                group(newpts, cluster);
+        }
+    };
+
+    std::vector<Elems> clusters;
+    for(auto it = sindex.begin(); it != sindex.end();) {
+        Elems cluster = {};
+        Elems pts = {*it};
+        group(pts, cluster);
+
+        for(auto& c : cluster) sindex.remove(c);
+        it = sindex.begin();
+
+        clusters.emplace_back(cluster);
+    }
+
+    ClusteredPoints result;
+    for(auto& cluster : clusters) {
+        result.emplace_back();
+        for(auto c : cluster) result.back().emplace_back(c.second);
+    }
+
+    return result;
+}
+
+std::vector<PointIndexEl> distance_queryfn(const Index3D& sindex,
+                                           const PointIndexEl& p,
+                                           double dist,
+                                           unsigned max_points)
+{
+    std::vector<PointIndexEl> tmp; tmp.reserve(max_points);
+    sindex.query(
+        bgi::nearest(p.first, max_points),
+        std::back_inserter(tmp)
+        );
+
+    for(auto it = tmp.begin(); it < tmp.end(); ++it)
+        if((p.first - it->first).norm() > dist) it = tmp.erase(it);
+
+    return tmp;
+}
+
+} // namespace
+
+// Clustering a set of points by the given criteria
+ClusteredPoints cluster(
+    const std::vector<unsigned>& indices,
+    std::function<Vec3d(unsigned)> pointfn,
+    double dist,
+    unsigned max_points)
+{
+    // A spatial index for querying the nearest points
+    Index3D sindex;
+
+    // Build the index
+    for(auto idx : indices) sindex.insert( std::make_pair(pointfn(idx), idx));
+
+    return cluster(sindex, max_points,
+                   [dist, max_points](const Index3D& sidx, const PointIndexEl& p)
+                   {
+                       return distance_queryfn(sidx, p, dist, max_points);
+                   });
+}
+
+// Clustering a set of points by the given criteria
+ClusteredPoints cluster(
+    const std::vector<unsigned>& indices,
+    std::function<Vec3d(unsigned)> pointfn,
+    std::function<bool(const PointIndexEl&, const PointIndexEl&)> predicate,
+    unsigned max_points)
+{
+    // A spatial index for querying the nearest points
+    Index3D sindex;
+
+    // Build the index
+    for(auto idx : indices) sindex.insert( std::make_pair(pointfn(idx), idx));
+
+    return cluster(sindex, max_points,
+                   [max_points, predicate](const Index3D& sidx, const PointIndexEl& p)
+                   {
+                       std::vector<PointIndexEl> tmp; tmp.reserve(max_points);
+                       sidx.query(bgi::satisfies([p, predicate](const PointIndexEl& e){
+                                      return predicate(p, e);
+                                  }), std::back_inserter(tmp));
+                       return tmp;
+                   });
+}
+
+ClusteredPoints cluster(const Eigen::MatrixXd& pts, double dist, unsigned max_points)
+{
+    // A spatial index for querying the nearest points
+    Index3D sindex;
+
+    // Build the index
+    for(Eigen::Index i = 0; i < pts.rows(); i++)
+        sindex.insert(std::make_pair(Vec3d(pts.row(i)), unsigned(i)));
+
+    return cluster(sindex, max_points,
+                   [dist, max_points](const Index3D& sidx, const PointIndexEl& p)
+                   {
+                       return distance_queryfn(sidx, p, dist, max_points);
+                   });
+}
+
+}} // namespace Slic3r::sla
diff --git a/src/libslic3r/SLA/Clustering.hpp b/src/libslic3r/SLA/Clustering.hpp
index 1b0d47d95..269ec2882 100644
--- a/src/libslic3r/SLA/Clustering.hpp
+++ b/src/libslic3r/SLA/Clustering.hpp
@@ -2,7 +2,8 @@
 #define SLA_CLUSTERING_HPP
 
 #include <vector>
-#include <libslic3r/SLA/Common.hpp>
+
+#include <libslic3r/Point.hpp>
 #include <libslic3r/SLA/SpatIndex.hpp>
 
 namespace Slic3r { namespace sla {
@@ -16,7 +17,7 @@ ClusteredPoints cluster(const std::vector<unsigned>& indices,
                         double dist,
                         unsigned max_points);
 
-ClusteredPoints cluster(const PointSet& points,
+ClusteredPoints cluster(const Eigen::MatrixXd& points,
                         double dist,
                         unsigned max_points);
 
@@ -26,5 +27,56 @@ ClusteredPoints cluster(
     std::function<bool(const PointIndexEl&, const PointIndexEl&)> predicate,
     unsigned max_points);
 
-}}
+// This function returns the position of the centroid in the input 'clust'
+// vector of point indices.
+template<class DistFn, class PointFn>
+long cluster_centroid(const ClusterEl &clust, PointFn pointfn, DistFn df)
+{
+    switch(clust.size()) {
+    case 0: /* empty cluster */ return -1;
+    case 1: /* only one element */ return 0;
+    case 2: /* if two elements, there is no center */ return 0;
+    default: ;
+    }
+
+    // The function works by calculating for each point the average distance
+    // from all the other points in the cluster. We create a selector bitmask of
+    // the same size as the cluster. The bitmask will have two true bits and
+    // false bits for the rest of items and we will loop through all the
+    // permutations of the bitmask (combinations of two points). Get the
+    // distance for the two points and add the distance to the averages.
+    // The point with the smallest average than wins.
+
+    // The complexity should be O(n^2) but we will mostly apply this function
+    // for small clusters only (cca 3 elements)
+
+    std::vector<bool> sel(clust.size(), false);   // create full zero bitmask
+    std::fill(sel.end() - 2, sel.end(), true);    // insert the two ones
+    std::vector<double> avgs(clust.size(), 0.0);  // store the average distances
+
+    do {
+        std::array<size_t, 2> idx;
+        for(size_t i = 0, j = 0; i < clust.size(); i++)
+            if(sel[i]) idx[j++] = i;
+
+        double d = df(pointfn(clust[idx[0]]),
+                      pointfn(clust[idx[1]]));
+
+        // add the distance to the sums for both associated points
+        for(auto i : idx) avgs[i] += d;
+
+        // now continue with the next permutation of the bitmask with two 1s
+    } while(std::next_permutation(sel.begin(), sel.end()));
+
+    // Divide by point size in the cluster to get the average (may be redundant)
+    for(auto& a : avgs) a /= clust.size();
+
+    // get the lowest average distance and return the index
+    auto minit = std::min_element(avgs.begin(), avgs.end());
+    return long(minit - avgs.begin());
+}
+
+
+}} // namespace Slic3r::sla
+
 #endif // CLUSTERING_HPP
diff --git a/src/libslic3r/SLA/Common.hpp b/src/libslic3r/SLA/Common.hpp
deleted file mode 100644
index ca616cabc..000000000
--- a/src/libslic3r/SLA/Common.hpp
+++ /dev/null
@@ -1,27 +0,0 @@
-#ifndef SLA_COMMON_HPP
-#define SLA_COMMON_HPP
-
-#include <memory>
-#include <vector>
-#include <numeric>
-#include <functional>
-#include <Eigen/Geometry>
-
-
-namespace Slic3r {
-    
-// Typedefs from Point.hpp
-typedef Eigen::Matrix<float, 3, 1, Eigen::DontAlign> Vec3f;
-typedef Eigen::Matrix<double, 3, 1, Eigen::DontAlign> Vec3d;
-typedef Eigen::Matrix<int, 3, 1, Eigen::DontAlign> Vec3i;
-typedef Eigen::Matrix<int, 4, 1, Eigen::DontAlign> Vec4i;
-
-namespace sla {
-
-using PointSet = Eigen::MatrixXd;
-
-} // namespace sla
-} // namespace Slic3r
-
-
-#endif // SLASUPPORTTREE_HPP
diff --git a/src/libslic3r/SLA/Contour3D.cpp b/src/libslic3r/SLA/Contour3D.cpp
index 408465d43..96d10af20 100644
--- a/src/libslic3r/SLA/Contour3D.cpp
+++ b/src/libslic3r/SLA/Contour3D.cpp
@@ -1,5 +1,5 @@
 #include <libslic3r/SLA/Contour3D.hpp>
-#include <libslic3r/SLA/EigenMesh3D.hpp>
+#include <libslic3r/SLA/IndexedMesh.hpp>
 
 #include <libslic3r/Format/objparser.hpp>
 
@@ -27,7 +27,7 @@ Contour3D::Contour3D(TriangleMesh &&trmesh)
     faces3.swap(trmesh.its.indices);
 }
 
-Contour3D::Contour3D(const EigenMesh3D &emesh) {
+Contour3D::Contour3D(const IndexedMesh &emesh) {
     points.reserve(emesh.vertices().size());
     faces3.reserve(emesh.indices().size());
     
diff --git a/src/libslic3r/SLA/Contour3D.hpp b/src/libslic3r/SLA/Contour3D.hpp
index 295612f19..3380cd6ab 100644
--- a/src/libslic3r/SLA/Contour3D.hpp
+++ b/src/libslic3r/SLA/Contour3D.hpp
@@ -1,13 +1,16 @@
 #ifndef SLA_CONTOUR3D_HPP
 #define SLA_CONTOUR3D_HPP
 
-#include <libslic3r/SLA/Common.hpp>
-
 #include <libslic3r/TriangleMesh.hpp>
 
-namespace Slic3r { namespace sla {
+namespace Slic3r {
 
-class EigenMesh3D;
+// Used for quads (TODO: remove this, and convert quads to triangles in OpenVDBUtils)
+using Vec4i = Eigen::Matrix<int, 4, 1, Eigen::DontAlign>;
+
+namespace sla {
+
+class IndexedMesh;
 
 /// Dumb vertex mesh consisting of triangles (or) quads. Capable of merging with
 /// other meshes of this type and converting to and from other mesh formats.
@@ -19,7 +22,7 @@ struct Contour3D {
     Contour3D() = default;
     Contour3D(const TriangleMesh &trmesh);
     Contour3D(TriangleMesh &&trmesh);
-    Contour3D(const EigenMesh3D  &emesh);
+    Contour3D(const IndexedMesh  &emesh);
     
     Contour3D& merge(const Contour3D& ctr);
     Contour3D& merge(const Pointf3s& triangles);
diff --git a/src/libslic3r/SLA/Hollowing.cpp b/src/libslic3r/SLA/Hollowing.cpp
index 0dd9436a1..5334054a0 100644
--- a/src/libslic3r/SLA/Hollowing.cpp
+++ b/src/libslic3r/SLA/Hollowing.cpp
@@ -3,11 +3,10 @@
 #include <libslic3r/OpenVDBUtils.hpp>
 #include <libslic3r/TriangleMesh.hpp>
 #include <libslic3r/SLA/Hollowing.hpp>
-#include <libslic3r/SLA/Contour3D.hpp>
-#include <libslic3r/SLA/EigenMesh3D.hpp>
-#include <libslic3r/SLA/SupportTreeBuilder.hpp>
+#include <libslic3r/SLA/IndexedMesh.hpp>
 #include <libslic3r/ClipperUtils.hpp>
 #include <libslic3r/SimplifyMesh.hpp>
+#include <libslic3r/SLA/SupportTreeMesher.hpp>
 
 #include <boost/log/trivial.hpp>
 
@@ -160,7 +159,7 @@ bool DrainHole::get_intersections(const Vec3f& s, const Vec3f& dir,
     const Eigen::ParametrizedLine<float, 3> ray(s, dir.normalized());
 
     for (size_t i=0; i<2; ++i)
-        out[i] = std::make_pair(sla::EigenMesh3D::hit_result::infty(), Vec3d::Zero());
+        out[i] = std::make_pair(sla::IndexedMesh::hit_result::infty(), Vec3d::Zero());
 
     const float sqr_radius = pow(radius, 2.f);
 
diff --git a/src/libslic3r/SLA/Hollowing.hpp b/src/libslic3r/SLA/Hollowing.hpp
index cc7d310ea..1f65fa8b7 100644
--- a/src/libslic3r/SLA/Hollowing.hpp
+++ b/src/libslic3r/SLA/Hollowing.hpp
@@ -2,7 +2,6 @@
 #define SLA_HOLLOWING_HPP
 
 #include <memory>
-#include <libslic3r/SLA/Common.hpp>
 #include <libslic3r/SLA/Contour3D.hpp>
 #include <libslic3r/SLA/JobController.hpp>
 
diff --git a/src/libslic3r/SLA/Common.cpp b/src/libslic3r/SLA/IndexedMesh.cpp
similarity index 53%
rename from src/libslic3r/SLA/Common.cpp
rename to src/libslic3r/SLA/IndexedMesh.cpp
index a7420a7fb..573b62b6d 100644
--- a/src/libslic3r/SLA/Common.cpp
+++ b/src/libslic3r/SLA/IndexedMesh.cpp
@@ -1,187 +1,18 @@
-#include <cmath>
-#include <libslic3r/SLA/Common.hpp>
-#include <libslic3r/SLA/Concurrency.hpp>
-#include <libslic3r/SLA/SpatIndex.hpp>
-#include <libslic3r/SLA/EigenMesh3D.hpp>
-#include <libslic3r/SLA/Contour3D.hpp>
-#include <libslic3r/SLA/Clustering.hpp>
+#include "IndexedMesh.hpp"
+#include "Concurrency.hpp"
+
 #include <libslic3r/AABBTreeIndirect.hpp>
+#include <libslic3r/TriangleMesh.hpp>
 
-// for concave hull merging decisions
-#include <libslic3r/SLA/BoostAdapter.hpp>
-#include "boost/geometry/index/rtree.hpp"
-
-#ifdef _MSC_VER
-#pragma warning(push)
-#pragma warning(disable: 4244)
-#pragma warning(disable: 4267)
-#endif
-
-
-#include <igl/remove_duplicate_vertices.h>
+#include <numeric>
 
 #ifdef SLIC3R_HOLE_RAYCASTER
-  #include <libslic3r/SLA/Hollowing.hpp>
+#include <libslic3r/SLA/Hollowing.hpp>
 #endif
 
+namespace Slic3r { namespace sla {
 
-#ifdef _MSC_VER
-#pragma warning(pop)
-#endif
-
-
-namespace Slic3r {
-namespace sla {
-
-
-/* **************************************************************************
- * PointIndex implementation
- * ************************************************************************** */
-
-class PointIndex::Impl {
-public:
-    using BoostIndex = boost::geometry::index::rtree< PointIndexEl,
-                                                     boost::geometry::index::rstar<16, 4> /* ? */ >;
-    
-    BoostIndex m_store;
-};
-
-PointIndex::PointIndex(): m_impl(new Impl()) {}
-PointIndex::~PointIndex() {}
-
-PointIndex::PointIndex(const PointIndex &cpy): m_impl(new Impl(*cpy.m_impl)) {}
-PointIndex::PointIndex(PointIndex&& cpy): m_impl(std::move(cpy.m_impl)) {}
-
-PointIndex& PointIndex::operator=(const PointIndex &cpy)
-{
-    m_impl.reset(new Impl(*cpy.m_impl));
-    return *this;
-}
-
-PointIndex& PointIndex::operator=(PointIndex &&cpy)
-{
-    m_impl.swap(cpy.m_impl);
-    return *this;
-}
-
-void PointIndex::insert(const PointIndexEl &el)
-{
-    m_impl->m_store.insert(el);
-}
-
-bool PointIndex::remove(const PointIndexEl& el)
-{
-    return m_impl->m_store.remove(el) == 1;
-}
-
-std::vector<PointIndexEl>
-PointIndex::query(std::function<bool(const PointIndexEl &)> fn) const
-{
-    namespace bgi = boost::geometry::index;
-    
-    std::vector<PointIndexEl> ret;
-    m_impl->m_store.query(bgi::satisfies(fn), std::back_inserter(ret));
-    return ret;
-}
-
-std::vector<PointIndexEl> PointIndex::nearest(const Vec3d &el, unsigned k = 1) const
-{
-    namespace bgi = boost::geometry::index;
-    std::vector<PointIndexEl> ret; ret.reserve(k);
-    m_impl->m_store.query(bgi::nearest(el, k), std::back_inserter(ret));
-    return ret;
-}
-
-size_t PointIndex::size() const
-{
-    return m_impl->m_store.size();
-}
-
-void PointIndex::foreach(std::function<void (const PointIndexEl &)> fn)
-{
-    for(auto& el : m_impl->m_store) fn(el);
-}
-
-void PointIndex::foreach(std::function<void (const PointIndexEl &)> fn) const
-{
-    for(const auto &el : m_impl->m_store) fn(el);
-}
-
-/* **************************************************************************
- * BoxIndex implementation
- * ************************************************************************** */
-
-class BoxIndex::Impl {
-public:
-    using BoostIndex = boost::geometry::index::
-        rtree<BoxIndexEl, boost::geometry::index::rstar<16, 4> /* ? */>;
-    
-    BoostIndex m_store;
-};
-
-BoxIndex::BoxIndex(): m_impl(new Impl()) {}
-BoxIndex::~BoxIndex() {}
-
-BoxIndex::BoxIndex(const BoxIndex &cpy): m_impl(new Impl(*cpy.m_impl)) {}
-BoxIndex::BoxIndex(BoxIndex&& cpy): m_impl(std::move(cpy.m_impl)) {}
-
-BoxIndex& BoxIndex::operator=(const BoxIndex &cpy)
-{
-    m_impl.reset(new Impl(*cpy.m_impl));
-    return *this;
-}
-
-BoxIndex& BoxIndex::operator=(BoxIndex &&cpy)
-{
-    m_impl.swap(cpy.m_impl);
-    return *this;
-}
-
-void BoxIndex::insert(const BoxIndexEl &el)
-{
-    m_impl->m_store.insert(el);
-}
-
-bool BoxIndex::remove(const BoxIndexEl& el)
-{
-    return m_impl->m_store.remove(el) == 1;
-}
-
-std::vector<BoxIndexEl> BoxIndex::query(const BoundingBox &qrbb,
-                                        BoxIndex::QueryType qt)
-{
-    namespace bgi = boost::geometry::index;
-    
-    std::vector<BoxIndexEl> ret; ret.reserve(m_impl->m_store.size());
-    
-    switch (qt) {
-    case qtIntersects:
-        m_impl->m_store.query(bgi::intersects(qrbb), std::back_inserter(ret));
-        break;
-    case qtWithin:
-        m_impl->m_store.query(bgi::within(qrbb), std::back_inserter(ret));
-    }
-    
-    return ret;
-}
-
-size_t BoxIndex::size() const
-{
-    return m_impl->m_store.size();
-}
-
-void BoxIndex::foreach(std::function<void (const BoxIndexEl &)> fn)
-{
-    for(auto& el : m_impl->m_store) fn(el);
-}
-
-
-/* ****************************************************************************
- * EigenMesh3D implementation
- * ****************************************************************************/
-
-
-class EigenMesh3D::AABBImpl {
+class IndexedMesh::AABBImpl {
 private:
     AABBTreeIndirect::Tree3f m_tree;
 
@@ -189,7 +20,7 @@ public:
     void init(const TriangleMesh& tm)
     {
         m_tree = AABBTreeIndirect::build_aabb_tree_over_indexed_triangle_set(
-                    tm.its.vertices, tm.its.indices);
+            tm.its.vertices, tm.its.indices);
     }
 
     void intersect_ray(const TriangleMesh& tm,
@@ -215,9 +46,9 @@ public:
         size_t idx_unsigned = 0;
         Vec3d closest_vec3d(closest);
         double dist = AABBTreeIndirect::squared_distance_to_indexed_triangle_set(
-                          tm.its.vertices,
-                          tm.its.indices,
-                          m_tree, point, idx_unsigned, closest_vec3d);
+            tm.its.vertices,
+            tm.its.indices,
+            m_tree, point, idx_unsigned, closest_vec3d);
         i = int(idx_unsigned);
         closest = closest_vec3d;
         return dist;
@@ -226,72 +57,71 @@ public:
 
 static const constexpr double MESH_EPS = 1e-6;
 
-EigenMesh3D::EigenMesh3D(const TriangleMesh& tmesh)
+IndexedMesh::IndexedMesh(const TriangleMesh& tmesh)
     : m_aabb(new AABBImpl()), m_tm(&tmesh)
 {
     auto&& bb = tmesh.bounding_box();
     m_ground_level += bb.min(Z);
-    
+
     // Build the AABB accelaration tree
     m_aabb->init(tmesh);
 }
 
-EigenMesh3D::~EigenMesh3D() {}
+IndexedMesh::~IndexedMesh() {}
 
-EigenMesh3D::EigenMesh3D(const EigenMesh3D &other):
+IndexedMesh::IndexedMesh(const IndexedMesh &other):
     m_tm(other.m_tm), m_ground_level(other.m_ground_level),
     m_aabb( new AABBImpl(*other.m_aabb) ) {}
 
 
-EigenMesh3D &EigenMesh3D::operator=(const EigenMesh3D &other)
+IndexedMesh &IndexedMesh::operator=(const IndexedMesh &other)
 {
     m_tm = other.m_tm;
     m_ground_level = other.m_ground_level;
     m_aabb.reset(new AABBImpl(*other.m_aabb)); return *this;
 }
 
-EigenMesh3D &EigenMesh3D::operator=(EigenMesh3D &&other) = default;
+IndexedMesh &IndexedMesh::operator=(IndexedMesh &&other) = default;
 
-EigenMesh3D::EigenMesh3D(EigenMesh3D &&other) = default;
+IndexedMesh::IndexedMesh(IndexedMesh &&other) = default;
 
 
 
-const std::vector<Vec3f>& EigenMesh3D::vertices() const
+const std::vector<Vec3f>& IndexedMesh::vertices() const
 {
     return m_tm->its.vertices;
 }
 
 
 
-const std::vector<Vec3i>& EigenMesh3D::indices()  const
+const std::vector<Vec3i>& IndexedMesh::indices()  const
 {
     return m_tm->its.indices;
 }
 
 
 
-const Vec3f& EigenMesh3D::vertices(size_t idx) const
+const Vec3f& IndexedMesh::vertices(size_t idx) const
 {
     return m_tm->its.vertices[idx];
 }
 
 
 
-const Vec3i& EigenMesh3D::indices(size_t idx) const
+const Vec3i& IndexedMesh::indices(size_t idx) const
 {
     return m_tm->its.indices[idx];
 }
 
 
 
-Vec3d EigenMesh3D::normal_by_face_id(int face_id) const {
+Vec3d IndexedMesh::normal_by_face_id(int face_id) const {
     return m_tm->stl.facet_start[face_id].normal.cast<double>();
 }
 
 
-
-EigenMesh3D::hit_result
-EigenMesh3D::query_ray_hit(const Vec3d &s, const Vec3d &dir) const
+IndexedMesh::hit_result
+IndexedMesh::query_ray_hit(const Vec3d &s, const Vec3d &dir) const
 {
     assert(is_approx(dir.norm(), 1.));
     igl::Hit hit;
@@ -319,13 +149,13 @@ EigenMesh3D::query_ray_hit(const Vec3d &s, const Vec3d &dir) const
     return ret;
 }
 
-std::vector<EigenMesh3D::hit_result>
-EigenMesh3D::query_ray_hits(const Vec3d &s, const Vec3d &dir) const
+std::vector<IndexedMesh::hit_result>
+IndexedMesh::query_ray_hits(const Vec3d &s, const Vec3d &dir) const
 {
-    std::vector<EigenMesh3D::hit_result> outs;
+    std::vector<IndexedMesh::hit_result> outs;
     std::vector<igl::Hit> hits;
     m_aabb->intersect_ray(*m_tm, s, dir, hits);
-    
+
     // The sort is necessary, the hits are not always sorted.
     std::sort(hits.begin(), hits.end(),
               [](const igl::Hit& a, const igl::Hit& b) { return a.t < b.t; });
@@ -334,13 +164,13 @@ EigenMesh3D::query_ray_hits(const Vec3d &s, const Vec3d &dir) const
     // along an axis of a cube due to floating-point approximations in igl (?)
     hits.erase(std::unique(hits.begin(), hits.end(),
                            [](const igl::Hit& a, const igl::Hit& b)
-                              { return a.t == b.t; }),
+                           { return a.t == b.t; }),
                hits.end());
 
     //  Convert the igl::Hit into hit_result
     outs.reserve(hits.size());
     for (const igl::Hit& hit : hits) {
-        outs.emplace_back(EigenMesh3D::hit_result(*this));
+        outs.emplace_back(IndexedMesh::hit_result(*this));
         outs.back().m_t = double(hit.t);
         outs.back().m_dir = dir;
         outs.back().m_source = s;
@@ -355,8 +185,8 @@ EigenMesh3D::query_ray_hits(const Vec3d &s, const Vec3d &dir) const
 
 
 #ifdef SLIC3R_HOLE_RAYCASTER
-EigenMesh3D::hit_result EigenMesh3D::filter_hits(
-                     const std::vector<EigenMesh3D::hit_result>& object_hits) const
+IndexedMesh::hit_result IndexedMesh::filter_hits(
+    const std::vector<IndexedMesh::hit_result>& object_hits) const
 {
     assert(! m_holes.empty());
     hit_result out(*this);
@@ -377,7 +207,7 @@ EigenMesh3D::hit_result EigenMesh3D::filter_hits(
     };
     std::vector<HoleHit> hole_isects;
     hole_isects.reserve(m_holes.size());
-    
+
     auto sf = s.cast<float>();
     auto dirf = dir.cast<float>();
 
@@ -452,7 +282,7 @@ EigenMesh3D::hit_result EigenMesh3D::filter_hits(
 #endif
 
 
-double EigenMesh3D::squared_distance(const Vec3d &p, int& i, Vec3d& c) const {
+double IndexedMesh::squared_distance(const Vec3d &p, int& i, Vec3d& c) const {
     double sqdst = 0;
     Eigen::Matrix<double, 1, 3> pp = p;
     Eigen::Matrix<double, 1, 3> cc;
@@ -461,31 +291,19 @@ double EigenMesh3D::squared_distance(const Vec3d &p, int& i, Vec3d& c) const {
     return sqdst;
 }
 
-/* ****************************************************************************
- * Misc functions
- * ****************************************************************************/
 
-namespace  {
-
-bool point_on_edge(const Vec3d& p, const Vec3d& e1, const Vec3d& e2,
-                   double eps = 0.05)
+static bool point_on_edge(const Vec3d& p, const Vec3d& e1, const Vec3d& e2,
+                          double eps = 0.05)
 {
     using Line3D = Eigen::ParametrizedLine<double, 3>;
-    
+
     auto line = Line3D::Through(e1, e2);
     double d = line.distance(p);
     return std::abs(d) < eps;
 }
 
-template<class Vec> double distance(const Vec& pp1, const Vec& pp2) {
-    auto p = pp2 - pp1;
-    return std::sqrt(p.transpose() * p);
-}
-
-}
-
 PointSet normals(const PointSet& points,
-                 const EigenMesh3D& mesh,
+                 const IndexedMesh& mesh,
                  double eps,
                  std::function<void()> thr, // throw on cancel
                  const std::vector<unsigned>& pt_indices)
@@ -531,11 +349,11 @@ PointSet normals(const PointSet& points,
             // ic will mark a single vertex.
             int ia = -1, ib = -1, ic = -1;
 
-            if (std::abs(distance(p, p1)) < eps) {
+            if (std::abs((p - p1).norm()) < eps) {
                 ic = trindex(0);
-            } else if (std::abs(distance(p, p2)) < eps) {
+            } else if (std::abs((p - p2).norm()) < eps) {
                 ic = trindex(1);
-            } else if (std::abs(distance(p, p3)) < eps) {
+            } else if (std::abs((p - p3).norm()) < eps) {
                 ic = trindex(2);
             } else if (point_on_edge(p, p1, p2, eps)) {
                 ia = trindex(0);
@@ -612,148 +430,4 @@ PointSet normals(const PointSet& points,
     return ret;
 }
 
-namespace bgi = boost::geometry::index;
-using Index3D = bgi::rtree< PointIndexEl, bgi::rstar<16, 4> /* ? */ >;
-
-namespace {
-
-bool cmp_ptidx_elements(const PointIndexEl& e1, const PointIndexEl& e2)
-{
-    return e1.second < e2.second;
-};
-
-ClusteredPoints cluster(Index3D &sindex,
-                        unsigned max_points,
-                        std::function<std::vector<PointIndexEl>(
-                            const Index3D &, const PointIndexEl &)> qfn)
-{
-    using Elems = std::vector<PointIndexEl>;
-    
-    // Recursive function for visiting all the points in a given distance to
-    // each other
-    std::function<void(Elems&, Elems&)> group =
-        [&sindex, &group, max_points, qfn](Elems& pts, Elems& cluster)
-    {        
-        for(auto& p : pts) {
-            std::vector<PointIndexEl> tmp = qfn(sindex, p);
-            
-            std::sort(tmp.begin(), tmp.end(), cmp_ptidx_elements);
-            
-            Elems newpts;
-            std::set_difference(tmp.begin(), tmp.end(),
-                                cluster.begin(), cluster.end(),
-                                std::back_inserter(newpts), cmp_ptidx_elements);
-            
-            int c = max_points && newpts.size() + cluster.size() > max_points?
-                        int(max_points - cluster.size()) : int(newpts.size());
-            
-            cluster.insert(cluster.end(), newpts.begin(), newpts.begin() + c);
-            std::sort(cluster.begin(), cluster.end(), cmp_ptidx_elements);
-            
-            if(!newpts.empty() && (!max_points || cluster.size() < max_points))
-                group(newpts, cluster);
-        }
-    };
-    
-    std::vector<Elems> clusters;
-    for(auto it = sindex.begin(); it != sindex.end();) {
-        Elems cluster = {};
-        Elems pts = {*it};
-        group(pts, cluster);
-        
-        for(auto& c : cluster) sindex.remove(c);
-        it = sindex.begin();
-        
-        clusters.emplace_back(cluster);
-    }
-    
-    ClusteredPoints result;
-    for(auto& cluster : clusters) {
-        result.emplace_back();
-        for(auto c : cluster) result.back().emplace_back(c.second);
-    }
-    
-    return result;
-}
-
-std::vector<PointIndexEl> distance_queryfn(const Index3D& sindex,
-                                           const PointIndexEl& p,
-                                           double dist,
-                                           unsigned max_points)
-{
-    std::vector<PointIndexEl> tmp; tmp.reserve(max_points);
-    sindex.query(
-        bgi::nearest(p.first, max_points),
-        std::back_inserter(tmp)
-        );
-    
-    for(auto it = tmp.begin(); it < tmp.end(); ++it)
-        if(distance(p.first, it->first) > dist) it = tmp.erase(it);
-    
-    return tmp;
-}
-
-} // namespace
-
-// Clustering a set of points by the given criteria
-ClusteredPoints cluster(
-    const std::vector<unsigned>& indices,
-    std::function<Vec3d(unsigned)> pointfn,
-    double dist,
-    unsigned max_points)
-{
-    // A spatial index for querying the nearest points
-    Index3D sindex;
-    
-    // Build the index
-    for(auto idx : indices) sindex.insert( std::make_pair(pointfn(idx), idx));
-    
-    return cluster(sindex, max_points,
-                   [dist, max_points](const Index3D& sidx, const PointIndexEl& p)
-                   {
-                       return distance_queryfn(sidx, p, dist, max_points);
-                   });
-}
-
-// Clustering a set of points by the given criteria
-ClusteredPoints cluster(
-    const std::vector<unsigned>& indices,
-    std::function<Vec3d(unsigned)> pointfn,
-    std::function<bool(const PointIndexEl&, const PointIndexEl&)> predicate,
-    unsigned max_points)
-{
-    // A spatial index for querying the nearest points
-    Index3D sindex;
-    
-    // Build the index
-    for(auto idx : indices) sindex.insert( std::make_pair(pointfn(idx), idx));
-    
-    return cluster(sindex, max_points,
-                   [max_points, predicate](const Index3D& sidx, const PointIndexEl& p)
-                   {
-                       std::vector<PointIndexEl> tmp; tmp.reserve(max_points);
-                       sidx.query(bgi::satisfies([p, predicate](const PointIndexEl& e){
-                                      return predicate(p, e);
-                                  }), std::back_inserter(tmp));
-                       return tmp;
-                   });
-}
-
-ClusteredPoints cluster(const PointSet& pts, double dist, unsigned max_points)
-{
-    // A spatial index for querying the nearest points
-    Index3D sindex;
-    
-    // Build the index
-    for(Eigen::Index i = 0; i < pts.rows(); i++)
-        sindex.insert(std::make_pair(Vec3d(pts.row(i)), unsigned(i)));
-    
-    return cluster(sindex, max_points,
-                   [dist, max_points](const Index3D& sidx, const PointIndexEl& p)
-                   {
-                       return distance_queryfn(sidx, p, dist, max_points);
-                   });
-}
-
-} // namespace sla
-} // namespace Slic3r
+}} // namespace Slic3r::sla
diff --git a/src/libslic3r/SLA/EigenMesh3D.hpp b/src/libslic3r/SLA/IndexedMesh.hpp
similarity index 81%
rename from src/libslic3r/SLA/EigenMesh3D.hpp
rename to src/libslic3r/SLA/IndexedMesh.hpp
index b932c0c18..a72492b34 100644
--- a/src/libslic3r/SLA/EigenMesh3D.hpp
+++ b/src/libslic3r/SLA/IndexedMesh.hpp
@@ -1,8 +1,10 @@
-#ifndef SLA_EIGENMESH3D_H
-#define SLA_EIGENMESH3D_H
+#ifndef SLA_INDEXEDMESH_H
+#define SLA_INDEXEDMESH_H
 
-#include <libslic3r/SLA/Common.hpp>
+#include <memory>
+#include <vector>
 
+#include <libslic3r/Point.hpp>
 
 // There is an implementation of a hole-aware raycaster that was eventually
 // not used in production version. It is now hidden under following define
@@ -19,10 +21,12 @@ class TriangleMesh;
 
 namespace sla {
 
+using PointSet = Eigen::MatrixXd;
+
 /// An index-triangle structure for libIGL functions. Also serves as an
 /// alternative (raw) input format for the SLASupportTree.
 //  Implemented in libslic3r/SLA/Common.cpp
-class EigenMesh3D {
+class IndexedMesh {
     class AABBImpl;
     
     const TriangleMesh* m_tm;
@@ -38,15 +42,15 @@ class EigenMesh3D {
 
 public:
     
-    explicit EigenMesh3D(const TriangleMesh&);
+    explicit IndexedMesh(const TriangleMesh&);
     
-    EigenMesh3D(const EigenMesh3D& other);
-    EigenMesh3D& operator=(const EigenMesh3D&);
+    IndexedMesh(const IndexedMesh& other);
+    IndexedMesh& operator=(const IndexedMesh&);
     
-    EigenMesh3D(EigenMesh3D &&other);
-    EigenMesh3D& operator=(EigenMesh3D &&other);
+    IndexedMesh(IndexedMesh &&other);
+    IndexedMesh& operator=(IndexedMesh &&other);
     
-    ~EigenMesh3D();
+    ~IndexedMesh();
     
     inline double ground_level() const { return m_ground_level + m_gnd_offset; }
     inline void ground_level_offset(double o) { m_gnd_offset = o; }
@@ -62,15 +66,15 @@ public:
         // m_t holds a distance from m_source to the intersection.
         double m_t = infty();
         int m_face_id = -1;
-        const EigenMesh3D *m_mesh = nullptr;
+        const IndexedMesh *m_mesh = nullptr;
         Vec3d m_dir;
         Vec3d m_source;
         Vec3d m_normal;
-        friend class EigenMesh3D;
+        friend class IndexedMesh;
         
         // A valid object of this class can only be obtained from
-        // EigenMesh3D::query_ray_hit method.
-        explicit inline hit_result(const EigenMesh3D& em): m_mesh(&em) {}
+        // IndexedMesh::query_ray_hit method.
+        explicit inline hit_result(const IndexedMesh& em): m_mesh(&em) {}
     public:
         // This denotes no hit on the mesh.
         static inline constexpr double infty() { return std::numeric_limits<double>::infinity(); }
@@ -83,7 +87,7 @@ public:
         inline Vec3d position() const { return m_source + m_dir * m_t; }
         inline int face() const { return m_face_id; }
         inline bool is_valid() const { return m_mesh != nullptr; }
-        inline bool is_hit() const { return !std::isinf(m_t); }
+        inline bool is_hit() const { return m_face_id >= 0 && !std::isinf(m_t); }
 
         inline const Vec3d& normal() const {
             assert(is_valid());
@@ -107,7 +111,7 @@ public:
     // This function is currently not used anywhere, it was written when the
     // holes were subtracted on slices, that is, before we started using CGAL
     // to actually cut the holes into the mesh.
-    hit_result filter_hits(const std::vector<EigenMesh3D::hit_result>& obj_hits) const;
+    hit_result filter_hits(const std::vector<IndexedMesh::hit_result>& obj_hits) const;
 #endif
 
     // Casting a ray on the mesh, returns the distance where the hit occures.
@@ -125,16 +129,18 @@ public:
     }
 
     Vec3d normal_by_face_id(int face_id) const;
+
+    const TriangleMesh * get_triangle_mesh() const { return m_tm; }
 };
 
 // Calculate the normals for the selected points (from 'points' set) on the
 // mesh. This will call squared distance for each point.
 PointSet normals(const PointSet& points,
-    const EigenMesh3D& convert_mesh,
+    const IndexedMesh& convert_mesh,
     double eps = 0.05,  // min distance from edges
     std::function<void()> throw_on_cancel = [](){},
     const std::vector<unsigned>& selected_points = {});
 
 }} // namespace Slic3r::sla
 
-#endif // EIGENMESH3D_H
+#endif // INDEXEDMESH_H
diff --git a/src/libslic3r/SLA/JobController.hpp b/src/libslic3r/SLA/JobController.hpp
index 3baa3d12d..b815e4d6f 100644
--- a/src/libslic3r/SLA/JobController.hpp
+++ b/src/libslic3r/SLA/JobController.hpp
@@ -2,6 +2,7 @@
 #define SLA_JOBCONTROLLER_HPP
 
 #include <functional>
+#include <string>
 
 namespace Slic3r { namespace sla {
 
diff --git a/src/libslic3r/SLA/Pad.cpp b/src/libslic3r/SLA/Pad.cpp
index d933ef5ed..f2b189cd1 100644
--- a/src/libslic3r/SLA/Pad.cpp
+++ b/src/libslic3r/SLA/Pad.cpp
@@ -1,5 +1,4 @@
 #include <libslic3r/SLA/Pad.hpp>
-#include <libslic3r/SLA/Common.hpp>
 #include <libslic3r/SLA/SpatIndex.hpp>
 #include <libslic3r/SLA/BoostAdapter.hpp>
 #include <libslic3r/SLA/Contour3D.hpp>
diff --git a/src/libslic3r/SLA/ReprojectPointsOnMesh.hpp b/src/libslic3r/SLA/ReprojectPointsOnMesh.hpp
index 702d1bce1..4737a6c21 100644
--- a/src/libslic3r/SLA/ReprojectPointsOnMesh.hpp
+++ b/src/libslic3r/SLA/ReprojectPointsOnMesh.hpp
@@ -4,7 +4,7 @@
 #include "libslic3r/Point.hpp"
 #include "SupportPoint.hpp"
 #include "Hollowing.hpp"
-#include "EigenMesh3D.hpp"
+#include "IndexedMesh.hpp"
 #include "libslic3r/Model.hpp"
 
 #include <tbb/parallel_for.h>
@@ -15,7 +15,7 @@ template<class Pt> Vec3d pos(const Pt &p) { return p.pos.template cast<double>()
 template<class Pt> void pos(Pt &p, const Vec3d &pp) { p.pos = pp.cast<float>(); }
 
 template<class PointType>
-void reproject_support_points(const EigenMesh3D &mesh, std::vector<PointType> &pts)
+void reproject_support_points(const IndexedMesh &mesh, std::vector<PointType> &pts)
 {
     tbb::parallel_for(size_t(0), pts.size(), [&mesh, &pts](size_t idx) {
         int junk;
@@ -40,7 +40,7 @@ inline void reproject_points_and_holes(ModelObject *object)
 
     TriangleMesh rmsh = object->raw_mesh();
     rmsh.require_shared_vertices();
-    EigenMesh3D emesh{rmsh};
+    IndexedMesh emesh{rmsh};
 
     if (has_sppoints)
         reproject_support_points(emesh, object->sla_support_points);
diff --git a/src/libslic3r/SLA/Rotfinder.cpp b/src/libslic3r/SLA/Rotfinder.cpp
index fda8383b1..81ef00e6b 100644
--- a/src/libslic3r/SLA/Rotfinder.cpp
+++ b/src/libslic3r/SLA/Rotfinder.cpp
@@ -2,7 +2,6 @@
 #include <exception>
 
 #include <libnest2d/optimizers/nlopt/genetic.hpp>
-#include <libslic3r/SLA/Common.hpp>
 #include <libslic3r/SLA/Rotfinder.hpp>
 #include <libslic3r/SLA/SupportTree.hpp>
 #include "Model.hpp"
diff --git a/src/libslic3r/SLA/SpatIndex.cpp b/src/libslic3r/SLA/SpatIndex.cpp
new file mode 100644
index 000000000..d95ba55be
--- /dev/null
+++ b/src/libslic3r/SLA/SpatIndex.cpp
@@ -0,0 +1,161 @@
+#include "SpatIndex.hpp"
+
+// for concave hull merging decisions
+#include <libslic3r/SLA/BoostAdapter.hpp>
+
+#ifdef _MSC_VER
+#pragma warning(push)
+#pragma warning(disable: 4244)
+#pragma warning(disable: 4267)
+#endif
+
+#include "boost/geometry/index/rtree.hpp"
+
+#ifdef _MSC_VER
+#pragma warning(pop)
+#endif
+
+namespace Slic3r { namespace sla {
+
+/* **************************************************************************
+ * PointIndex implementation
+ * ************************************************************************** */
+
+class PointIndex::Impl {
+public:
+    using BoostIndex = boost::geometry::index::rtree< PointIndexEl,
+                                                     boost::geometry::index::rstar<16, 4> /* ? */ >;
+
+    BoostIndex m_store;
+};
+
+PointIndex::PointIndex(): m_impl(new Impl()) {}
+PointIndex::~PointIndex() {}
+
+PointIndex::PointIndex(const PointIndex &cpy): m_impl(new Impl(*cpy.m_impl)) {}
+PointIndex::PointIndex(PointIndex&& cpy): m_impl(std::move(cpy.m_impl)) {}
+
+PointIndex& PointIndex::operator=(const PointIndex &cpy)
+{
+    m_impl.reset(new Impl(*cpy.m_impl));
+    return *this;
+}
+
+PointIndex& PointIndex::operator=(PointIndex &&cpy)
+{
+    m_impl.swap(cpy.m_impl);
+    return *this;
+}
+
+void PointIndex::insert(const PointIndexEl &el)
+{
+    m_impl->m_store.insert(el);
+}
+
+bool PointIndex::remove(const PointIndexEl& el)
+{
+    return m_impl->m_store.remove(el) == 1;
+}
+
+std::vector<PointIndexEl>
+PointIndex::query(std::function<bool(const PointIndexEl &)> fn) const
+{
+    namespace bgi = boost::geometry::index;
+
+    std::vector<PointIndexEl> ret;
+    m_impl->m_store.query(bgi::satisfies(fn), std::back_inserter(ret));
+    return ret;
+}
+
+std::vector<PointIndexEl> PointIndex::nearest(const Vec3d &el, unsigned k = 1) const
+{
+    namespace bgi = boost::geometry::index;
+    std::vector<PointIndexEl> ret; ret.reserve(k);
+    m_impl->m_store.query(bgi::nearest(el, k), std::back_inserter(ret));
+    return ret;
+}
+
+size_t PointIndex::size() const
+{
+    return m_impl->m_store.size();
+}
+
+void PointIndex::foreach(std::function<void (const PointIndexEl &)> fn)
+{
+    for(auto& el : m_impl->m_store) fn(el);
+}
+
+void PointIndex::foreach(std::function<void (const PointIndexEl &)> fn) const
+{
+    for(const auto &el : m_impl->m_store) fn(el);
+}
+
+/* **************************************************************************
+ * BoxIndex implementation
+ * ************************************************************************** */
+
+class BoxIndex::Impl {
+public:
+    using BoostIndex = boost::geometry::index::
+        rtree<BoxIndexEl, boost::geometry::index::rstar<16, 4> /* ? */>;
+
+    BoostIndex m_store;
+};
+
+BoxIndex::BoxIndex(): m_impl(new Impl()) {}
+BoxIndex::~BoxIndex() {}
+
+BoxIndex::BoxIndex(const BoxIndex &cpy): m_impl(new Impl(*cpy.m_impl)) {}
+BoxIndex::BoxIndex(BoxIndex&& cpy): m_impl(std::move(cpy.m_impl)) {}
+
+BoxIndex& BoxIndex::operator=(const BoxIndex &cpy)
+{
+    m_impl.reset(new Impl(*cpy.m_impl));
+    return *this;
+}
+
+BoxIndex& BoxIndex::operator=(BoxIndex &&cpy)
+{
+    m_impl.swap(cpy.m_impl);
+    return *this;
+}
+
+void BoxIndex::insert(const BoxIndexEl &el)
+{
+    m_impl->m_store.insert(el);
+}
+
+bool BoxIndex::remove(const BoxIndexEl& el)
+{
+    return m_impl->m_store.remove(el) == 1;
+}
+
+std::vector<BoxIndexEl> BoxIndex::query(const BoundingBox &qrbb,
+                                        BoxIndex::QueryType qt)
+{
+    namespace bgi = boost::geometry::index;
+
+    std::vector<BoxIndexEl> ret; ret.reserve(m_impl->m_store.size());
+
+    switch (qt) {
+    case qtIntersects:
+        m_impl->m_store.query(bgi::intersects(qrbb), std::back_inserter(ret));
+        break;
+    case qtWithin:
+        m_impl->m_store.query(bgi::within(qrbb), std::back_inserter(ret));
+    }
+
+    return ret;
+}
+
+size_t BoxIndex::size() const
+{
+    return m_impl->m_store.size();
+}
+
+void BoxIndex::foreach(std::function<void (const BoxIndexEl &)> fn)
+{
+    for(auto& el : m_impl->m_store) fn(el);
+}
+
+}} // namespace Slic3r::sla
diff --git a/src/libslic3r/SLA/SpatIndex.hpp b/src/libslic3r/SLA/SpatIndex.hpp
index 2955cdcdf..ef059d3ae 100644
--- a/src/libslic3r/SLA/SpatIndex.hpp
+++ b/src/libslic3r/SLA/SpatIndex.hpp
@@ -73,7 +73,7 @@ public:
     BoxIndex& operator=(BoxIndex&&);
     
     void insert(const BoxIndexEl&);
-    inline void insert(const BoundingBox& bb, unsigned idx)
+    void insert(const BoundingBox& bb, unsigned idx)
     {
         insert(std::make_pair(bb, unsigned(idx)));
     }
diff --git a/src/libslic3r/SLA/SupportPoint.hpp b/src/libslic3r/SLA/SupportPoint.hpp
index 202a614c3..2b973697b 100644
--- a/src/libslic3r/SLA/SupportPoint.hpp
+++ b/src/libslic3r/SLA/SupportPoint.hpp
@@ -2,7 +2,6 @@
 #define SLA_SUPPORTPOINT_HPP
 
 #include <vector>
-#include <libslic3r/SLA/Common.hpp>
 #include <libslic3r/ExPolygon.hpp>
 
 namespace Slic3r { namespace sla {
@@ -29,13 +28,13 @@ struct SupportPoint
                  float pos_y,
                  float pos_z,
                  float head_radius,
-                 bool  new_island)
+                 bool  new_island = false)
         : pos(pos_x, pos_y, pos_z)
         , head_front_radius(head_radius)
         , is_new_island(new_island)
     {}
     
-    SupportPoint(Vec3f position, float head_radius, bool new_island)
+    SupportPoint(Vec3f position, float head_radius, bool new_island = false)
         : pos(position)
         , head_front_radius(head_radius)
         , is_new_island(new_island)
diff --git a/src/libslic3r/SLA/SupportPointGenerator.cpp b/src/libslic3r/SLA/SupportPointGenerator.cpp
index 78c2ced35..3cd075ae6 100644
--- a/src/libslic3r/SLA/SupportPointGenerator.cpp
+++ b/src/libslic3r/SLA/SupportPointGenerator.cpp
@@ -50,7 +50,7 @@ float SupportPointGenerator::distance_limit(float angle) const
 }*/
 
 SupportPointGenerator::SupportPointGenerator(
-        const sla::EigenMesh3D &emesh,
+        const sla::IndexedMesh &emesh,
         const std::vector<ExPolygons> &slices,
         const std::vector<float> &     heights,
         const Config &                 config,
@@ -64,7 +64,7 @@ SupportPointGenerator::SupportPointGenerator(
 }
 
 SupportPointGenerator::SupportPointGenerator(
-        const EigenMesh3D &emesh,
+        const IndexedMesh &emesh,
         const SupportPointGenerator::Config &config,
         std::function<void ()> throw_on_cancel, 
         std::function<void (int)> statusfn)
@@ -95,8 +95,8 @@ void SupportPointGenerator::project_onto_mesh(std::vector<sla::SupportPoint>& po
                     m_throw_on_cancel();
                 Vec3f& p = points[point_id].pos;
                 // Project the point upward and downward and choose the closer intersection with the mesh.
-                sla::EigenMesh3D::hit_result hit_up   = m_emesh.query_ray_hit(p.cast<double>(), Vec3d(0., 0., 1.));
-                sla::EigenMesh3D::hit_result hit_down = m_emesh.query_ray_hit(p.cast<double>(), Vec3d(0., 0., -1.));
+                sla::IndexedMesh::hit_result hit_up   = m_emesh.query_ray_hit(p.cast<double>(), Vec3d(0., 0., 1.));
+                sla::IndexedMesh::hit_result hit_down = m_emesh.query_ray_hit(p.cast<double>(), Vec3d(0., 0., -1.));
 
                 bool up   = hit_up.is_hit();
                 bool down = hit_down.is_hit();
@@ -104,7 +104,7 @@ void SupportPointGenerator::project_onto_mesh(std::vector<sla::SupportPoint>& po
                 if (!up && !down)
                     continue;
 
-                sla::EigenMesh3D::hit_result& hit = (!down || (hit_up.distance() < hit_down.distance())) ? hit_up : hit_down;
+                sla::IndexedMesh::hit_result& hit = (!down || (hit_up.distance() < hit_down.distance())) ? hit_up : hit_down;
                 p = p + (hit.distance() * hit.direction()).cast<float>();
             }
         });
@@ -523,15 +523,12 @@ void SupportPointGenerator::uniformly_cover(const ExPolygons& islands, Structure
     }
 }
 
-void remove_bottom_points(std::vector<SupportPoint> &pts, double gnd_lvl, double tolerance)
+void remove_bottom_points(std::vector<SupportPoint> &pts, float lvl)
 {
     // get iterator to the reorganized vector end
-    auto endit =
-        std::remove_if(pts.begin(), pts.end(),
-                       [tolerance, gnd_lvl](const sla::SupportPoint &sp) {
-        double diff = std::abs(gnd_lvl -
-                               double(sp.pos(Z)));
-        return diff <= tolerance;
+    auto endit = std::remove_if(pts.begin(), pts.end(), [lvl]
+                                (const sla::SupportPoint &sp) {
+        return sp.pos.z() <= lvl;
     });
 
     // erase all elements after the new end
diff --git a/src/libslic3r/SLA/SupportPointGenerator.hpp b/src/libslic3r/SLA/SupportPointGenerator.hpp
index 2fe8e11fc..f1b377025 100644
--- a/src/libslic3r/SLA/SupportPointGenerator.hpp
+++ b/src/libslic3r/SLA/SupportPointGenerator.hpp
@@ -3,9 +3,8 @@
 
 #include <random>
 
-#include <libslic3r/SLA/Common.hpp>
 #include <libslic3r/SLA/SupportPoint.hpp>
-#include <libslic3r/SLA/EigenMesh3D.hpp>
+#include <libslic3r/SLA/IndexedMesh.hpp>
 
 #include <libslic3r/BoundingBox.hpp>
 #include <libslic3r/ClipperUtils.hpp>
@@ -28,10 +27,10 @@ public:
         inline float tear_pressure() const { return 1.f; }  // pressure that the display exerts    (the force unit per mm2)
     };
     
-    SupportPointGenerator(const EigenMesh3D& emesh, const std::vector<ExPolygons>& slices,
+    SupportPointGenerator(const IndexedMesh& emesh, const std::vector<ExPolygons>& slices,
                     const std::vector<float>& heights, const Config& config, std::function<void(void)> throw_on_cancel, std::function<void(int)> statusfn);
     
-    SupportPointGenerator(const EigenMesh3D& emesh, const Config& config, std::function<void(void)> throw_on_cancel, std::function<void(int)> statusfn);
+    SupportPointGenerator(const IndexedMesh& emesh, const Config& config, std::function<void(void)> throw_on_cancel, std::function<void(int)> statusfn);
     
     const std::vector<SupportPoint>& output() const { return m_output; }
     std::vector<SupportPoint>& output() { return m_output; }
@@ -207,14 +206,14 @@ private:
     static void output_structures(const std::vector<Structure> &structures);
 #endif // SLA_SUPPORTPOINTGEN_DEBUG
     
-    const EigenMesh3D& m_emesh;
+    const IndexedMesh& m_emesh;
     std::function<void(void)> m_throw_on_cancel;
     std::function<void(int)>  m_statusfn;
     
     std::mt19937 m_rng;
 };
 
-void remove_bottom_points(std::vector<SupportPoint> &pts, double gnd_lvl, double tolerance);
+void remove_bottom_points(std::vector<SupportPoint> &pts, float lvl);
 
 }} // namespace Slic3r::sla
 
diff --git a/src/libslic3r/SLA/SupportTree.cpp b/src/libslic3r/SLA/SupportTree.cpp
index 528778b68..1bb4cfab7 100644
--- a/src/libslic3r/SLA/SupportTree.cpp
+++ b/src/libslic3r/SLA/SupportTree.cpp
@@ -5,9 +5,9 @@
 
 #include <numeric>
 #include <libslic3r/SLA/SupportTree.hpp>
-#include <libslic3r/SLA/Common.hpp>
 #include <libslic3r/SLA/SpatIndex.hpp>
 #include <libslic3r/SLA/SupportTreeBuilder.hpp>
+#include <libslic3r/SLA/SupportTreeBuildsteps.hpp>
 
 #include <libslic3r/MTUtils.hpp>
 #include <libslic3r/ClipperUtils.hpp>
@@ -28,20 +28,6 @@
 namespace Slic3r {
 namespace sla {
 
-// Compile time configuration value definitions:
-
-// The max Z angle for a normal at which it will get completely ignored.
-const double SupportConfig::normal_cutoff_angle = 150.0 * M_PI / 180.0;
-
-// The shortest distance of any support structure from the model surface
-const double SupportConfig::safety_distance_mm = 0.5;
-
-const double SupportConfig::max_solo_pillar_height_mm = 15.0;
-const double SupportConfig::max_dual_pillar_height_mm = 35.0;
-const double   SupportConfig::optimizer_rel_score_diff = 1e-6;
-const unsigned SupportConfig::optimizer_max_iterations = 1000;
-const unsigned SupportConfig::pillar_cascade_neighbors = 3;
-
 void SupportTree::retrieve_full_mesh(TriangleMesh &outmesh) const {
     outmesh.merge(retrieve_mesh(MeshType::Support));
     outmesh.merge(retrieve_mesh(MeshType::Pad));
@@ -103,9 +89,11 @@ SupportTree::UPtr SupportTree::create(const SupportableMesh &sm,
     builder->m_ctl = ctl;
     
     if (sm.cfg.enabled) {
-        builder->build(sm);
+        // Execute takes care about the ground_level
+        SupportTreeBuildsteps::execute(*builder, sm);
         builder->merge_and_cleanup();   // clean metadata, leave only the meshes.
     } else {
+        // If a pad gets added later, it will be in the right Z level
         builder->ground_level = sm.emesh.ground_level();
     }
     
diff --git a/src/libslic3r/SLA/SupportTree.hpp b/src/libslic3r/SLA/SupportTree.hpp
index c6255aa2f..4be90161d 100644
--- a/src/libslic3r/SLA/SupportTree.hpp
+++ b/src/libslic3r/SLA/SupportTree.hpp
@@ -5,9 +5,8 @@
 #include <memory>
 #include <Eigen/Geometry>
 
-#include <libslic3r/SLA/Common.hpp>
 #include <libslic3r/SLA/Pad.hpp>
-#include <libslic3r/SLA/EigenMesh3D.hpp>
+#include <libslic3r/SLA/IndexedMesh.hpp>
 #include <libslic3r/SLA/SupportPoint.hpp>
 #include <libslic3r/SLA/JobController.hpp>
 
@@ -32,7 +31,7 @@ enum class PillarConnectionMode
     dynamic
 };
 
-struct SupportConfig
+struct SupportTreeConfig
 {
     bool   enabled = true;
     
@@ -45,6 +44,8 @@ struct SupportConfig
     // Radius of the back side of the 3d arrow.
     double head_back_radius_mm = 0.5;
 
+    double head_fallback_radius_mm = 0.25;
+
     // Width in mm from the back sphere center to the front sphere center.
     double head_width_mm = 1.0;
 
@@ -95,36 +96,43 @@ struct SupportConfig
     // /////////////////////////////////////////////////////////////////////////
 
     // The max Z angle for a normal at which it will get completely ignored.
-    static const double normal_cutoff_angle;
+    static const double constexpr normal_cutoff_angle = 150.0 * M_PI / 180.0;
 
     // The shortest distance of any support structure from the model surface
-    static const double safety_distance_mm;
+    static const double constexpr safety_distance_mm = 0.5;
 
-    static const double max_solo_pillar_height_mm;
-    static const double max_dual_pillar_height_mm;
-    static const double   optimizer_rel_score_diff;
-    static const unsigned optimizer_max_iterations;
-    static const unsigned pillar_cascade_neighbors;
+    static const double constexpr max_solo_pillar_height_mm = 15.0;
+    static const double constexpr max_dual_pillar_height_mm = 35.0;
+    static const double constexpr optimizer_rel_score_diff = 1e-6;
+    static const unsigned constexpr optimizer_max_iterations = 1000;
+    static const unsigned constexpr pillar_cascade_neighbors = 3;
     
 };
 
+// TODO: Part of future refactor
+//class SupportConfig {
+//    std::optional<SupportTreeConfig> tree_cfg {std::in_place_t{}}; // fill up
+//    std::optional<PadConfig>         pad_cfg;
+//};
+
 enum class MeshType { Support, Pad };
 
 struct SupportableMesh
 {
-    EigenMesh3D   emesh;
+    IndexedMesh  emesh;
     SupportPoints pts;
-    SupportConfig cfg;
+    SupportTreeConfig cfg;
+    PadConfig     pad_cfg;
 
     explicit SupportableMesh(const TriangleMesh & trmsh,
                              const SupportPoints &sp,
-                             const SupportConfig &c)
+                             const SupportTreeConfig &c)
         : emesh{trmsh}, pts{sp}, cfg{c}
     {}
     
-    explicit SupportableMesh(const EigenMesh3D   &em,
+    explicit SupportableMesh(const IndexedMesh   &em,
                              const SupportPoints &sp,
-                             const SupportConfig &c)
+                             const SupportTreeConfig &c)
         : emesh{em}, pts{sp}, cfg{c}
     {}
 };
diff --git a/src/libslic3r/SLA/SupportTreeBuilder.cpp b/src/libslic3r/SLA/SupportTreeBuilder.cpp
index cf6e7e020..daa01ef24 100644
--- a/src/libslic3r/SLA/SupportTreeBuilder.cpp
+++ b/src/libslic3r/SLA/SupportTreeBuilder.cpp
@@ -1,336 +1,26 @@
+#define NOMINMAX
+
 #include <libslic3r/SLA/SupportTreeBuilder.hpp>
 #include <libslic3r/SLA/SupportTreeBuildsteps.hpp>
+#include <libslic3r/SLA/SupportTreeMesher.hpp>
 #include <libslic3r/SLA/Contour3D.hpp>
 
 namespace Slic3r {
 namespace sla {
 
-Contour3D sphere(double rho, Portion portion, double fa) {
-    
-    Contour3D ret;
-    
-    // prohibit close to zero radius
-    if(rho <= 1e-6 && rho >= -1e-6) return ret;
-    
-    auto& vertices = ret.points;
-    auto& facets = ret.faces3;
-    
-    // Algorithm:
-    // Add points one-by-one to the sphere grid and form facets using relative
-    // coordinates. Sphere is composed effectively of a mesh of stacked circles.
-    
-    // adjust via rounding to get an even multiple for any provided angle.
-    double angle = (2*PI / floor(2*PI / fa));
-    
-    // Ring to be scaled to generate the steps of the sphere
-    std::vector<double> ring;
-    
-    for (double i = 0; i < 2*PI; i+=angle) ring.emplace_back(i);
-    
-    const auto sbegin = size_t(2*std::get<0>(portion)/angle);
-    const auto send = size_t(2*std::get<1>(portion)/angle);
-    
-    const size_t steps = ring.size();
-    const double increment = 1.0 / double(steps);
-    
-    // special case: first ring connects to 0,0,0
-    // insert and form facets.
-    if(sbegin == 0)
-        vertices.emplace_back(Vec3d(0.0, 0.0, -rho + increment*sbegin*2.0*rho));
-    
-    auto id = coord_t(vertices.size());
-    for (size_t i = 0; i < ring.size(); i++) {
-        // Fixed scaling
-        const double z = -rho + increment*rho*2.0 * (sbegin + 1.0);
-        // radius of the circle for this step.
-        const double r = std::sqrt(std::abs(rho*rho - z*z));
-        Vec2d b = Eigen::Rotation2Dd(ring[i]) * Eigen::Vector2d(0, r);
-        vertices.emplace_back(Vec3d(b(0), b(1), z));
-        
-        if (sbegin == 0)
-            (i == 0) ? facets.emplace_back(coord_t(ring.size()), 0, 1) :
-        			   facets.emplace_back(id - 1, 0, id);
-        ++id;
-    }
-    
-    // General case: insert and form facets for each step,
-    // joining it to the ring below it.
-    for (size_t s = sbegin + 2; s < send - 1; s++) {
-        const double z = -rho + increment*double(s*2.0*rho);
-        const double r = std::sqrt(std::abs(rho*rho - z*z));
-        
-        for (size_t i = 0; i < ring.size(); i++) {
-            Vec2d b = Eigen::Rotation2Dd(ring[i]) * Eigen::Vector2d(0, r);
-            vertices.emplace_back(Vec3d(b(0), b(1), z));
-            auto id_ringsize = coord_t(id - int(ring.size()));
-            if (i == 0) {
-                // wrap around
-                facets.emplace_back(id - 1, id, id + coord_t(ring.size() - 1) );
-                facets.emplace_back(id - 1, id_ringsize, id);
-            } else {
-                facets.emplace_back(id_ringsize - 1, id_ringsize, id);
-                facets.emplace_back(id - 1, id_ringsize - 1, id);
-            }
-            id++;
-        }
-    }
-    
-    // special case: last ring connects to 0,0,rho*2.0
-    // only form facets.
-    if(send >= size_t(2*PI / angle)) {
-        vertices.emplace_back(Vec3d(0.0, 0.0, -rho + increment*send*2.0*rho));
-        for (size_t i = 0; i < ring.size(); i++) {
-            auto id_ringsize = coord_t(id - int(ring.size()));
-            if (i == 0) {
-                // third vertex is on the other side of the ring.
-                facets.emplace_back(id - 1, id_ringsize, id);
-            } else {
-                auto ci = coord_t(id_ringsize + coord_t(i));
-                facets.emplace_back(ci - 1, ci, id);
-            }
-        }
-    }
-    id++;
-    
-    return ret;
-}
-
-Contour3D cylinder(double r, double h, size_t ssteps, const Vec3d &sp)
-{
-    Contour3D ret;
-    
-    auto steps = int(ssteps);
-    auto& points = ret.points;
-    auto& indices = ret.faces3;
-    points.reserve(2*ssteps);
-    double a = 2*PI/steps;
-    
-    Vec3d jp = sp;
-    Vec3d endp = {sp(X), sp(Y), sp(Z) + h};
-    
-    // Upper circle points
-    for(int i = 0; i < steps; ++i) {
-        double phi = i*a;
-        double ex = endp(X) + r*std::cos(phi);
-        double ey = endp(Y) + r*std::sin(phi);
-        points.emplace_back(ex, ey, endp(Z));
-    }
-    
-    // Lower circle points
-    for(int i = 0; i < steps; ++i) {
-        double phi = i*a;
-        double x = jp(X) + r*std::cos(phi);
-        double y = jp(Y) + r*std::sin(phi);
-        points.emplace_back(x, y, jp(Z));
-    }
-    
-    // Now create long triangles connecting upper and lower circles
-    indices.reserve(2*ssteps);
-    auto offs = steps;
-    for(int i = 0; i < steps - 1; ++i) {
-        indices.emplace_back(i, i + offs, offs + i + 1);
-        indices.emplace_back(i, offs + i + 1, i + 1);
-    }
-    
-    // Last triangle connecting the first and last vertices
-    auto last = steps - 1;
-    indices.emplace_back(0, last, offs);
-    indices.emplace_back(last, offs + last, offs);
-    
-    // According to the slicing algorithms, we need to aid them with generating
-    // a watertight body. So we create a triangle fan for the upper and lower
-    // ending of the cylinder to close the geometry.
-    points.emplace_back(jp); int ci = int(points.size() - 1);
-    for(int i = 0; i < steps - 1; ++i)
-        indices.emplace_back(i + offs + 1, i + offs, ci);
-    
-    indices.emplace_back(offs, steps + offs - 1, ci);
-    
-    points.emplace_back(endp); ci = int(points.size() - 1);
-    for(int i = 0; i < steps - 1; ++i)
-        indices.emplace_back(ci, i, i + 1);
-    
-    indices.emplace_back(steps - 1, 0, ci);
-    
-    return ret;
-}
-
 Head::Head(double       r_big_mm,
            double       r_small_mm,
            double       length_mm,
            double       penetration,
            const Vec3d &direction,
-           const Vec3d &offset,
-           const size_t circlesteps)
-    : steps(circlesteps)
-    , dir(direction)
-    , tr(offset)
+           const Vec3d &offset)
+    : dir(direction)
+    , pos(offset)
     , r_back_mm(r_big_mm)
     , r_pin_mm(r_small_mm)
     , width_mm(length_mm)
     , penetration_mm(penetration)
 {
-    assert(width_mm > 0.);
-    assert(r_back_mm > 0.);
-    assert(r_pin_mm > 0.);
-    
-    // We create two spheres which will be connected with a robe that fits
-    // both circles perfectly.
-    
-    // Set up the model detail level
-    const double detail = 2*PI/steps;
-    
-    // We don't generate whole circles. Instead, we generate only the
-    // portions which are visible (not covered by the robe) To know the
-    // exact portion of the bottom and top circles we need to use some
-    // rules of tangent circles from which we can derive (using simple
-    // triangles the following relations:
-    
-    // The height of the whole mesh
-    const double h = r_big_mm + r_small_mm + width_mm;
-    double phi = PI/2 - std::acos( (r_big_mm - r_small_mm) / h );
-    
-    // To generate a whole circle we would pass a portion of (0, Pi)
-    // To generate only a half horizontal circle we can pass (0, Pi/2)
-    // The calculated phi is an offset to the half circles needed to smooth
-    // the transition from the circle to the robe geometry
-    
-    auto&& s1 = sphere(r_big_mm, make_portion(0, PI/2 + phi), detail);
-    auto&& s2 = sphere(r_small_mm, make_portion(PI/2 + phi, PI), detail);
-    
-    for(auto& p : s2.points) p.z() += h;
-    
-    mesh.merge(s1);
-    mesh.merge(s2);
-    
-    for(size_t idx1 = s1.points.size() - steps, idx2 = s1.points.size();
-        idx1 < s1.points.size() - 1;
-        idx1++, idx2++)
-    {
-        coord_t i1s1 = coord_t(idx1), i1s2 = coord_t(idx2);
-        coord_t i2s1 = i1s1 + 1, i2s2 = i1s2 + 1;
-        
-        mesh.faces3.emplace_back(i1s1, i2s1, i2s2);
-        mesh.faces3.emplace_back(i1s1, i2s2, i1s2);
-    }
-    
-    auto i1s1 = coord_t(s1.points.size()) - coord_t(steps);
-    auto i2s1 = coord_t(s1.points.size()) - 1;
-    auto i1s2 = coord_t(s1.points.size());
-    auto i2s2 = coord_t(s1.points.size()) + coord_t(steps) - 1;
-    
-    mesh.faces3.emplace_back(i2s2, i2s1, i1s1);
-    mesh.faces3.emplace_back(i1s2, i2s2, i1s1);
-    
-    // To simplify further processing, we translate the mesh so that the
-    // last vertex of the pointing sphere (the pinpoint) will be at (0,0,0)
-    for(auto& p : mesh.points) p.z() -= (h + r_small_mm - penetration_mm);
-}
-
-Pillar::Pillar(const Vec3d &jp, const Vec3d &endp, double radius, size_t st):
-    r(radius), steps(st), endpt(endp), starts_from_head(false)
-{
-    assert(steps > 0);
-    
-    height = jp(Z) - endp(Z);
-    if(height > EPSILON) { // Endpoint is below the starting point
-        
-        // We just create a bridge geometry with the pillar parameters and
-        // move the data.
-        Contour3D body = cylinder(radius, height, st, endp);
-        mesh.points.swap(body.points);
-        mesh.faces3.swap(body.faces3);
-    }
-}
-
-Pillar &Pillar::add_base(double baseheight, double radius)
-{
-    if(baseheight <= 0) return *this;
-    if(baseheight > height) baseheight = height;
-    
-    assert(steps >= 0);
-    auto last = int(steps - 1);
-    
-    if(radius < r ) radius = r;
-    
-    double a = 2*PI/steps;
-    double z = endpt(Z) + baseheight;
-    
-    for(size_t i = 0; i < steps; ++i) {
-        double phi = i*a;
-        double x = endpt(X) + r*std::cos(phi);
-        double y = endpt(Y) + r*std::sin(phi);
-        base.points.emplace_back(x, y, z);
-    }
-    
-    for(size_t i = 0; i < steps; ++i) {
-        double phi = i*a;
-        double x = endpt(X) + radius*std::cos(phi);
-        double y = endpt(Y) + radius*std::sin(phi);
-        base.points.emplace_back(x, y, z - baseheight);
-    }
-    
-    auto ep = endpt; ep(Z) += baseheight;
-    base.points.emplace_back(endpt);
-    base.points.emplace_back(ep);
-    
-    auto& indices = base.faces3;
-    auto hcenter = int(base.points.size() - 1);
-    auto lcenter = int(base.points.size() - 2);
-    auto offs = int(steps);
-    for(int i = 0; i < last; ++i) {
-        indices.emplace_back(i, i + offs, offs + i + 1);
-        indices.emplace_back(i, offs + i + 1, i + 1);
-        indices.emplace_back(i, i + 1, hcenter);
-        indices.emplace_back(lcenter, offs + i + 1, offs + i);
-    }
-    
-    indices.emplace_back(0, last, offs);
-    indices.emplace_back(last, offs + last, offs);
-    indices.emplace_back(hcenter, last, 0);
-    indices.emplace_back(offs, offs + last, lcenter);
-    return *this;
-}
-
-Bridge::Bridge(const Vec3d &j1, const Vec3d &j2, double r_mm, size_t steps):
-    r(r_mm), startp(j1), endp(j2)
-{
-    using Quaternion = Eigen::Quaternion<double>;
-    Vec3d dir = (j2 - j1).normalized();
-    double d = distance(j2, j1);
-    
-    mesh = cylinder(r, d, steps);
-    
-    auto quater = Quaternion::FromTwoVectors(Vec3d{0,0,1}, dir);
-    for(auto& p : mesh.points) p = quater * p + j1;
-}
-
-CompactBridge::CompactBridge(const Vec3d &sp,
-                             const Vec3d &ep,
-                             const Vec3d &n,
-                             double       r,
-                             bool         endball,
-                             size_t       steps)
-{
-    Vec3d startp = sp + r * n;
-    Vec3d dir = (ep - startp).normalized();
-    Vec3d endp = ep - r * dir;
-    
-    Bridge br(startp, endp, r, steps);
-    mesh.merge(br.mesh);
-    
-    // now add the pins
-    double fa = 2*PI/steps;
-    auto upperball = sphere(r, Portion{PI / 2 - fa, PI}, fa);
-    for(auto& p : upperball.points) p += startp;
-    
-    if(endball) {
-        auto lowerball = sphere(r, Portion{0, PI/2 + 2*fa}, fa);
-        for(auto& p : lowerball.points) p += endp;
-        mesh.merge(lowerball);
-    }
-    
-    mesh.merge(upperball);
 }
 
 Pad::Pad(const TriangleMesh &support_mesh,
@@ -368,7 +58,6 @@ SupportTreeBuilder::SupportTreeBuilder(SupportTreeBuilder &&o)
     , m_pillars{std::move(o.m_pillars)}
     , m_bridges{std::move(o.m_bridges)}
     , m_crossbridges{std::move(o.m_crossbridges)}
-    , m_compact_bridges{std::move(o.m_compact_bridges)}
     , m_pad{std::move(o.m_pad)}
     , m_meshcache{std::move(o.m_meshcache)}
     , m_meshcache_valid{o.m_meshcache_valid}
@@ -382,7 +71,6 @@ SupportTreeBuilder::SupportTreeBuilder(const SupportTreeBuilder &o)
     , m_pillars{o.m_pillars}
     , m_bridges{o.m_bridges}
     , m_crossbridges{o.m_crossbridges}
-    , m_compact_bridges{o.m_compact_bridges}
     , m_pad{o.m_pad}
     , m_meshcache{o.m_meshcache}
     , m_meshcache_valid{o.m_meshcache_valid}
@@ -397,7 +85,6 @@ SupportTreeBuilder &SupportTreeBuilder::operator=(SupportTreeBuilder &&o)
     m_pillars = std::move(o.m_pillars);
     m_bridges = std::move(o.m_bridges);
     m_crossbridges = std::move(o.m_crossbridges);
-    m_compact_bridges = std::move(o.m_compact_bridges);
     m_pad = std::move(o.m_pad);
     m_meshcache = std::move(o.m_meshcache);
     m_meshcache_valid = o.m_meshcache_valid;
@@ -413,7 +100,6 @@ SupportTreeBuilder &SupportTreeBuilder::operator=(const SupportTreeBuilder &o)
     m_pillars = o.m_pillars;
     m_bridges = o.m_bridges;
     m_crossbridges = o.m_crossbridges;
-    m_compact_bridges = o.m_compact_bridges;
     m_pad = o.m_pad;
     m_meshcache = o.m_meshcache;
     m_meshcache_valid = o.m_meshcache_valid;
@@ -422,7 +108,19 @@ SupportTreeBuilder &SupportTreeBuilder::operator=(const SupportTreeBuilder &o)
     return *this;
 }
 
-const TriangleMesh &SupportTreeBuilder::merged_mesh() const
+void SupportTreeBuilder::add_pillar_base(long pid, double baseheight, double radius)
+{
+    std::lock_guard<Mutex> lk(m_mutex);
+    assert(pid >= 0 && size_t(pid) < m_pillars.size());
+    Pillar& pll = m_pillars[size_t(pid)];
+    m_pedestals.emplace_back(pll.endpt, std::min(baseheight, pll.height),
+                             std::max(radius, pll.r), pll.r);
+
+    m_pedestals.back().id = m_pedestals.size() - 1;
+    m_meshcache_valid = false;
+}
+
+const TriangleMesh &SupportTreeBuilder::merged_mesh(size_t steps) const
 {
     if (m_meshcache_valid) return m_meshcache;
     
@@ -430,35 +128,44 @@ const TriangleMesh &SupportTreeBuilder::merged_mesh() const
     
     for (auto &head : m_heads) {
         if (ctl().stopcondition()) break;
-        if (head.is_valid()) merged.merge(head.mesh);
+        if (head.is_valid()) merged.merge(get_mesh(head, steps));
     }
     
-    for (auto &stick : m_pillars) {
+    for (auto &pill : m_pillars) {
         if (ctl().stopcondition()) break;
-        merged.merge(stick.mesh);
-        merged.merge(stick.base);
+        merged.merge(get_mesh(pill, steps));
+    }
+
+    for (auto &pedest : m_pedestals) {
+        if (ctl().stopcondition()) break;
+        merged.merge(get_mesh(pedest, steps));
     }
     
     for (auto &j : m_junctions) {
         if (ctl().stopcondition()) break;
-        merged.merge(j.mesh);
+        merged.merge(get_mesh(j, steps));
     }
-    
-    for (auto &cb : m_compact_bridges) {
-        if (ctl().stopcondition()) break;
-        merged.merge(cb.mesh);
-    }
-    
+
     for (auto &bs : m_bridges) {
         if (ctl().stopcondition()) break;
-        merged.merge(bs.mesh);
+        merged.merge(get_mesh(bs, steps));
     }
     
     for (auto &bs : m_crossbridges) {
         if (ctl().stopcondition()) break;
-        merged.merge(bs.mesh);
+        merged.merge(get_mesh(bs, steps));
     }
-    
+
+    for (auto &bs : m_diffbridges) {
+        if (ctl().stopcondition()) break;
+        merged.merge(get_mesh(bs, steps));
+    }
+
+    for (auto &anch : m_anchors) {
+        if (ctl().stopcondition()) break;
+        merged.merge(get_mesh(anch, steps));
+    }
+
     if (ctl().stopcondition()) {
         // In case of failure we have to return an empty mesh
         m_meshcache = TriangleMesh();
@@ -499,7 +206,6 @@ const TriangleMesh &SupportTreeBuilder::merge_and_cleanup()
     m_pillars = {};
     m_junctions = {};
     m_bridges = {};
-    m_compact_bridges = {};
     
     return ret;
 }
@@ -514,11 +220,4 @@ const TriangleMesh &SupportTreeBuilder::retrieve_mesh(MeshType meshtype) const
     return m_meshcache;
 }
 
-bool SupportTreeBuilder::build(const SupportableMesh &sm)
-{
-    ground_level = sm.emesh.ground_level() - sm.cfg.object_elevation_mm;
-    return SupportTreeBuildsteps::execute(*this, sm);
-}
-
-}
-}
+}} // namespace Slic3r::sla
diff --git a/src/libslic3r/SLA/SupportTreeBuilder.hpp b/src/libslic3r/SLA/SupportTreeBuilder.hpp
index 90cf417c8..f29263ca3 100644
--- a/src/libslic3r/SLA/SupportTreeBuilder.hpp
+++ b/src/libslic3r/SLA/SupportTreeBuilder.hpp
@@ -2,7 +2,6 @@
 #define SLA_SUPPORTTREEBUILDER_HPP
 
 #include <libslic3r/SLA/Concurrency.hpp>
-#include <libslic3r/SLA/Common.hpp>
 #include <libslic3r/SLA/SupportTree.hpp>
 #include <libslic3r/SLA/Contour3D.hpp>
 #include <libslic3r/SLA/Pad.hpp>
@@ -50,13 +49,6 @@ namespace sla {
  * nearby pillar.
  */
 
-using Coordf = double;
-using Portion = std::tuple<double, double>;
-
-inline Portion make_portion(double a, double b) {
-    return std::make_tuple(a, b);
-}
-
 template<class Vec> double distance(const Vec& p) {
     return std::sqrt(p.transpose() * p);
 }
@@ -66,33 +58,25 @@ template<class Vec> double distance(const Vec& pp1, const Vec& pp2) {
     return distance(p);
 }
 
-Contour3D sphere(double rho, Portion portion = make_portion(0.0, 2.0*PI),
-                 double fa=(2*PI/360));
+const Vec3d DOWN = {0.0, 0.0, -1.0};
 
-// Down facing cylinder in Z direction with arguments:
-// r: radius
-// h: Height
-// ssteps: how many edges will create the base circle
-// sp: starting point
-Contour3D cylinder(double r, double h, size_t ssteps = 45, const Vec3d &sp = {0,0,0});
+struct SupportTreeNode
+{
+    static const constexpr long ID_UNSET = -1;
 
-const constexpr long ID_UNSET = -1;
+    long id = ID_UNSET; // For identification withing a tree.
+};
 
-struct Head {
-    Contour3D mesh;
-    
-    size_t steps = 45;
-    Vec3d dir = {0, 0, -1};
-    Vec3d tr = {0, 0, 0};
+// A pinhead originating from a support point
+struct Head: public SupportTreeNode {
+    Vec3d dir = DOWN;
+    Vec3d pos = {0, 0, 0};
     
     double r_back_mm = 1;
     double r_pin_mm = 0.5;
     double width_mm = 2;
     double penetration_mm = 0.5;
-    
-    // For identification purposes. This will be used as the index into the
-    // container holding the head structures. See SLASupportTree::Impl
-    long id = ID_UNSET;
+
     
     // If there is a pillar connecting to this head, then the id will be set.
     long pillar_id = ID_UNSET;
@@ -106,31 +90,23 @@ struct Head {
          double r_small_mm,
          double length_mm,
          double penetration,
-         const Vec3d &direction = {0, 0, -1},  // direction (normal to the dull end)
-         const Vec3d &offset = {0, 0, 0},      // displacement
-         const size_t circlesteps = 45);
-    
-    void transform()
+         const Vec3d &direction = DOWN,  // direction (normal to the dull end)
+         const Vec3d &offset = {0, 0, 0}      // displacement
+         );
+
+    inline double real_width() const
     {
-        using Quaternion = Eigen::Quaternion<double>;
-        
-        // We rotate the head to the specified direction The head's pointing
-        // side is facing upwards so this means that it would hold a support
-        // point with a normal pointing straight down. This is the reason of
-        // the -1 z coordinate
-        auto quatern = Quaternion::FromTwoVectors(Vec3d{0, 0, -1}, dir);
-        
-        for(auto& p : mesh.points) p = quatern * p + tr;
+        return 2 * r_pin_mm + width_mm + 2 * r_back_mm ;
     }
-    
+
     inline double fullwidth() const
     {
-        return 2 * r_pin_mm + width_mm + 2*r_back_mm - penetration_mm;
+        return real_width() - penetration_mm;
     }
     
     inline Vec3d junction_point() const
     {
-        return tr + ( 2 * r_pin_mm + width_mm + r_back_mm - penetration_mm)*dir;
+        return pos + (fullwidth() - r_back_mm) * dir;
     }
     
     inline double request_pillar_radius(double radius) const
@@ -140,31 +116,17 @@ struct Head {
     }
 };
 
-struct Junction {
-    Contour3D mesh;
+// A junction connecting bridges and pillars
+struct Junction: public SupportTreeNode {
     double r = 1;
-    size_t steps = 45;
     Vec3d pos;
-    
-    long id = ID_UNSET;
-    
-    Junction(const Vec3d& tr, double r_mm, size_t stepnum = 45):
-        r(r_mm), steps(stepnum), pos(tr)
-    {
-        mesh = sphere(r_mm, make_portion(0, PI), 2*PI/steps);
-        for(auto& p : mesh.points) p += tr;
-    }
+
+    Junction(const Vec3d &tr, double r_mm) : r(r_mm), pos(tr) {}
 };
 
-struct Pillar {
-    Contour3D mesh;
-    Contour3D base;
-    double r = 1;
-    size_t steps = 0;
+struct Pillar: public SupportTreeNode {
+    double height, r;
     Vec3d endpt;
-    double height = 0;
-    
-    long id = ID_UNSET;
     
     // If the pillar connects to a head, this is the id of that head
     bool starts_from_head = true; // Could start from a junction as well
@@ -175,54 +137,52 @@ struct Pillar {
     
     // How many pillars are cascaded with this one
     unsigned links = 0;
-    
-    Pillar(const Vec3d& jp, const Vec3d& endp,
-           double radius = 1, size_t st = 45);
-    
-    Pillar(const Junction &junc, const Vec3d &endp)
-        : Pillar(junc.pos, endp, junc.r, junc.steps)
-    {}
-    
-    Pillar(const Head &head, const Vec3d &endp, double radius = 1)
-        : Pillar(head.junction_point(), endp,
-                 head.request_pillar_radius(radius), head.steps)
-    {}
-    
-    inline Vec3d startpoint() const
+
+    Pillar(const Vec3d &endp, double h, double radius = 1.):
+        height{h}, r(radius), endpt(endp), starts_from_head(false) {}
+
+    Vec3d startpoint() const
     {
-        return {endpt(X), endpt(Y), endpt(Z) + height};
+        return {endpt.x(), endpt.y(), endpt.z() + height};
     }
     
-    inline const Vec3d& endpoint() const { return endpt; }
-    
-    Pillar& add_base(double baseheight = 3, double radius = 2);
+    const Vec3d& endpoint() const { return endpt; }
 };
 
+// A base for pillars or bridges that end on the ground
+struct Pedestal: public SupportTreeNode {
+    Vec3d pos;
+    double height, r_bottom, r_top;
+
+    Pedestal(const Vec3d &p, double h, double rbottom, double rtop)
+        : pos{p}, height{h}, r_bottom{rbottom}, r_top{rtop}
+    {}
+};
+
+// This is the thing that anchors a pillar or bridge to the model body.
+// It is actually a reverse pinhead.
+struct Anchor: public Head { using Head::Head; };
+
 // A Bridge between two pillars (with junction endpoints)
-struct Bridge {
-    Contour3D mesh;
+struct Bridge: public SupportTreeNode {
     double r = 0.8;
-    long id = ID_UNSET;
     Vec3d startp = Vec3d::Zero(), endp = Vec3d::Zero();
     
     Bridge(const Vec3d &j1,
            const Vec3d &j2,
-           double       r_mm  = 0.8,
-           size_t       steps = 45);
+           double       r_mm  = 0.8): r{r_mm}, startp{j1}, endp{j2}
+    {}
+
+    double get_length() const { return (endp - startp).norm(); }
+    Vec3d  get_dir() const { return (endp - startp).normalized(); }
 };
 
-// A bridge that spans from model surface to model surface with small connecting
-// edges on the endpoints. Used for headless support points.
-struct CompactBridge {
-    Contour3D mesh;
-    long id = ID_UNSET;
-    
-    CompactBridge(const Vec3d& sp,
-                  const Vec3d& ep,
-                  const Vec3d& n,
-                  double r,
-                  bool endball = true,
-                  size_t steps = 45);
+struct DiffBridge: public Bridge {
+    double end_r;
+
+    DiffBridge(const Vec3d &p_s, const Vec3d &p_e, double r_s, double r_e)
+        : Bridge{p_s, p_e, r_s}, end_r{r_e}
+    {}
 };
 
 // A wrapper struct around the pad
@@ -258,13 +218,16 @@ struct Pad {
 // merged mesh. It can be retrieved using a dedicated method (pad())
 class SupportTreeBuilder: public SupportTree {
     // For heads it is beneficial to use the same IDs as for the support points.
-    std::vector<Head> m_heads;
-    std::vector<size_t> m_head_indices;
-    std::vector<Pillar> m_pillars;
-    std::vector<Junction> m_junctions;
-    std::vector<Bridge> m_bridges;
-    std::vector<Bridge> m_crossbridges;
-    std::vector<CompactBridge> m_compact_bridges;    
+    std::vector<Head>       m_heads;
+    std::vector<size_t>     m_head_indices;
+    std::vector<Pillar>     m_pillars;
+    std::vector<Junction>   m_junctions;
+    std::vector<Bridge>     m_bridges;
+    std::vector<Bridge>     m_crossbridges;
+    std::vector<DiffBridge> m_diffbridges;
+    std::vector<Pedestal>   m_pedestals;
+    std::vector<Anchor>     m_anchors;
+
     Pad m_pad;
     
     using Mutex = ccr::SpinningMutex;
@@ -274,8 +237,8 @@ class SupportTreeBuilder: public SupportTree {
     mutable bool m_meshcache_valid = false;
     mutable double m_model_height = 0; // the full height of the model
     
-    template<class...Args>
-    const Bridge& _add_bridge(std::vector<Bridge> &br, Args&&... args)
+    template<class BridgeT, class...Args>
+    const BridgeT& _add_bridge(std::vector<BridgeT> &br, Args&&... args)
     {
         std::lock_guard<Mutex> lk(m_mutex);
         br.emplace_back(std::forward<Args>(args)...);
@@ -306,7 +269,7 @@ public:
         return m_heads.back();
     }
     
-    template<class...Args> long add_pillar(long headid, Args&&... args)
+    template<class...Args> long add_pillar(long headid, double length)
     {
         std::lock_guard<Mutex> lk(m_mutex);
         if (m_pillars.capacity() < m_heads.size())
@@ -315,7 +278,9 @@ public:
         assert(headid >= 0 && size_t(headid) < m_head_indices.size());
         Head &head = m_heads[m_head_indices[size_t(headid)]];
         
-        m_pillars.emplace_back(head, std::forward<Args>(args)...);
+        Vec3d hjp = head.junction_point() - Vec3d{0, 0, length};
+        m_pillars.emplace_back(hjp, length, head.r_back_mm);
+
         Pillar& pillar = m_pillars.back();
         pillar.id = long(m_pillars.size() - 1);
         head.pillar_id = pillar.id;
@@ -326,11 +291,15 @@ public:
         return pillar.id;
     }
     
-    void add_pillar_base(long pid, double baseheight = 3, double radius = 2)
+    void add_pillar_base(long pid, double baseheight = 3, double radius = 2);
+
+    template<class...Args> const Anchor& add_anchor(Args&&...args)
     {
         std::lock_guard<Mutex> lk(m_mutex);
-        assert(pid >= 0 && size_t(pid) < m_pillars.size());
-        m_pillars[size_t(pid)].add_base(baseheight, radius);
+        m_anchors.emplace_back(std::forward<Args>(args)...);
+        m_anchors.back().id = long(m_junctions.size() - 1);
+        m_meshcache_valid = false;
+        return m_anchors.back();
     }
     
     void increment_bridges(const Pillar& pillar)
@@ -371,17 +340,6 @@ public:
         return pillar.id;
     }
     
-    const Pillar& head_pillar(unsigned headid) const
-    {
-        std::lock_guard<Mutex> lk(m_mutex);
-        assert(headid < m_head_indices.size());
-        
-        const Head& h = m_heads[m_head_indices[headid]];
-        assert(h.pillar_id >= 0 && h.pillar_id < long(m_pillars.size()));
-        
-        return m_pillars[size_t(h.pillar_id)];
-    }
-    
     template<class...Args> const Junction& add_junction(Args&&... args)
     {
         std::lock_guard<Mutex> lk(m_mutex);
@@ -391,18 +349,18 @@ public:
         return m_junctions.back();
     }
     
-    const Bridge& add_bridge(const Vec3d &s, const Vec3d &e, double r, size_t n = 45)
+    const Bridge& add_bridge(const Vec3d &s, const Vec3d &e, double r)
     {
-        return _add_bridge(m_bridges, s, e, r, n);
+        return _add_bridge(m_bridges, s, e, r);
     }
     
-    const Bridge& add_bridge(long headid, const Vec3d &endp, size_t s = 45)
+    const Bridge& add_bridge(long headid, const Vec3d &endp)
     {
         std::lock_guard<Mutex> lk(m_mutex);
         assert(headid >= 0 && size_t(headid) < m_head_indices.size());
         
         Head &h = m_heads[m_head_indices[size_t(headid)]];
-        m_bridges.emplace_back(h.junction_point(), endp, h.r_back_mm, s);
+        m_bridges.emplace_back(h.junction_point(), endp, h.r_back_mm);
         m_bridges.back().id = long(m_bridges.size() - 1);
         
         h.bridge_id = m_bridges.back().id;
@@ -414,14 +372,10 @@ public:
     {
         return _add_bridge(m_crossbridges, std::forward<Args>(args)...);
     }
-    
-    template<class...Args> const CompactBridge& add_compact_bridge(Args&&...args)
+
+    template<class...Args> const DiffBridge& add_diffbridge(Args&&... args)
     {
-        std::lock_guard<Mutex> lk(m_mutex);
-        m_compact_bridges.emplace_back(std::forward<Args>(args)...);
-        m_compact_bridges.back().id = long(m_compact_bridges.size() - 1);
-        m_meshcache_valid = false;
-        return m_compact_bridges.back();
+        return _add_bridge(m_diffbridges, std::forward<Args>(args)...);
     }
     
     Head &head(unsigned id)
@@ -439,7 +393,7 @@ public:
     }
     
     inline const std::vector<Pillar> &pillars() const { return m_pillars; }
-    inline const std::vector<Head> &heads() const { return m_heads; }
+    inline const std::vector<Head>   &heads() const { return m_heads; }
     inline const std::vector<Bridge> &bridges() const { return m_bridges; }
     inline const std::vector<Bridge> &crossbridges() const { return m_crossbridges; }
     
@@ -464,7 +418,7 @@ public:
     const Pad& pad() const { return m_pad; }
     
     // WITHOUT THE PAD!!!
-    const TriangleMesh &merged_mesh() const;
+    const TriangleMesh &merged_mesh(size_t steps = 45) const;
     
     // WITH THE PAD
     double full_height() const;
@@ -488,8 +442,6 @@ public:
     
     virtual const TriangleMesh &retrieve_mesh(
         MeshType meshtype = MeshType::Support) const override;
-
-    bool build(const SupportableMesh &supportable_mesh);
 };
 
 }} // namespace Slic3r::sla
diff --git a/src/libslic3r/SLA/SupportTreeBuildsteps.cpp b/src/libslic3r/SLA/SupportTreeBuildsteps.cpp
index 29ad6057f..2b40f0082 100644
--- a/src/libslic3r/SLA/SupportTreeBuildsteps.cpp
+++ b/src/libslic3r/SLA/SupportTreeBuildsteps.cpp
@@ -1,19 +1,36 @@
 #include <libslic3r/SLA/SupportTreeBuildsteps.hpp>
 
-#include <libnest2d/optimizers/nlopt/genetic.hpp>
-#include <libnest2d/optimizers/nlopt/subplex.hpp>
+#include <libslic3r/SLA/SpatIndex.hpp>
+#include <libslic3r/Optimizer.hpp>
 #include <boost/log/trivial.hpp>
 
 namespace Slic3r {
 namespace sla {
 
-static const Vec3d DOWN = {0.0, 0.0, -1.0};
+using Slic3r::opt::initvals;
+using Slic3r::opt::bounds;
+using Slic3r::opt::StopCriteria;
+using Slic3r::opt::Optimizer;
+using Slic3r::opt::AlgNLoptSubplex;
+using Slic3r::opt::AlgNLoptGenetic;
 
-using libnest2d::opt::initvals;
-using libnest2d::opt::bound;
-using libnest2d::opt::StopCriteria;
-using libnest2d::opt::GeneticOptimizer;
-using libnest2d::opt::SubplexOptimizer;
+StopCriteria get_criteria(const SupportTreeConfig &cfg)
+{
+    return StopCriteria{}
+        .rel_score_diff(cfg.optimizer_rel_score_diff)
+        .max_iterations(cfg.optimizer_max_iterations);
+}
+
+template<class C, class Hit = IndexedMesh::hit_result>
+static Hit min_hit(const C &hits)
+{
+    auto mit = std::min_element(hits.begin(), hits.end(),
+                                [](const Hit &h1, const Hit &h2) {
+                                    return h1.distance() < h2.distance();
+                                });
+
+    return *mit;
+}
 
 SupportTreeBuildsteps::SupportTreeBuildsteps(SupportTreeBuilder &   builder,
                                              const SupportableMesh &sm)
@@ -27,7 +44,7 @@ SupportTreeBuildsteps::SupportTreeBuildsteps(SupportTreeBuilder &   builder,
 {
     // Prepare the support points in Eigen/IGL format as well, we will use
     // it mostly in this form.
-    
+
     long i = 0;
     for (const SupportPoint &sp : m_support_pts) {
         m_points.row(i)(X) = double(sp.pos(X));
@@ -41,9 +58,11 @@ bool SupportTreeBuildsteps::execute(SupportTreeBuilder &   builder,
                                     const SupportableMesh &sm)
 {
     if(sm.pts.empty()) return false;
-    
+
+    builder.ground_level = sm.emesh.ground_level() - sm.cfg.object_elevation_mm;
+
     SupportTreeBuildsteps alg(builder, sm);
-    
+
     // Let's define the individual steps of the processing. We can experiment
     // later with the ordering and the dependencies between them.
     enum Steps {
@@ -54,59 +73,52 @@ bool SupportTreeBuildsteps::execute(SupportTreeBuilder &   builder,
         ROUTING_GROUND,
         ROUTING_NONGROUND,
         CASCADE_PILLARS,
-        HEADLESS,
         MERGE_RESULT,
         DONE,
         ABORT,
         NUM_STEPS
         //...
     };
-    
+
     // Collect the algorithm steps into a nice sequence
     std::array<std::function<void()>, NUM_STEPS> program = {
         [] () {
             // Begin...
             // Potentially clear up the shared data (not needed for now)
         },
-        
+
         std::bind(&SupportTreeBuildsteps::filter, &alg),
-        
+
         std::bind(&SupportTreeBuildsteps::add_pinheads, &alg),
-        
+
         std::bind(&SupportTreeBuildsteps::classify, &alg),
-        
+
         std::bind(&SupportTreeBuildsteps::routing_to_ground, &alg),
-        
+
         std::bind(&SupportTreeBuildsteps::routing_to_model, &alg),
-        
+
         std::bind(&SupportTreeBuildsteps::interconnect_pillars, &alg),
-        
-        std::bind(&SupportTreeBuildsteps::routing_headless, &alg),
-        
+
         std::bind(&SupportTreeBuildsteps::merge_result, &alg),
-        
+
         [] () {
             // Done
         },
-        
+
         [] () {
             // Abort
         }
     };
-    
+
     Steps pc = BEGIN;
-    
+
     if(sm.cfg.ground_facing_only) {
         program[ROUTING_NONGROUND] = []() {
             BOOST_LOG_TRIVIAL(info)
                 << "Skipping model-facing supports as requested.";
         };
-        program[HEADLESS] = []() {
-            BOOST_LOG_TRIVIAL(info) << "Skipping headless stick generation as"
-                                       " requested.";
-        };
     }
-    
+
     // Let's define a simple automaton that will run our program.
     auto progress = [&builder, &pc] () {
         static const std::array<std::string, NUM_STEPS> stepstr {
@@ -117,12 +129,11 @@ bool SupportTreeBuildsteps::execute(SupportTreeBuilder &   builder,
             "Routing to ground",
             "Routing supports to model surface",
             "Interconnecting pillars",
-            "Processing small holes",
             "Merging support mesh",
             "Done",
             "Abort"
         };
-        
+
         static const std::array<unsigned, NUM_STEPS> stepstate {
             0,
             10,
@@ -131,14 +142,13 @@ bool SupportTreeBuildsteps::execute(SupportTreeBuilder &   builder,
             60,
             70,
             80,
-            85,
             99,
             100,
             0
         };
-        
+
         if(builder.ctl().stopcondition()) pc = ABORT;
-        
+
         switch(pc) {
         case BEGIN: pc = FILTER; break;
         case FILTER: pc = PINHEADS; break;
@@ -146,143 +156,76 @@ bool SupportTreeBuildsteps::execute(SupportTreeBuilder &   builder,
         case CLASSIFY: pc = ROUTING_GROUND; break;
         case ROUTING_GROUND: pc = ROUTING_NONGROUND; break;
         case ROUTING_NONGROUND: pc = CASCADE_PILLARS; break;
-        case CASCADE_PILLARS: pc = HEADLESS; break;
-        case HEADLESS: pc = MERGE_RESULT; break;
+        case CASCADE_PILLARS: pc = MERGE_RESULT; break;
         case MERGE_RESULT: pc = DONE; break;
         case DONE:
         case ABORT: break;
         default: ;
         }
-        
+
         builder.ctl().statuscb(stepstate[pc], stepstr[pc]);
     };
-    
+
     // Just here we run the computation...
     while(pc < DONE) {
         progress();
         program[pc]();
     }
-    
+
     return pc == ABORT;
 }
 
-// Give points on a 3D ring with given center, radius and orientation
-// method based on:
-// https://math.stackexchange.com/questions/73237/parametric-equation-of-a-circle-in-3d-space
-template<size_t N>
-class PointRing {
-    std::array<double, N> m_phis;
-    
-    // Two vectors that will be perpendicular to each other and to the
-    // axis. Values for a(X) and a(Y) are now arbitrary, a(Z) is just a
-    // placeholder.
-    // a and b vectors are perpendicular to the ring direction and to each other.
-    // Together they define the plane where we have to iterate with the
-    // given angles in the 'm_phis' vector
-    Vec3d a = {0, 1, 0}, b;
-    double m_radius = 0.;
-    
-    static inline bool constexpr is_one(double val) 
-    { 
-        return std::abs(std::abs(val) - 1) < 1e-20;
-    }
-    
-public:
-    
-    PointRing(const Vec3d &n)
-    {
-        m_phis = linspace_array<N>(0., 2 * PI);
-    
-        // We have to address the case when the direction vector v (same as
-        // dir) is coincident with one of the world axes. In this case two of
-        // its components will be completely zero and one is 1.0. Our method
-        // becomes dangerous here due to division with zero. Instead, vector
-        // 'a' can be an element-wise rotated version of 'v'
-        if(is_one(n(X)) || is_one(n(Y)) || is_one(n(Z))) {
-            a = {n(Z), n(X), n(Y)};
-            b = {n(Y), n(Z), n(X)};
-        }
-        else {
-            a(Z) = -(n(Y)*a(Y)) / n(Z); a.normalize();
-            b = a.cross(n);
-        }
-    }
-    
-    Vec3d get(size_t idx, const Vec3d src, double r) const
-    {
-        double phi = m_phis[idx];
-        double sinphi = std::sin(phi);
-        double cosphi = std::cos(phi);
-     
-        double rpscos = r * cosphi;
-        double rpssin = r * sinphi;
-     
-        // Point on the sphere
-        return {src(X) + rpscos * a(X) + rpssin * b(X),
-                src(Y) + rpscos * a(Y) + rpssin * b(Y),
-                src(Z) + rpscos * a(Z) + rpssin * b(Z)};
-    }
-};
-
-template<class C, class Hit = EigenMesh3D::hit_result> 
-static Hit min_hit(const C &hits)
-{
-    auto mit = std::min_element(hits.begin(), hits.end(), 
-                                [](const Hit &h1, const Hit &h2) { 
-        return h1.distance() < h2.distance(); 
-    });
-    
-    return *mit;
-}
-
-EigenMesh3D::hit_result SupportTreeBuildsteps::pinhead_mesh_intersect(
-    const Vec3d &s, const Vec3d &dir, double r_pin, double r_back, double width)
+IndexedMesh::hit_result SupportTreeBuildsteps::pinhead_mesh_intersect(
+    const Vec3d &s,
+    const Vec3d &dir,
+    double       r_pin,
+    double       r_back,
+    double       width,
+    double       sd)
 {
     static const size_t SAMPLES = 8;
-    
+
     // Move away slightly from the touching point to avoid raycasting on the
     // inner surface of the mesh.
-    
-    const double& sd = m_cfg.safety_distance_mm;
-    
+
     auto& m = m_mesh;
-    using HitResult = EigenMesh3D::hit_result;
-    
+    using HitResult = IndexedMesh::hit_result;
+
     // Hit results
     std::array<HitResult, SAMPLES> hits;
-    
+
     struct Rings {
         double rpin;
         double rback;
         Vec3d  spin;
         Vec3d  sback;
         PointRing<SAMPLES> ring;
-        
+
         Vec3d backring(size_t idx) { return ring.get(idx, sback, rback); }
         Vec3d pinring(size_t idx) { return ring.get(idx, spin, rpin); }
     } rings {r_pin + sd, r_back + sd, s, s + width * dir, dir};
-    
+
     // We will shoot multiple rays from the head pinpoint in the direction
     // of the pinhead robe (side) surface. The result will be the smallest
     // hit distance.
-    
-    ccr::enumerate(hits.begin(), hits.end(), 
+
+    ccr::enumerate(hits.begin(), hits.end(),
                    [&m, &rings, sd](HitResult &hit, size_t i) {
-    
+
        // Point on the circle on the pin sphere
        Vec3d ps = rings.pinring(i);
        // This is the point on the circle on the back sphere
        Vec3d p = rings.backring(i);
-       
+
        // Point ps is not on mesh but can be inside or
        // outside as well. This would cause many problems
        // with ray-casting. To detect the position we will
        // use the ray-casting result (which has an is_inside
-       // predicate).       
-    
+       // predicate).
+
        Vec3d n = (p - ps).normalized();
        auto  q = m.query_ray_hit(ps + sd * n, n);
-    
+
        if (q.is_inside()) { // the hit is inside the model
            if (q.distance() > rings.rpin) {
                // If we are inside the model and the hit
@@ -307,40 +250,38 @@ EigenMesh3D::hit_result SupportTreeBuildsteps::pinhead_mesh_intersect(
        } else
            hit = q;
     });
-    
+
     return min_hit(hits);
 }
 
-EigenMesh3D::hit_result SupportTreeBuildsteps::bridge_mesh_intersect(
-    const Vec3d &src, const Vec3d &dir, double r, bool ins_check)
+IndexedMesh::hit_result SupportTreeBuildsteps::bridge_mesh_intersect(
+    const Vec3d &src, const Vec3d &dir, double r, double sd)
 {
     static const size_t SAMPLES = 8;
     PointRing<SAMPLES> ring{dir};
-    
-    using Hit = EigenMesh3D::hit_result;
-    
+
+    using Hit = IndexedMesh::hit_result;
+
     // Hit results
     std::array<Hit, SAMPLES> hits;
-    
-    ccr::enumerate(hits.begin(), hits.end(), 
-                   [this, r, src, ins_check, &ring, dir] (Hit &hit, size_t i) {
-        
-        const double sd = m_cfg.safety_distance_mm;
-        
+
+    ccr::enumerate(hits.begin(), hits.end(),
+                [this, r, src, /*ins_check,*/ &ring, dir, sd] (Hit &hit, size_t i) {
+
         // Point on the circle on the pin sphere
         Vec3d p = ring.get(i, src, r + sd);
-        
-        auto hr = m_mesh.query_ray_hit(p + sd * dir, dir);
-        
-        if(ins_check && hr.is_inside()) {
+
+        auto hr = m_mesh.query_ray_hit(p + r * dir, dir);
+
+        if(/*ins_check && */hr.is_inside()) {
             if(hr.distance() > 2 * r + sd) hit = Hit(0.0);
             else {
                 // re-cast the ray from the outside of the object
-                hit = m_mesh.query_ray_hit(p + (hr.distance() + 2 * sd) * dir, dir);
+                hit = m_mesh.query_ray_hit(p + (hr.distance() + EPSILON) * dir, dir);
             }
         } else hit = hr;
     });
-    
+
     return min_hit(hits);
 }
 
@@ -354,61 +295,61 @@ bool SupportTreeBuildsteps::interconnect(const Pillar &pillar,
     // shorter pillar is too short to start a new bridge but the taller
     // pillar could still be bridged with the shorter one.
     bool was_connected = false;
-    
+
     Vec3d supper = pillar.startpoint();
     Vec3d slower = nextpillar.startpoint();
     Vec3d eupper = pillar.endpoint();
     Vec3d elower = nextpillar.endpoint();
-    
+
     double zmin = m_builder.ground_level + m_cfg.base_height_mm;
     eupper(Z) = std::max(eupper(Z), zmin);
     elower(Z) = std::max(elower(Z), zmin);
-    
+
     // The usable length of both pillars should be positive
     if(slower(Z) - elower(Z) < 0) return false;
     if(supper(Z) - eupper(Z) < 0) return false;
-    
+
     double pillar_dist = distance(Vec2d{slower(X), slower(Y)},
                                   Vec2d{supper(X), supper(Y)});
     double bridge_distance = pillar_dist / std::cos(-m_cfg.bridge_slope);
     double zstep = pillar_dist * std::tan(-m_cfg.bridge_slope);
-    
+
     if(pillar_dist < 2 * m_cfg.head_back_radius_mm ||
         pillar_dist > m_cfg.max_pillar_link_distance_mm) return false;
-    
+
     if(supper(Z) < slower(Z)) supper.swap(slower);
     if(eupper(Z) < elower(Z)) eupper.swap(elower);
-    
+
     double startz = 0, endz = 0;
-    
+
     startz = slower(Z) - zstep < supper(Z) ? slower(Z) - zstep : slower(Z);
     endz = eupper(Z) + zstep > elower(Z) ? eupper(Z) + zstep : eupper(Z);
-    
+
     if(slower(Z) - eupper(Z) < std::abs(zstep)) {
         // no space for even one cross
-        
+
         // Get max available space
         startz = std::min(supper(Z), slower(Z) - zstep);
         endz = std::max(eupper(Z) + zstep, elower(Z));
-        
+
         // Align to center
         double available_dist = (startz - endz);
         double rounds = std::floor(available_dist / std::abs(zstep));
         startz -= 0.5 * (available_dist - rounds * std::abs(zstep));
     }
-    
+
     auto pcm = m_cfg.pillar_connection_mode;
     bool docrosses =
         pcm == PillarConnectionMode::cross ||
         (pcm == PillarConnectionMode::dynamic &&
          pillar_dist > 2*m_cfg.base_radius_mm);
-    
+
     // 'sj' means starting junction, 'ej' is the end junction of a bridge.
     // They will be swapped in every iteration thus the zig-zag pattern.
     // According to a config parameter, a second bridge may be added which
     // results in a cross connection between the pillars.
     Vec3d sj = supper, ej = slower; sj(Z) = startz; ej(Z) = sj(Z) + zstep;
-    
+
     // TODO: This is a workaround to not have a faulty last bridge
     while(ej(Z) >= eupper(Z) /*endz*/) {
         if(bridge_mesh_distance(sj, dirv(sj, ej), pillar.r) >= bridge_distance)
@@ -416,7 +357,7 @@ bool SupportTreeBuildsteps::interconnect(const Pillar &pillar,
             m_builder.add_crossbridge(sj, ej, pillar.r);
             was_connected = true;
         }
-        
+
         // double bridging: (crosses)
         if(docrosses) {
             Vec3d sjback(ej(X), ej(Y), sj(Z));
@@ -429,11 +370,11 @@ bool SupportTreeBuildsteps::interconnect(const Pillar &pillar,
                 was_connected = true;
             }
         }
-        
+
         sj.swap(ej);
         ej(Z) = sj(Z) + zstep;
     }
-    
+
     return was_connected;
 }
 
@@ -443,228 +384,242 @@ bool SupportTreeBuildsteps::connect_to_nearpillar(const Head &head,
     auto nearpillar = [this, nearpillar_id]() -> const Pillar& {
         return m_builder.pillar(nearpillar_id);
     };
-    
-    if (m_builder.bridgecount(nearpillar()) > m_cfg.max_bridges_on_pillar) 
+
+    if (m_builder.bridgecount(nearpillar()) > m_cfg.max_bridges_on_pillar)
         return false;
-    
+
     Vec3d headjp = head.junction_point();
     Vec3d nearjp_u = nearpillar().startpoint();
     Vec3d nearjp_l = nearpillar().endpoint();
-    
+
     double r = head.r_back_mm;
     double d2d = distance(to_2d(headjp), to_2d(nearjp_u));
     double d3d = distance(headjp, nearjp_u);
-    
+
     double hdiff = nearjp_u(Z) - headjp(Z);
     double slope = std::atan2(hdiff, d2d);
-    
+
     Vec3d bridgestart = headjp;
     Vec3d bridgeend = nearjp_u;
-    double max_len = m_cfg.max_bridge_length_mm;
+    double max_len = r * m_cfg.max_bridge_length_mm / m_cfg.head_back_radius_mm;
     double max_slope = m_cfg.bridge_slope;
     double zdiff = 0.0;
-    
+
     // check the default situation if feasible for a bridge
     if(d3d > max_len || slope > -max_slope) {
         // not feasible to connect the two head junctions. We have to search
         // for a suitable touch point.
-        
+
         double Zdown = headjp(Z) + d2d * std::tan(-max_slope);
         Vec3d touchjp = bridgeend; touchjp(Z) = Zdown;
         double D = distance(headjp, touchjp);
         zdiff = Zdown - nearjp_u(Z);
-        
+
         if(zdiff > 0) {
             Zdown -= zdiff;
             bridgestart(Z) -= zdiff;
             touchjp(Z) = Zdown;
-            
+
             double t = bridge_mesh_distance(headjp, DOWN, r);
-            
+
             // We can't insert a pillar under the source head to connect
             // with the nearby pillar's starting junction
             if(t < zdiff) return false;
         }
-        
+
         if(Zdown <= nearjp_u(Z) && Zdown >= nearjp_l(Z) && D < max_len)
             bridgeend(Z) = Zdown;
         else
             return false;
     }
-    
+
     // There will be a minimum distance from the ground where the
     // bridge is allowed to connect. This is an empiric value.
-    double minz = m_builder.ground_level + 2 * m_cfg.head_width_mm;
+    double minz = m_builder.ground_level + 4 * head.r_back_mm;
     if(bridgeend(Z) < minz) return false;
-    
+
     double t = bridge_mesh_distance(bridgestart, dirv(bridgestart, bridgeend), r);
-    
+
     // Cannot insert the bridge. (further search might not worth the hassle)
     if(t < distance(bridgestart, bridgeend)) return false;
-    
+
     std::lock_guard<ccr::BlockingMutex> lk(m_bridge_mutex);
-    
+
     if (m_builder.bridgecount(nearpillar()) < m_cfg.max_bridges_on_pillar) {
         // A partial pillar is needed under the starting head.
         if(zdiff > 0) {
-            m_builder.add_pillar(head.id, bridgestart, r);
+            m_builder.add_pillar(head.id, headjp.z() - bridgestart.z());
             m_builder.add_junction(bridgestart, r);
-            m_builder.add_bridge(bridgestart, bridgeend, head.r_back_mm);
+            m_builder.add_bridge(bridgestart, bridgeend, r);
         } else {
             m_builder.add_bridge(head.id, bridgeend);
         }
-        
+
         m_builder.increment_bridges(nearpillar());
     } else return false;
-    
+
     return true;
 }
 
-bool SupportTreeBuildsteps::search_pillar_and_connect(const Head &head)
-{
-    PointIndex spindex = m_pillar_index.guarded_clone();
-    
-    long nearest_id = ID_UNSET;
-    
-    Vec3d querypoint = head.junction_point();
-    
-    while(nearest_id < 0 && !spindex.empty()) { m_thr();
-        // loop until a suitable head is not found
-        // if there is a pillar closer than the cluster center
-        // (this may happen as the clustering is not perfect)
-        // than we will bridge to this closer pillar
-        
-        Vec3d qp(querypoint(X), querypoint(Y), m_builder.ground_level);
-        auto qres = spindex.nearest(qp, 1);
-        if(qres.empty()) break;
-        
-        auto ne = qres.front();
-        nearest_id = ne.second;
-        
-        if(nearest_id >= 0) {
-            if(size_t(nearest_id) < m_builder.pillarcount()) {
-                if(!connect_to_nearpillar(head, nearest_id)) {
-                    nearest_id = ID_UNSET;    // continue searching
-                    spindex.remove(ne);       // without the current pillar
-                }
-            }
-        }
-    }
-    
-    return nearest_id >= 0;
-}
-
-void SupportTreeBuildsteps::create_ground_pillar(const Vec3d &jp,
+bool SupportTreeBuildsteps::create_ground_pillar(const Vec3d &hjp,
                                                  const Vec3d &sourcedir,
                                                  double       radius,
                                                  long         head_id)
 {
-    const double SLOPE = 1. / std::cos(m_cfg.bridge_slope);
-    
-    double gndlvl       = m_builder.ground_level;
-    Vec3d  endp         = {jp(X), jp(Y), gndlvl};
-    double sd           = m_cfg.pillar_base_safety_distance_mm;
-    long   pillar_id    = ID_UNSET;
-    double min_dist     = sd + m_cfg.base_radius_mm + EPSILON;
-    double dist         = 0;
-    bool   can_add_base = true;
-    bool   normal_mode  = true;
-    
-    // If in zero elevation mode and the pillar is too close to the model body,
-    // the support pillar can not be placed in the gap between the model and
-    // the pad, and the pillar bases must not touch the model body either.
-    // To solve this, a corrector bridge is inserted between the starting point
-    // (jp) and the new pillar.
-    if (m_cfg.object_elevation_mm < EPSILON
-        && (dist = std::sqrt(m_mesh.squared_distance(endp))) < min_dist) {
-        // Get the distance from the mesh. This can be later optimized
-        // to get the distance in 2D plane because we are dealing with
-        // the ground level only.
+    Vec3d  jp           = hjp, endp = jp, dir = sourcedir;
+    long   pillar_id    = SupportTreeNode::ID_UNSET;
+    bool   can_add_base = false, non_head = false;
 
-        normal_mode = false;
+    double gndlvl = 0.; // The Z level where pedestals should be
+    double jp_gnd = 0.; // The lowest Z where a junction center can be
+    double gap_dist = 0.; // The gap distance between the model and the pad
 
-        // The min distance needed to move away from the model in XY plane.
-        double current_d = min_dist - dist;
-        double current_bride_d = SLOPE * current_d;
+    auto to_floor = [&gndlvl](const Vec3d &p) { return Vec3d{p.x(), p.y(), gndlvl}; };
 
+    auto eval_limits = [this, &radius, &can_add_base, &gndlvl, &gap_dist, &jp_gnd]
+        (bool base_en = true)
+    {
+        can_add_base  = base_en && radius >= m_cfg.head_back_radius_mm;
+        double base_r = can_add_base ? m_cfg.base_radius_mm : 0.;
+        gndlvl        = m_builder.ground_level;
+        if (!can_add_base) gndlvl -= m_mesh.ground_level_offset();
+        jp_gnd   = gndlvl + (can_add_base ? 0. : m_cfg.head_back_radius_mm);
+        gap_dist = m_cfg.pillar_base_safety_distance_mm + base_r + EPSILON;
+    };
+
+    eval_limits();
+
+    // We are dealing with a mini pillar that's potentially too long
+    if (radius < m_cfg.head_back_radius_mm && jp.z() - gndlvl > 20 * radius)
+    {
+        std::optional<DiffBridge> diffbr =
+            search_widening_path(jp, dir, radius, m_cfg.head_back_radius_mm);
+
+        if (diffbr && diffbr->endp.z() > jp_gnd) {
+            auto &br = m_builder.add_diffbridge(*diffbr);
+            if (head_id >= 0) m_builder.head(head_id).bridge_id = br.id;
+            endp = diffbr->endp;
+            radius = diffbr->end_r;
+            m_builder.add_junction(endp, radius);
+            non_head = true;
+            dir = diffbr->get_dir();
+            eval_limits();
+        } else return false;
+    }
+
+    if (m_cfg.object_elevation_mm < EPSILON)
+    {
         // get a suitable direction for the corrector bridge. It is the
         // original sourcedir's azimuth but the polar angle is saturated to the
         // configured bridge slope.
-        auto [polar, azimuth] = dir_to_spheric(sourcedir);
+        auto [polar, azimuth] = dir_to_spheric(dir);
         polar = PI - m_cfg.bridge_slope;
-        auto dir = spheric_to_dir(polar, azimuth).normalized();
-        
-        StopCriteria scr;
-        scr.stop_score = min_dist;
-        SubplexOptimizer solver(scr);
-        
-        // Search for a distance along the corrector bridge to move the endpoint
-        // sufficiently away form the model body. The first few optimization
-        // cycles should succeed here.
-        auto result = solver.optimize_max(
-            [this, dir, jp, gndlvl](double mv) {
-                Vec3d endpt = jp + mv * dir;
-                endpt(Z)    = gndlvl;
-                return std::sqrt(m_mesh.squared_distance(endpt));
-            },
-            initvals(current_bride_d), 
-            bound(0.0, m_cfg.max_bridge_length_mm - current_bride_d));
-        
-        endp         = jp + std::get<0>(result.optimum) * dir;
-        Vec3d pgnd   = {endp(X), endp(Y), gndlvl};
-        can_add_base = result.score > min_dist;
-        
-        double gnd_offs = m_mesh.ground_level_offset();
-        auto abort_in_shame =
-            [gnd_offs, &normal_mode, &can_add_base, &endp, jp, gndlvl]()
-        {
-            normal_mode  = true;
-            can_add_base = false;   // Nothing left to do, hope for the best
-            endp         = {jp(X), jp(Y), gndlvl - gnd_offs };
-        };
-        
-        // We have to check if the bridge is feasible.
-        if (bridge_mesh_distance(jp, dir, radius) < (endp - jp).norm())
-            abort_in_shame();
-        else {
-            // If the new endpoint is below ground, do not make a pillar
-            if (endp(Z) < gndlvl)
-                endp = endp - SLOPE * (gndlvl - endp(Z)) * dir; // back off
-            else {
-                
-                auto hit = bridge_mesh_intersect(endp, DOWN, radius);
-                if (!std::isinf(hit.distance())) abort_in_shame();
-                
-                pillar_id = m_builder.add_pillar(endp, pgnd, radius);
-                
-                if (can_add_base)
-                    m_builder.add_pillar_base(pillar_id, m_cfg.base_height_mm,
-                                              m_cfg.base_radius_mm);
+        Vec3d d = spheric_to_dir(polar, azimuth).normalized();
+        double t = bridge_mesh_distance(endp, dir, radius);
+        double tmax = std::min(m_cfg.max_bridge_length_mm, t);
+        t = 0.;
+
+        double zd = endp.z() - jp_gnd;
+        double tmax2 = zd / std::sqrt(1 - m_cfg.bridge_slope * m_cfg.bridge_slope);
+        tmax = std::min(tmax, tmax2);
+
+        Vec3d nexp = endp;
+        double dlast = 0.;
+        while (((dlast = std::sqrt(m_mesh.squared_distance(to_floor(nexp)))) < gap_dist ||
+                !std::isinf(bridge_mesh_distance(nexp, DOWN, radius))) && t < tmax) {
+            t += radius;
+            nexp = endp + t * d;
+        }
+
+        if (dlast < gap_dist && can_add_base) {
+            nexp         = endp;
+            t            = 0.;
+            can_add_base = false;
+            eval_limits(can_add_base);
+
+            zd = endp.z() - jp_gnd;
+            tmax2 = zd / std::sqrt(1 - m_cfg.bridge_slope * m_cfg.bridge_slope);
+            tmax = std::min(tmax, tmax2);
+
+            while (((dlast = std::sqrt(m_mesh.squared_distance(to_floor(nexp)))) < gap_dist ||
+                    !std::isinf(bridge_mesh_distance(nexp, DOWN, radius))) && t < tmax) {
+                t += radius;
+                nexp = endp + t * d;
             }
-            
-            m_builder.add_bridge(jp, endp, radius);
-            m_builder.add_junction(endp, radius);
-            
-            // Add a degenerated pillar and the bridge.
-            // The degenerate pillar will have zero length and it will
-            // prevent from queries of head_pillar() to have non-existing
-            // pillar when the head should have one.
-            if (head_id >= 0)
-                m_builder.add_pillar(head_id, jp, radius);
+        }
+
+        // Could not find a path to avoid the pad gap
+        if (dlast < gap_dist) return false;
+
+        if (t > 0.) { // Need to make additional bridge
+            const Bridge& br = m_builder.add_bridge(endp, nexp, radius);
+            if (head_id >= 0) m_builder.head(head_id).bridge_id = br.id;
+
+            m_builder.add_junction(nexp, radius);
+            endp = nexp;
+            non_head = true;
         }
     }
-    
-    if (normal_mode) {
-        pillar_id = head_id >= 0 ? m_builder.add_pillar(head_id, endp, radius) :
-                                   m_builder.add_pillar(jp, endp, radius);
 
-        if (can_add_base)
-            m_builder.add_pillar_base(pillar_id, m_cfg.base_height_mm,
-                                      m_cfg.base_radius_mm);
-    }
-    
+    Vec3d gp = to_floor(endp);
+    double h = endp.z() - gp.z();
+
+    pillar_id = head_id >= 0 && !non_head ? m_builder.add_pillar(head_id, h) :
+                                            m_builder.add_pillar(gp, h, radius);
+
+    if (can_add_base)
+        add_pillar_base(pillar_id);
+
     if(pillar_id >= 0) // Save the pillar endpoint in the spatial index
-        m_pillar_index.guarded_insert(endp, unsigned(pillar_id));
+        m_pillar_index.guarded_insert(m_builder.pillar(pillar_id).endpt,
+                                      unsigned(pillar_id));
+
+    return true;
+}
+
+std::optional<DiffBridge> SupportTreeBuildsteps::search_widening_path(
+    const Vec3d &jp, const Vec3d &dir, double radius, double new_radius)
+{
+    double w = radius + 2 * m_cfg.head_back_radius_mm;
+    double stopval = w + jp.z() - m_builder.ground_level;
+    Optimizer<AlgNLoptSubplex> solver(get_criteria(m_cfg).stop_score(stopval));
+
+    auto [polar, azimuth] = dir_to_spheric(dir);
+
+    double fallback_ratio = radius / m_cfg.head_back_radius_mm;
+
+    auto oresult = solver.to_max().optimize(
+        [this, jp, radius, new_radius](const opt::Input<3> &input) {
+            auto &[plr, azm, t] = input;
+
+            auto   d   = spheric_to_dir(plr, azm).normalized();
+            double ret = pinhead_mesh_intersect(jp, d, radius, new_radius, t)
+                             .distance();
+            double down = bridge_mesh_distance(jp + t * d, d, new_radius);
+
+            if (ret > t && std::isinf(down))
+                ret += jp.z() - m_builder.ground_level;
+
+            return ret;
+        },
+        initvals({polar, azimuth, w}), // start with what we have
+        bounds({
+            {PI - m_cfg.bridge_slope, PI}, // Must not exceed the slope limit
+            {-PI, PI}, // azimuth can be a full search
+            {radius + m_cfg.head_back_radius_mm,
+                  fallback_ratio * m_cfg.max_bridge_length_mm}
+        }));
+
+    if (oresult.score >= stopval) {
+        polar       = std::get<0>(oresult.optimum);
+        azimuth     = std::get<1>(oresult.optimum);
+        double t    = std::get<2>(oresult.optimum);
+        Vec3d  endp = jp + t * spheric_to_dir(polar, azimuth);
+
+        return DiffBridge(jp, endp, radius, m_cfg.head_back_radius_mm);
+    }
+
+    return {};
 }
 
 void SupportTreeBuildsteps::filter()
@@ -672,7 +627,7 @@ void SupportTreeBuildsteps::filter()
     // Get the points that are too close to each other and keep only the
     // first one
     auto aliases = cluster(m_points, D_SP, 2);
-    
+
     PtIndices filtered_indices;
     filtered_indices.reserve(aliases.size());
     m_iheads.reserve(aliases.size());
@@ -681,136 +636,130 @@ void SupportTreeBuildsteps::filter()
         // Here we keep only the front point of the cluster.
         filtered_indices.emplace_back(a.front());
     }
-    
+
     // calculate the normals to the triangles for filtered points
     auto nmls = sla::normals(m_points, m_mesh, m_cfg.head_front_radius_mm,
                              m_thr, filtered_indices);
-    
+
     // Not all of the support points have to be a valid position for
     // support creation. The angle may be inappropriate or there may
     // not be enough space for the pinhead. Filtering is applied for
     // these reasons.
-    
-    ccr::SpinningMutex mutex;
-    auto addfn = [&mutex](PtIndices &container, unsigned val) {
-        std::lock_guard<ccr::SpinningMutex> lk(mutex);
-        container.emplace_back(val);
-    };
-    
-    auto filterfn = [this, &nmls, addfn](unsigned fidx, size_t i) {
+
+    std::vector<Head> heads; heads.reserve(m_support_pts.size());
+    for (const SupportPoint &sp : m_support_pts) {
         m_thr();
-        
+        heads.emplace_back(
+            std::nan(""),
+            sp.head_front_radius,
+            0.,
+            m_cfg.head_penetration_mm,
+            Vec3d::Zero(),         // dir
+            sp.pos.cast<double>() // displacement
+            );
+    }
+
+    std::function<void(unsigned, size_t, double)> filterfn;
+    filterfn = [this, &nmls, &heads, &filterfn](unsigned fidx, size_t i, double back_r) {
+        m_thr();
+
         auto n = nmls.row(Eigen::Index(i));
-        
+
         // for all normals we generate the spherical coordinates and
         // saturate the polar angle to 45 degrees from the bottom then
         // convert back to standard coordinates to get the new normal.
         // Then we just create a quaternion from the two normals
         // (Quaternion::FromTwoVectors) and apply the rotation to the
         // arrow head.
-        
+
         auto [polar, azimuth] = dir_to_spheric(n);
-        
+
         // skip if the tilt is not sane
-        if(polar >= PI - m_cfg.normal_cutoff_angle) {
-            
-            // We saturate the polar angle to 3pi/4
-            polar = std::max(polar, 3*PI / 4);
-            
-            // save the head (pinpoint) position
-            Vec3d hp = m_points.row(fidx);
-            
-            double w = m_cfg.head_width_mm +
-                       m_cfg.head_back_radius_mm +
-                       2*m_cfg.head_front_radius_mm;
-            
-            double pin_r = double(m_support_pts[fidx].head_front_radius);
-            
-            // Reassemble the now corrected normal
-            auto nn = spheric_to_dir(polar, azimuth).normalized();
-            
-            // check available distance
-            EigenMesh3D::hit_result t
-                = pinhead_mesh_intersect(hp, // touching point
-                                         nn, // normal
-                                         pin_r,
-                                         m_cfg.head_back_radius_mm,
-                                         w);
-            
-            if(t.distance() <= w) {
-                
-                // Let's try to optimize this angle, there might be a
-                // viable normal that doesn't collide with the model
-                // geometry and its very close to the default.
-                
-                StopCriteria stc;
-                stc.max_iterations = m_cfg.optimizer_max_iterations;
-                stc.relative_score_difference = m_cfg.optimizer_rel_score_diff;
-                stc.stop_score = w; // space greater than w is enough
-                GeneticOptimizer solver(stc);
-                solver.seed(0); // we want deterministic behavior
-                
-                auto oresult = solver.optimize_max(
-                    [this, pin_r, w, hp](double plr, double azm)
-                    {
-                        auto dir = spheric_to_dir(plr, azm).normalized();
-                        
-                        double score = pinhead_mesh_distance(
-                            hp, dir, pin_r, m_cfg.head_back_radius_mm, w);
-                        
-                        return score;
-                    },
-                    initvals(polar, azimuth), // start with what we have
-                    bound(3 * PI / 4, PI),    // Must not exceed the tilt limit
-                    bound(-PI, PI) // azimuth can be a full search
-                    );
-                
-                if(oresult.score > w) {
-                    polar = std::get<0>(oresult.optimum);
-                    azimuth = std::get<1>(oresult.optimum);
-                    nn = spheric_to_dir(polar, azimuth).normalized();
-                    t = EigenMesh3D::hit_result(oresult.score);
-                }
-            }
-            
-            // save the verified and corrected normal
-            m_support_nmls.row(fidx) = nn;
-            
-            if (t.distance() > w) {
-                // Check distance from ground, we might have zero elevation.
-                if (hp(Z) + w * nn(Z) < m_builder.ground_level) {
-                    addfn(m_iheadless, fidx);
-                } else {
-                    // mark the point for needing a head.
-                    addfn(m_iheads, fidx);
-                }
-            } else if (polar >= 3 * PI / 4) {
-                // Headless supports do not tilt like the headed ones
-                // so the normal should point almost to the ground.
-                addfn(m_iheadless, fidx);
+        if (polar < PI - m_cfg.normal_cutoff_angle) return;
+
+        // We saturate the polar angle to 3pi/4
+        polar = std::max(polar, PI - m_cfg.bridge_slope);
+
+        // save the head (pinpoint) position
+        Vec3d hp = m_points.row(fidx);
+
+        double lmin = m_cfg.head_width_mm, lmax = lmin;
+
+        if (back_r < m_cfg.head_back_radius_mm) {
+            lmin = 0., lmax = m_cfg.head_penetration_mm;
+        }
+
+        // The distance needed for a pinhead to not collide with model.
+        double w = lmin + 2 * back_r + 2 * m_cfg.head_front_radius_mm -
+                   m_cfg.head_penetration_mm;
+
+        double pin_r = double(m_support_pts[fidx].head_front_radius);
+
+        // Reassemble the now corrected normal
+        auto nn = spheric_to_dir(polar, azimuth).normalized();
+
+        // check available distance
+        IndexedMesh::hit_result t = pinhead_mesh_intersect(hp, nn, pin_r,
+                                                           back_r, w);
+
+        if (t.distance() < w) {
+            // Let's try to optimize this angle, there might be a
+            // viable normal that doesn't collide with the model
+            // geometry and its very close to the default.
+
+            Optimizer<AlgNLoptGenetic> solver(get_criteria(m_cfg));
+            solver.seed(0); // we want deterministic behavior
+
+            auto oresult = solver.to_max().optimize(
+                [this, pin_r, back_r, hp](const opt::Input<3> &input)
+                {
+                    auto &[plr, azm, l] = input;
+
+                    auto dir = spheric_to_dir(plr, azm).normalized();
+
+                    return pinhead_mesh_intersect(
+                        hp, dir, pin_r, back_r, l).distance();
+                },
+                initvals({polar, azimuth, (lmin + lmax) / 2.}), // start with what we have
+                bounds({
+                    {PI - m_cfg.bridge_slope, PI},    // Must not exceed the slope limit
+                    {-PI, PI}, // azimuth can be a full search
+                    {lmin, lmax}
+                }));
+
+            if(oresult.score > w) {
+                polar = std::get<0>(oresult.optimum);
+                azimuth = std::get<1>(oresult.optimum);
+                nn = spheric_to_dir(polar, azimuth).normalized();
+                lmin = std::get<2>(oresult.optimum);
+                t = IndexedMesh::hit_result(oresult.score);
             }
         }
+
+        if (t.distance() > w && hp(Z) + w * nn(Z) >= m_builder.ground_level) {
+            Head &h = heads[fidx];
+            h.id = fidx; h.dir = nn; h.width_mm = lmin; h.r_back_mm = back_r;
+        } else if (back_r > m_cfg.head_fallback_radius_mm) {
+            filterfn(fidx, i, m_cfg.head_fallback_radius_mm);
+        }
     };
-    
-    ccr::enumerate(filtered_indices.begin(), filtered_indices.end(), filterfn);
-    
+
+    ccr::enumerate(filtered_indices.begin(), filtered_indices.end(),
+                   [this, &filterfn](unsigned fidx, size_t i) {
+                       filterfn(fidx, i, m_cfg.head_back_radius_mm);
+                   });
+
+    for (size_t i = 0; i < heads.size(); ++i)
+        if (heads[i].is_valid()) {
+            m_builder.add_head(i, heads[i]);
+            m_iheads.emplace_back(i);
+        }
+
     m_thr();
 }
 
 void SupportTreeBuildsteps::add_pinheads()
 {
-    for (unsigned i : m_iheads) {
-        m_thr();
-        m_builder.add_head(
-            i,
-            m_cfg.head_back_radius_mm,
-            m_support_pts[i].head_front_radius,
-            m_cfg.head_width_mm,
-            m_cfg.head_penetration_mm,
-            m_support_nmls.row(i),         // dir
-            m_support_pts[i].pos.cast<double>() // displacement
-            );
-    }
 }
 
 void SupportTreeBuildsteps::classify()
@@ -819,37 +768,37 @@ void SupportTreeBuildsteps::classify()
     PtIndices ground_head_indices;
     ground_head_indices.reserve(m_iheads.size());
     m_iheads_onmodel.reserve(m_iheads.size());
-    
+
     // First we decide which heads reach the ground and can be full
     // pillars and which shall be connected to the model surface (or
     // search a suitable path around the surface that leads to the
     // ground -- TODO)
     for(unsigned i : m_iheads) {
         m_thr();
-        
-        auto& head = m_builder.head(i);
+
+        Head &head = m_builder.head(i);
         double r = head.r_back_mm;
         Vec3d headjp = head.junction_point();
-        
+
         // collision check
         auto hit = bridge_mesh_intersect(headjp, DOWN, r);
-        
+
         if(std::isinf(hit.distance())) ground_head_indices.emplace_back(i);
         else if(m_cfg.ground_facing_only)  head.invalidate();
         else m_iheads_onmodel.emplace_back(i);
-        
+
         m_head_to_ground_scans[i] = hit;
     }
-    
+
     // We want to search for clusters of points that are far enough
     // from each other in the XY plane to not cross their pillar bases
     // These clusters of support points will join in one pillar,
     // possibly in their centroid support point.
-    
+
     auto pointfn = [this](unsigned i) {
         return m_builder.head(i).junction_point();
     };
-    
+
     auto predicate = [this](const PointIndexEl &e1,
                             const PointIndexEl &e2) {
         double d2d = distance(to_2d(e1.first), to_2d(e2.first));
@@ -864,14 +813,12 @@ void SupportTreeBuildsteps::classify()
 
 void SupportTreeBuildsteps::routing_to_ground()
 {
-    const double pradius = m_cfg.head_back_radius_mm;
-    
     ClusterEl cl_centroids;
     cl_centroids.reserve(m_pillar_clusters.size());
-    
+
     for (auto &cl : m_pillar_clusters) {
         m_thr();
-        
+
         // place all the centroid head positions into the index. We
         // will query for alternative pillar positions. If a sidehead
         // cannot connect to the cluster centroid, we have to search
@@ -879,9 +826,9 @@ void SupportTreeBuildsteps::routing_to_ground()
         // elements in the cluster, the centroid is arbitrary and the
         // sidehead is allowed to connect to a nearby pillar to
         // increase structural stability.
-        
+
         if (cl.empty()) continue;
-        
+
         // get the current cluster centroid
         auto &      thr    = m_thr;
         const auto &points = m_points;
@@ -895,43 +842,44 @@ void SupportTreeBuildsteps::routing_to_ground()
 
         assert(lcid >= 0);
         unsigned hid = cl[size_t(lcid)]; // Head ID
-        
+
         cl_centroids.emplace_back(hid);
-        
+
         Head &h = m_builder.head(hid);
-        h.transform();
-        
-        create_ground_pillar(h.junction_point(), h.dir, h.r_back_mm, h.id);
+
+        if (!create_ground_pillar(h.junction_point(), h.dir, h.r_back_mm, h.id)) {
+            BOOST_LOG_TRIVIAL(warning)
+                << "Pillar cannot be created for support point id: " << hid;
+            m_iheads_onmodel.emplace_back(h.id);
+            continue;
+        }
     }
-    
+
     // now we will go through the clusters ones again and connect the
     // sidepoints with the cluster centroid (which is a ground pillar)
     // or a nearby pillar if the centroid is unreachable.
     size_t ci = 0;
     for (auto cl : m_pillar_clusters) {
         m_thr();
-        
+
         auto cidx = cl_centroids[ci++];
-        
-        // TODO: don't consider the cluster centroid but calculate a
-        // central position where the pillar can be placed. this way
-        // the weight is distributed more effectively on the pillar.
-        
-        auto centerpillarID = m_builder.head_pillar(cidx).id;
-        
-        for (auto c : cl) {
-            m_thr();
-            if (c == cidx) continue;
-            
-            auto &sidehead = m_builder.head(c);
-            sidehead.transform();
-            
-            if (!connect_to_nearpillar(sidehead, centerpillarID) &&
-                !search_pillar_and_connect(sidehead)) {
-                Vec3d pstart = sidehead.junction_point();
-                // Vec3d pend = Vec3d{pstart(X), pstart(Y), gndlvl};
-                // Could not find a pillar, create one
-                create_ground_pillar(pstart, sidehead.dir, pradius, sidehead.id);
+
+        auto q = m_pillar_index.query(m_builder.head(cidx).junction_point(), 1);
+        if (!q.empty()) {
+            long centerpillarID = q.front().second;
+            for (auto c : cl) {
+                m_thr();
+                if (c == cidx) continue;
+
+                auto &sidehead = m_builder.head(c);
+
+                if (!connect_to_nearpillar(sidehead, centerpillarID) &&
+                    !search_pillar_and_connect(sidehead)) {
+                    Vec3d pstart = sidehead.junction_point();
+                    // Vec3d pend = Vec3d{pstart(X), pstart(Y), gndlvl};
+                    // Could not find a pillar, create one
+                    create_ground_pillar(pstart, sidehead.dir, sidehead.r_back_mm, sidehead.id);
+                }
             }
         }
     }
@@ -943,62 +891,66 @@ bool SupportTreeBuildsteps::connect_to_ground(Head &head, const Vec3d &dir)
     double r = head.r_back_mm;
     double t = bridge_mesh_distance(hjp, dir, head.r_back_mm);
     double d = 0, tdown = 0;
-    t = std::min(t, m_cfg.max_bridge_length_mm);
+    t = std::min(t, m_cfg.max_bridge_length_mm * r / m_cfg.head_back_radius_mm);
 
     while (d < t && !std::isinf(tdown = bridge_mesh_distance(hjp + d * dir, DOWN, r)))
         d += r;
-    
+
     if(!std::isinf(tdown)) return false;
-    
+
     Vec3d endp = hjp + d * dir;
-    m_builder.add_bridge(head.id, endp);
-    m_builder.add_junction(endp, head.r_back_mm);
-    
-    this->create_ground_pillar(endp, dir, head.r_back_mm);
-    
-    return true;
+    bool ret = false;
+
+    if ((ret = create_ground_pillar(endp, dir, head.r_back_mm))) {
+        m_builder.add_bridge(head.id, endp);
+        m_builder.add_junction(endp, head.r_back_mm);
+    }
+
+    return ret;
 }
 
 bool SupportTreeBuildsteps::connect_to_ground(Head &head)
 {
     if (connect_to_ground(head, head.dir)) return true;
-    
+
     // Optimize bridge direction:
     // Straight path failed so we will try to search for a suitable
     // direction out of the cavity.
     auto [polar, azimuth] = dir_to_spheric(head.dir);
-    
-    StopCriteria stc;
-    stc.max_iterations = m_cfg.optimizer_max_iterations;
-    stc.relative_score_difference = m_cfg.optimizer_rel_score_diff;
-    stc.stop_score = 1e6;
-    GeneticOptimizer solver(stc);
+
+    Optimizer<AlgNLoptGenetic> solver(get_criteria(m_cfg).stop_score(1e6));
     solver.seed(0); // we want deterministic behavior
-    
+
     double r_back = head.r_back_mm;
-    Vec3d hjp = head.junction_point();    
-    auto oresult = solver.optimize_max(
-        [this, hjp, r_back](double plr, double azm) {
+    Vec3d hjp = head.junction_point();
+    auto oresult = solver.to_max().optimize(
+        [this, hjp, r_back](const opt::Input<2> &input) {
+            auto &[plr, azm] = input;
             Vec3d n = spheric_to_dir(plr, azm).normalized();
             return bridge_mesh_distance(hjp, n, r_back);
         },
-        initvals(polar, azimuth),  // let's start with what we have
-        bound(3*PI/4, PI),  // Must not exceed the slope limit
-        bound(-PI, PI)      // azimuth can be a full range search
-        );
-    
+        initvals({polar, azimuth}),  // let's start with what we have
+        bounds({ {PI - m_cfg.bridge_slope, PI}, {-PI, PI} })
+    );
+
     Vec3d bridgedir = spheric_to_dir(oresult.optimum).normalized();
     return connect_to_ground(head, bridgedir);
 }
 
 bool SupportTreeBuildsteps::connect_to_model_body(Head &head)
 {
-    if (head.id <= ID_UNSET) return false;
-    
+    if (head.id <= SupportTreeNode::ID_UNSET) return false;
+
     auto it = m_head_to_ground_scans.find(unsigned(head.id));
     if (it == m_head_to_ground_scans.end()) return false;
-    
+
     auto &hit = it->second;
+
+    if (!hit.is_hit()) {
+        // TODO scan for potential anchor points on model surface
+        return false;
+    }
+
     Vec3d hjp = head.junction_point();
     double zangle = std::asin(hit.direction()(Z));
     zangle = std::max(zangle, PI/4);
@@ -1006,9 +958,11 @@ bool SupportTreeBuildsteps::connect_to_model_body(Head &head)
 
     // The width of the tail head that we would like to have...
     h = std::min(hit.distance() - head.r_back_mm, h);
-    
-    if(h <= 0.) return false;
-    
+
+    // If this is a mini pillar dont bother with the tail width, can be 0.
+    if (head.r_back_mm < m_cfg.head_back_radius_mm) h = std::max(h, 0.);
+    else if (h <= 0.) return false;
+
     Vec3d endp{hjp(X), hjp(Y), hjp(Z) - hit.distance() + h};
     auto center_hit = m_mesh.query_ray_hit(hjp, DOWN);
 
@@ -1016,13 +970,11 @@ bool SupportTreeBuildsteps::connect_to_model_body(Head &head)
     Vec3d hitp = std::abs(hitdiff) < 2*head.r_back_mm?
                      center_hit.position() : hit.position();
 
-    head.transform();
-
-    long pillar_id = m_builder.add_pillar(head.id, endp, head.r_back_mm);
+    long pillar_id = m_builder.add_pillar(head.id, hjp.z() - endp.z());
     Pillar &pill = m_builder.pillar(pillar_id);
 
     Vec3d taildir = endp - hitp;
-    double dist = distance(endp, hitp) + m_cfg.head_penetration_mm;
+    double dist = (hitp - endp).norm() + m_cfg.head_penetration_mm;
     double w = dist - 2 * head.r_pin_mm - head.r_back_mm;
 
     if (w < 0.) {
@@ -1030,19 +982,53 @@ bool SupportTreeBuildsteps::connect_to_model_body(Head &head)
         w = 0.;
     }
 
-    Head tailhead(head.r_back_mm, head.r_pin_mm, w,
-                  m_cfg.head_penetration_mm, taildir, hitp);
+    m_builder.add_anchor(head.r_back_mm, head.r_pin_mm, w,
+                         m_cfg.head_penetration_mm, taildir, hitp);
 
-    tailhead.transform();
-    pill.base = tailhead.mesh;
-    
     m_pillar_index.guarded_insert(pill.endpoint(), pill.id);
-    
+
     return true;
 }
 
+bool SupportTreeBuildsteps::search_pillar_and_connect(const Head &source)
+{
+    // Hope that a local copy takes less time than the whole search loop.
+    // We also need to remove elements progressively from the copied index.
+    PointIndex spindex = m_pillar_index.guarded_clone();
+
+    long nearest_id = SupportTreeNode::ID_UNSET;
+
+    Vec3d querypt = source.junction_point();
+
+    while(nearest_id < 0 && !spindex.empty()) { m_thr();
+        // loop until a suitable head is not found
+        // if there is a pillar closer than the cluster center
+        // (this may happen as the clustering is not perfect)
+        // than we will bridge to this closer pillar
+
+        Vec3d qp(querypt(X), querypt(Y), m_builder.ground_level);
+        auto qres = spindex.nearest(qp, 1);
+        if(qres.empty()) break;
+
+        auto ne = qres.front();
+        nearest_id = ne.second;
+
+        if(nearest_id >= 0) {
+            if (size_t(nearest_id) < m_builder.pillarcount()) {
+                if(!connect_to_nearpillar(source, nearest_id) ||
+                    m_builder.pillar(nearest_id).r < source.r_back_mm) {
+                    nearest_id = SupportTreeNode::ID_UNSET;    // continue searching
+                    spindex.remove(ne);       // without the current pillar
+                }
+            }
+        }
+    }
+
+    return nearest_id >= 0;
+}
+
 void SupportTreeBuildsteps::routing_to_model()
-{   
+{
     // We need to check if there is an easy way out to the bed surface.
     // If it can be routed there with a bridge shorter than
     // min_bridge_distance.
@@ -1050,23 +1036,23 @@ void SupportTreeBuildsteps::routing_to_model()
     ccr::enumerate(m_iheads_onmodel.begin(), m_iheads_onmodel.end(),
                    [this] (const unsigned idx, size_t) {
         m_thr();
-        
+
         auto& head = m_builder.head(idx);
-        
+
         // Search nearby pillar
-        if(search_pillar_and_connect(head)) { head.transform(); return; }
-        
+        if (search_pillar_and_connect(head)) { return; }
+
         // Cannot connect to nearby pillar. We will try to search for
         // a route to the ground.
-        if(connect_to_ground(head)) { head.transform(); return; }
-        
+        if (connect_to_ground(head)) { return; }
+
         // No route to the ground, so connect to the model body as a last resort
         if (connect_to_model_body(head)) { return; }
-        
+
         // We have failed to route this head.
         BOOST_LOG_TRIVIAL(warning)
-            << "Failed to route model facing support point. ID: " << idx;
-        
+                << "Failed to route model facing support point. ID: " << idx;
+
         head.invalidate();
     });
 }
@@ -1076,19 +1062,19 @@ void SupportTreeBuildsteps::interconnect_pillars()
     // Now comes the algorithm that connects pillars with each other.
     // Ideally every pillar should be connected with at least one of its
     // neighbors if that neighbor is within max_pillar_link_distance
-    
+
     // Pillars with height exceeding H1 will require at least one neighbor
     // to connect with. Height exceeding H2 require two neighbors.
     double H1 = m_cfg.max_solo_pillar_height_mm;
     double H2 = m_cfg.max_dual_pillar_height_mm;
     double d = m_cfg.max_pillar_link_distance_mm;
-    
+
     //A connection between two pillars only counts if the height ratio is
     // bigger than 50%
     double min_height_ratio = 0.5;
-    
+
     std::set<unsigned long> pairs;
-    
+
     // A function to connect one pillar with its neighbors. THe number of
     // neighbors is given in the configuration. This function if called
     // for every pillar in the pillar index. A pair of pillar will not
@@ -1098,66 +1084,68 @@ void SupportTreeBuildsteps::interconnect_pillars()
         [this, d, &pairs, min_height_ratio, H1] (const PointIndexEl& el)
     {
         Vec3d qp = el.first;    // endpoint of the pillar
-        
+
         const Pillar& pillar = m_builder.pillar(el.second); // actual pillar
-        
+
         // Get the max number of neighbors a pillar should connect to
         unsigned neighbors = m_cfg.pillar_cascade_neighbors;
-        
+
         // connections are already enough for the pillar
         if(pillar.links >= neighbors) return;
-        
+
+        double max_d = d * pillar.r / m_cfg.head_back_radius_mm;
         // Query all remaining points within reach
-        auto qres = m_pillar_index.query([qp, d](const PointIndexEl& e){
-            return distance(e.first, qp) < d;
+        auto qres = m_pillar_index.query([qp, max_d](const PointIndexEl& e){
+            return distance(e.first, qp) < max_d;
         });
-        
+
         // sort the result by distance (have to check if this is needed)
         std::sort(qres.begin(), qres.end(),
                   [qp](const PointIndexEl& e1, const PointIndexEl& e2){
                       return distance(e1.first, qp) < distance(e2.first, qp);
                   });
-        
+
         for(auto& re : qres) { // process the queried neighbors
-            
+
             if(re.second == el.second) continue; // Skip self
-            
+
             auto a = el.second, b = re.second;
-            
+
             // Get unique hash for the given pair (order doesn't matter)
             auto hashval = pairhash(a, b);
-            
+
             // Search for the pair amongst the remembered pairs
             if(pairs.find(hashval) != pairs.end()) continue;
-            
+
             const Pillar& neighborpillar = m_builder.pillar(re.second);
-            
+
             // this neighbor is occupied, skip
-            if(neighborpillar.links >= neighbors) continue;
-            
+            if (neighborpillar.links >= neighbors) continue;
+            if (neighborpillar.r < pillar.r) continue;
+
             if(interconnect(pillar, neighborpillar)) {
                 pairs.insert(hashval);
-                
+
                 // If the interconnection length between the two pillars is
                 // less than 50% of the longer pillar's height, don't count
                 if(pillar.height < H1 ||
                     neighborpillar.height / pillar.height > min_height_ratio)
                     m_builder.increment_links(pillar);
-                
+
                 if(neighborpillar.height < H1 ||
                     pillar.height / neighborpillar.height > min_height_ratio)
                     m_builder.increment_links(neighborpillar);
-                
+
             }
-            
+
             // connections are enough for one pillar
             if(pillar.links >= neighbors) break;
         }
     };
-    
+
     // Run the cascade for the pillars in the index
     m_pillar_index.foreach(cascadefn);
-    
+
     // We would be done here if we could allow some pillars to not be
     // connected with any neighbors. But this might leave the support tree
     // unprintable.
@@ -1165,16 +1153,16 @@ void SupportTreeBuildsteps::interconnect_pillars()
     // The current solution is to insert additional pillars next to these
     // lonely pillars. One or even two additional pillar might get inserted
     // depending on the length of the lonely pillar.
-    
+
     size_t pillarcount = m_builder.pillarcount();
-    
+
     // Again, go through all pillars, this time in the whole support tree
     // not just the index.
     for(size_t pid = 0; pid < pillarcount; pid++) {
         auto pillar = [this, pid]() { return m_builder.pillar(pid); };
-        
+
         // Decide how many additional pillars will be needed:
-        
+
         unsigned needpillars = 0;
         if (pillar().bridges > m_cfg.max_bridges_on_pillar)
             needpillars = 3;
@@ -1185,28 +1173,28 @@ void SupportTreeBuildsteps::interconnect_pillars()
             // No neighbors could be found and the pillar is too long.
             needpillars = 1;
         }
-        
+
         needpillars = std::max(pillar().links, needpillars) - pillar().links;
         if (needpillars == 0) continue;
-        
+
         // Search for new pillar locations:
-        
+
         bool   found    = false;
         double alpha    = 0; // goes to 2Pi
         double r        = 2 * m_cfg.base_radius_mm;
         Vec3d  pillarsp = pillar().startpoint();
-        
+
         // temp value for starting point detection
         Vec3d sp(pillarsp(X), pillarsp(Y), pillarsp(Z) - r);
-        
+
         // A vector of bool for placement feasbility
         std::vector<bool>  canplace(needpillars, false);
         std::vector<Vec3d> spts(needpillars); // vector of starting points
-        
+
         double gnd      = m_builder.ground_level;
         double min_dist = m_cfg.pillar_base_safety_distance_mm +
                           m_cfg.base_radius_mm + EPSILON;
-        
+
         while(!found && alpha < 2*PI) {
             for (unsigned n = 0;
                  n < needpillars && (!n || canplace[n - 1]);
@@ -1217,36 +1205,38 @@ void SupportTreeBuildsteps::interconnect_pillars()
                 s(X) += std::cos(a) * r;
                 s(Y) += std::sin(a) * r;
                 spts[n] = s;
-                
+
                 // Check the path vertically down
                 Vec3d check_from = s + Vec3d{0., 0., pillar().r};
                 auto hr = bridge_mesh_intersect(check_from, DOWN, pillar().r);
                 Vec3d gndsp{s(X), s(Y), gnd};
-                
+
                 // If the path is clear, check for pillar base collisions
                 canplace[n] = std::isinf(hr.distance()) &&
                               std::sqrt(m_mesh.squared_distance(gndsp)) >
                                   min_dist;
             }
-            
+
             found = std::all_of(canplace.begin(), canplace.end(),
                                 [](bool v) { return v; });
-            
+
             // 20 angles will be tried...
             alpha += 0.1 * PI;
         }
-        
+
         std::vector<long> newpills;
         newpills.reserve(needpillars);
 
         if (found)
             for (unsigned n = 0; n < needpillars; n++) {
-                Vec3d  s = spts[n];
-                Pillar p(s, Vec3d(s(X), s(Y), gnd), pillar().r);
-                p.add_base(m_cfg.base_height_mm, m_cfg.base_radius_mm);
+                Vec3d s = spts[n];
+                Pillar p(Vec3d{s.x(), s.y(), gnd}, s.z() - gnd, pillar().r);
 
                 if (interconnect(pillar(), p)) {
                     Pillar &pp = m_builder.pillar(m_builder.add_pillar(p));
+
+                    add_pillar_base(pp.id);
+
                     m_pillar_index.insert(pp.endpoint(), unsigned(pp.id));
 
                     m_builder.add_junction(s, pillar().r);
@@ -1255,9 +1245,8 @@ void SupportTreeBuildsteps::interconnect_pillars()
                     if (distance(pillarsp, s) < t)
                         m_builder.add_bridge(pillarsp, s, pillar().r);
 
-                    if (pillar().endpoint()(Z) > m_builder.ground_level)
-                        m_builder.add_junction(pillar().endpoint(),
-                                               pillar().r);
+                    if (pillar().endpoint()(Z) > m_builder.ground_level + pillar().r)
+                        m_builder.add_junction(pillar().endpoint(), pillar().r);
 
                     newpills.emplace_back(pp.id);
                     m_builder.increment_links(pillar());
@@ -1275,51 +1264,10 @@ void SupportTreeBuildsteps::interconnect_pillars()
                     m_builder.increment_links(nxpll);
                 }
             }
-            
+
             m_pillar_index.foreach(cascadefn);
         }
     }
 }
 
-void SupportTreeBuildsteps::routing_headless()
-{
-    // For now we will just generate smaller headless sticks with a sharp
-    // ending point that connects to the mesh surface.
-    
-    // We will sink the pins into the model surface for a distance of 1/3 of
-    // the pin radius
-    for(unsigned i : m_iheadless) {
-        m_thr();
-        
-        const auto R = double(m_support_pts[i].head_front_radius);
-        const double HWIDTH_MM = std::min(R, m_cfg.head_penetration_mm);
-        
-        // Exact support position
-        Vec3d sph = m_support_pts[i].pos.cast<double>();
-        Vec3d n = m_support_nmls.row(i);   // mesh outward normal
-        Vec3d sp = sph - n * HWIDTH_MM;     // stick head start point
-        
-        Vec3d sj = sp + R * n;              // stick start point
-        
-        // This is only for checking
-        double idist = bridge_mesh_distance(sph, DOWN, R, true);
-        double realdist = ray_mesh_intersect(sj, DOWN).distance();
-        double dist = realdist;
-        
-        if (std::isinf(dist)) dist = sph(Z) - m_builder.ground_level;
-        
-        if(std::isnan(idist) || idist < 2*R || std::isnan(dist) || dist < 2*R) {
-            BOOST_LOG_TRIVIAL(warning) << "Can not find route for headless"
-                                       << " support stick at: "
-                                       << sj.transpose();
-            continue;
-        }
-        
-        bool use_endball = !std::isinf(realdist);
-        Vec3d ej = sj + (dist + HWIDTH_MM) * DOWN ;
-        m_builder.add_compact_bridge(sp, ej, n, R, use_endball);
-    }
-}
-
-}
-}
+}} // namespace Slic3r::sla
diff --git a/src/libslic3r/SLA/SupportTreeBuildsteps.hpp b/src/libslic3r/SLA/SupportTreeBuildsteps.hpp
index cfe78fe97..013666f07 100644
--- a/src/libslic3r/SLA/SupportTreeBuildsteps.hpp
+++ b/src/libslic3r/SLA/SupportTreeBuildsteps.hpp
@@ -5,6 +5,7 @@
 
 #include <libslic3r/SLA/SupportTreeBuilder.hpp>
 #include <libslic3r/SLA/Clustering.hpp>
+#include <libslic3r/SLA/SpatIndex.hpp>
 
 namespace Slic3r {
 namespace sla {
@@ -16,9 +17,7 @@ enum { // For indexing Eigen vectors as v(X), v(Y), v(Z) instead of numbers
     X, Y, Z
 };
 
-inline Vec2d to_vec2(const Vec3d& v3) {
-    return {v3(X), v3(Y)};
-}
+inline Vec2d to_vec2(const Vec3d &v3) { return {v3(X), v3(Y)}; }
 
 inline std::pair<double, double> dir_to_spheric(const Vec3d &n, double norm = 1.)
 {
@@ -46,55 +45,71 @@ inline Vec3d spheric_to_dir(const std::pair<double, double> &v)
     return spheric_to_dir(v.first, v.second);
 }
 
-// This function returns the position of the centroid in the input 'clust'
-// vector of point indices.
-template<class DistFn>
-long cluster_centroid(const ClusterEl& clust,
-                      const std::function<Vec3d(size_t)> &pointfn,
-                      DistFn df)
+inline Vec3d spheric_to_dir(const std::array<double, 2> &v)
 {
-    switch(clust.size()) {
-    case 0: /* empty cluster */ return ID_UNSET;
-    case 1: /* only one element */ return 0;
-    case 2: /* if two elements, there is no center */ return 0;
-    default: ;
+    return spheric_to_dir(v[0], v[1]);
+}
+
+// Give points on a 3D ring with given center, radius and orientation
+// method based on:
+// https://math.stackexchange.com/questions/73237/parametric-equation-of-a-circle-in-3d-space
+template<size_t N>
+class PointRing {
+    std::array<double, N> m_phis;
+
+    // Two vectors that will be perpendicular to each other and to the
+    // axis. Values for a(X) and a(Y) are now arbitrary, a(Z) is just a
+    // placeholder.
+    // a and b vectors are perpendicular to the ring direction and to each other.
+    // Together they define the plane where we have to iterate with the
+    // given angles in the 'm_phis' vector
+    Vec3d a = {0, 1, 0}, b;
+    double m_radius = 0.;
+
+    static inline bool constexpr is_one(double val)
+    {
+        return std::abs(std::abs(val) - 1) < 1e-20;
     }
 
-    // The function works by calculating for each point the average distance
-    // from all the other points in the cluster. We create a selector bitmask of
-    // the same size as the cluster. The bitmask will have two true bits and
-    // false bits for the rest of items and we will loop through all the
-    // permutations of the bitmask (combinations of two points). Get the
-    // distance for the two points and add the distance to the averages.
-    // The point with the smallest average than wins.
+public:
 
-    // The complexity should be O(n^2) but we will mostly apply this function
-    // for small clusters only (cca 3 elements)
+    PointRing(const Vec3d &n)
+    {
+        m_phis = linspace_array<N>(0., 2 * PI);
 
-    std::vector<bool> sel(clust.size(), false);   // create full zero bitmask
-    std::fill(sel.end() - 2, sel.end(), true);    // insert the two ones
-    std::vector<double> avgs(clust.size(), 0.0);  // store the average distances
+        // We have to address the case when the direction vector v (same as
+        // dir) is coincident with one of the world axes. In this case two of
+        // its components will be completely zero and one is 1.0. Our method
+        // becomes dangerous here due to division with zero. Instead, vector
+        // 'a' can be an element-wise rotated version of 'v'
+        if(is_one(n(X)) || is_one(n(Y)) || is_one(n(Z))) {
+            a = {n(Z), n(X), n(Y)};
+            b = {n(Y), n(Z), n(X)};
+        }
+        else {
+            a(Z) = -(n(Y)*a(Y)) / n(Z); a.normalize();
+            b = a.cross(n);
+        }
+    }
 
-    do {
-        std::array<size_t, 2> idx;
-        for(size_t i = 0, j = 0; i < clust.size(); i++) if(sel[i]) idx[j++] = i;
+    Vec3d get(size_t idx, const Vec3d src, double r) const
+    {
+        double phi = m_phis[idx];
+        double sinphi = std::sin(phi);
+        double cosphi = std::cos(phi);
 
-        double d = df(pointfn(clust[idx[0]]),
-                      pointfn(clust[idx[1]]));
+        double rpscos = r * cosphi;
+        double rpssin = r * sinphi;
 
-        // add the distance to the sums for both associated points
-        for(auto i : idx) avgs[i] += d;
+        // Point on the sphere
+        return {src(X) + rpscos * a(X) + rpssin * b(X),
+                src(Y) + rpscos * a(Y) + rpssin * b(Y),
+                src(Z) + rpscos * a(Z) + rpssin * b(Z)};
+    }
+};
 
-        // now continue with the next permutation of the bitmask with two 1s
-    } while(std::next_permutation(sel.begin(), sel.end()));
-
-    // Divide by point size in the cluster to get the average (may be redundant)
-    for(auto& a : avgs) a /= clust.size();
-
-    // get the lowest average distance and return the index
-    auto minit = std::min_element(avgs.begin(), avgs.end());
-    return long(minit - avgs.begin());
-}
+//IndexedMesh::hit_result query_hit(const SupportableMesh &msh, const Bridge &br, double safety_d = std::nan(""));
+//IndexedMesh::hit_result query_hit(const SupportableMesh &msh, const Head &br, double safety_d = std::nan(""));
 
 inline Vec3d dirv(const Vec3d& startp, const Vec3d& endp) {
     return (endp - startp).normalized();
@@ -170,8 +185,8 @@ IntegerOnly<DoubleI> pairhash(I a, I b)
 }
 
 class SupportTreeBuildsteps {
-    const SupportConfig& m_cfg;
-    const EigenMesh3D& m_mesh;
+    const SupportTreeConfig& m_cfg;
+    const IndexedMesh& m_mesh;
     const std::vector<SupportPoint>& m_support_pts;
 
     using PtIndices = std::vector<unsigned>;
@@ -180,7 +195,7 @@ class SupportTreeBuildsteps {
     PtIndices m_iheads_onmodel;
     PtIndices m_iheadless;         // headless support points
     
-    std::map<unsigned, EigenMesh3D::hit_result> m_head_to_ground_scans;
+    std::map<unsigned, IndexedMesh::hit_result> m_head_to_ground_scans;
 
     // normals for support points from model faces.
     PointSet  m_support_nmls;
@@ -206,7 +221,7 @@ class SupportTreeBuildsteps {
     // When bridging heads to pillars... TODO: find a cleaner solution
     ccr::BlockingMutex m_bridge_mutex;
 
-    inline EigenMesh3D::hit_result ray_mesh_intersect(const Vec3d& s, 
+    inline IndexedMesh::hit_result ray_mesh_intersect(const Vec3d& s, 
                                                       const Vec3d& dir)
     {
         return m_mesh.query_ray_hit(s, dir);
@@ -223,16 +238,24 @@ class SupportTreeBuildsteps {
     // point was inside the model, an "invalid" hit_result will be returned
     // with a zero distance value instead of a NAN. This way the result can
     // be used safely for comparison with other distances.
-    EigenMesh3D::hit_result pinhead_mesh_intersect(
+    IndexedMesh::hit_result pinhead_mesh_intersect(
         const Vec3d& s,
         const Vec3d& dir,
         double r_pin,
         double r_back,
-        double width);
-    
-    template<class...Args>
-    inline double pinhead_mesh_distance(Args&&...args) {
-        return pinhead_mesh_intersect(std::forward<Args>(args)...).distance();
+        double width,
+        double safety_d);
+
+    IndexedMesh::hit_result pinhead_mesh_intersect(
+        const Vec3d& s,
+        const Vec3d& dir,
+        double r_pin,
+        double r_back,
+        double width)
+    {
+        return pinhead_mesh_intersect(s, dir, r_pin, r_back, width,
+                                      r_back * m_cfg.safety_distance_mm /
+                                          m_cfg.head_back_radius_mm);
     }
 
     // Checking bridge (pillar and stick as well) intersection with the model.
@@ -243,11 +266,21 @@ class SupportTreeBuildsteps {
     // point was inside the model, an "invalid" hit_result will be returned
     // with a zero distance value instead of a NAN. This way the result can
     // be used safely for comparison with other distances.
-    EigenMesh3D::hit_result bridge_mesh_intersect(
+    IndexedMesh::hit_result bridge_mesh_intersect(
         const Vec3d& s,
         const Vec3d& dir,
         double r,
-        bool ins_check = false);
+        double safety_d);
+
+    IndexedMesh::hit_result bridge_mesh_intersect(
+        const Vec3d& s,
+        const Vec3d& dir,
+        double r)
+    {
+        return bridge_mesh_intersect(s, dir, r,
+                                     r * m_cfg.safety_distance_mm /
+                                         m_cfg.head_back_radius_mm);
+    }
     
     template<class...Args>
     inline double bridge_mesh_distance(Args&&...args) {
@@ -268,20 +301,29 @@ class SupportTreeBuildsteps {
     inline bool connect_to_ground(Head& head);
     
     bool connect_to_model_body(Head &head);
-    
-    bool search_pillar_and_connect(const Head& head);
+
+    bool search_pillar_and_connect(const Head& source);
     
     // This is a proxy function for pillar creation which will mind the gap
     // between the pad and the model bottom in zero elevation mode.
     // jp is the starting junction point which needs to be routed down.
     // sourcedir is the allowed direction of an optional bridge between the
     // jp junction and the final pillar.
-    void create_ground_pillar(const Vec3d &jp,
+    bool create_ground_pillar(const Vec3d &jp,
                               const Vec3d &sourcedir,
                               double       radius,
-                              long         head_id = ID_UNSET);
-    
-    
+                              long         head_id = SupportTreeNode::ID_UNSET);
+
+    void add_pillar_base(long pid)
+    {
+        m_builder.add_pillar_base(pid, m_cfg.base_height_mm, m_cfg.base_radius_mm);
+    }
+
+    std::optional<DiffBridge> search_widening_path(const Vec3d &jp,
+                                                   const Vec3d &dir,
+                                                   double       radius,
+                                                   double       new_radius);
+
 public:
     SupportTreeBuildsteps(SupportTreeBuilder & builder, const SupportableMesh &sm);
 
@@ -324,11 +366,6 @@ public:
 
     void interconnect_pillars();
 
-    // Step: process the support points where there is not enough space for a
-    // full pinhead. In this case we will use a rounded sphere as a touching
-    // point and use a thinner bridge (let's call it a stick).
-    void routing_headless ();
-
     inline void merge_result() { m_builder.merged_mesh(); }
 
     static bool execute(SupportTreeBuilder & builder, const SupportableMesh &sm);
diff --git a/src/libslic3r/SLA/SupportTreeMesher.cpp b/src/libslic3r/SLA/SupportTreeMesher.cpp
new file mode 100644
index 000000000..15491775b
--- /dev/null
+++ b/src/libslic3r/SLA/SupportTreeMesher.cpp
@@ -0,0 +1,266 @@
+#include "SupportTreeMesher.hpp"
+
+namespace Slic3r { namespace sla {
+
+Contour3D sphere(double rho, Portion portion, double fa) {
+
+    Contour3D ret;
+
+    // prohibit close to zero radius
+    if(rho <= 1e-6 && rho >= -1e-6) return ret;
+
+    auto& vertices = ret.points;
+    auto& facets = ret.faces3;
+
+    // Algorithm:
+    // Add points one-by-one to the sphere grid and form facets using relative
+    // coordinates. Sphere is composed effectively of a mesh of stacked circles.
+
+    // adjust via rounding to get an even multiple for any provided angle.
+    double angle = (2*PI / floor(2*PI / fa));
+
+    // Ring to be scaled to generate the steps of the sphere
+    std::vector<double> ring;
+
+    for (double i = 0; i < 2*PI; i+=angle) ring.emplace_back(i);
+
+    const auto sbegin = size_t(2*std::get<0>(portion)/angle);
+    const auto send = size_t(2*std::get<1>(portion)/angle);
+
+    const size_t steps = ring.size();
+    const double increment = 1.0 / double(steps);
+
+    // special case: first ring connects to 0,0,0
+    // insert and form facets.
+    if(sbegin == 0)
+        vertices.emplace_back(Vec3d(0.0, 0.0, -rho + increment*sbegin*2.0*rho));
+
+    auto id = coord_t(vertices.size());
+    for (size_t i = 0; i < ring.size(); i++) {
+        // Fixed scaling
+        const double z = -rho + increment*rho*2.0 * (sbegin + 1.0);
+        // radius of the circle for this step.
+        const double r = std::sqrt(std::abs(rho*rho - z*z));
+        Vec2d b = Eigen::Rotation2Dd(ring[i]) * Eigen::Vector2d(0, r);
+        vertices.emplace_back(Vec3d(b(0), b(1), z));
+
+        if (sbegin == 0)
+            (i == 0) ? facets.emplace_back(coord_t(ring.size()), 0, 1) :
+                       facets.emplace_back(id - 1, 0, id);
+        ++id;
+    }
+
+    // General case: insert and form facets for each step,
+    // joining it to the ring below it.
+    for (size_t s = sbegin + 2; s < send - 1; s++) {
+        const double z = -rho + increment*double(s*2.0*rho);
+        const double r = std::sqrt(std::abs(rho*rho - z*z));
+
+        for (size_t i = 0; i < ring.size(); i++) {
+            Vec2d b = Eigen::Rotation2Dd(ring[i]) * Eigen::Vector2d(0, r);
+            vertices.emplace_back(Vec3d(b(0), b(1), z));
+            auto id_ringsize = coord_t(id - int(ring.size()));
+            if (i == 0) {
+                // wrap around
+                facets.emplace_back(id - 1, id, id + coord_t(ring.size() - 1) );
+                facets.emplace_back(id - 1, id_ringsize, id);
+            } else {
+                facets.emplace_back(id_ringsize - 1, id_ringsize, id);
+                facets.emplace_back(id - 1, id_ringsize - 1, id);
+            }
+            id++;
+        }
+    }
+
+    // special case: last ring connects to 0,0,rho*2.0
+    // only form facets.
+    if(send >= size_t(2*PI / angle)) {
+        vertices.emplace_back(Vec3d(0.0, 0.0, -rho + increment*send*2.0*rho));
+        for (size_t i = 0; i < ring.size(); i++) {
+            auto id_ringsize = coord_t(id - int(ring.size()));
+            if (i == 0) {
+                // third vertex is on the other side of the ring.
+                facets.emplace_back(id - 1, id_ringsize, id);
+            } else {
+                auto ci = coord_t(id_ringsize + coord_t(i));
+                facets.emplace_back(ci - 1, ci, id);
+            }
+        }
+    }
+    id++;
+
+    return ret;
+}
+
+Contour3D cylinder(double r, double h, size_t ssteps, const Vec3d &sp)
+{
+    assert(ssteps > 0);
+
+    Contour3D ret;
+
+    auto steps = int(ssteps);
+    auto& points = ret.points;
+    auto& indices = ret.faces3;
+    points.reserve(2*ssteps);
+    double a = 2*PI/steps;
+
+    Vec3d jp = sp;
+    Vec3d endp = {sp(X), sp(Y), sp(Z) + h};
+
+    // Upper circle points
+    for(int i = 0; i < steps; ++i) {
+        double phi = i*a;
+        double ex = endp(X) + r*std::cos(phi);
+        double ey = endp(Y) + r*std::sin(phi);
+        points.emplace_back(ex, ey, endp(Z));
+    }
+
+    // Lower circle points
+    for(int i = 0; i < steps; ++i) {
+        double phi = i*a;
+        double x = jp(X) + r*std::cos(phi);
+        double y = jp(Y) + r*std::sin(phi);
+        points.emplace_back(x, y, jp(Z));
+    }
+
+    // Now create long triangles connecting upper and lower circles
+    indices.reserve(2*ssteps);
+    auto offs = steps;
+    for(int i = 0; i < steps - 1; ++i) {
+        indices.emplace_back(i, i + offs, offs + i + 1);
+        indices.emplace_back(i, offs + i + 1, i + 1);
+    }
+
+    // Last triangle connecting the first and last vertices
+    auto last = steps - 1;
+    indices.emplace_back(0, last, offs);
+    indices.emplace_back(last, offs + last, offs);
+
+    // According to the slicing algorithms, we need to aid them with generating
+    // a watertight body. So we create a triangle fan for the upper and lower
+    // ending of the cylinder to close the geometry.
+    points.emplace_back(jp); int ci = int(points.size() - 1);
+    for(int i = 0; i < steps - 1; ++i)
+        indices.emplace_back(i + offs + 1, i + offs, ci);
+
+    indices.emplace_back(offs, steps + offs - 1, ci);
+
+    points.emplace_back(endp); ci = int(points.size() - 1);
+    for(int i = 0; i < steps - 1; ++i)
+        indices.emplace_back(ci, i, i + 1);
+
+    indices.emplace_back(steps - 1, 0, ci);
+
+    return ret;
+}
+
+Contour3D pinhead(double r_pin, double r_back, double length, size_t steps)
+{
+    assert(steps > 0);
+    assert(length >= 0.);
+    assert(r_back > 0.);
+    assert(r_pin > 0.);
+
+    Contour3D mesh;
+
+    // We create two spheres which will be connected with a robe that fits
+    // both circles perfectly.
+
+    // Set up the model detail level
+    const double detail = 2 * PI / steps;
+
+    // We don't generate whole circles. Instead, we generate only the
+    // portions which are visible (not covered by the robe) To know the
+    // exact portion of the bottom and top circles we need to use some
+    // rules of tangent circles from which we can derive (using simple
+    // triangles the following relations:
+
+    // The height of the whole mesh
+    const double h   = r_back + r_pin + length;
+    double       phi = PI / 2. - std::acos((r_back - r_pin) / h);
+
+    // To generate a whole circle we would pass a portion of (0, Pi)
+    // To generate only a half horizontal circle we can pass (0, Pi/2)
+    // The calculated phi is an offset to the half circles needed to smooth
+    // the transition from the circle to the robe geometry
+
+    auto &&s1 = sphere(r_back, make_portion(0, PI / 2 + phi), detail);
+    auto &&s2 = sphere(r_pin, make_portion(PI / 2 + phi, PI), detail);
+
+    for (auto &p : s2.points) p.z() += h;
+
+    mesh.merge(s1);
+    mesh.merge(s2);
+
+    for (size_t idx1 = s1.points.size() - steps, idx2 = s1.points.size();
+         idx1 < s1.points.size() - 1; idx1++, idx2++) {
+        coord_t i1s1 = coord_t(idx1), i1s2 = coord_t(idx2);
+        coord_t i2s1 = i1s1 + 1, i2s2 = i1s2 + 1;
+
+        mesh.faces3.emplace_back(i1s1, i2s1, i2s2);
+        mesh.faces3.emplace_back(i1s1, i2s2, i1s2);
+    }
+
+    auto i1s1 = coord_t(s1.points.size()) - coord_t(steps);
+    auto i2s1 = coord_t(s1.points.size()) - 1;
+    auto i1s2 = coord_t(s1.points.size());
+    auto i2s2 = coord_t(s1.points.size()) + coord_t(steps) - 1;
+
+    mesh.faces3.emplace_back(i2s2, i2s1, i1s1);
+    mesh.faces3.emplace_back(i1s2, i2s2, i1s1);
+
+    return mesh;
+}
+
+Contour3D halfcone(double       baseheight,
+                   double       r_bottom,
+                   double       r_top,
+                   const Vec3d &pos,
+                   size_t       steps)
+{
+    assert(steps > 0);
+
+    if (baseheight <= 0 || steps <= 0) return {};
+
+    Contour3D base;
+
+    double a    = 2 * PI / steps;
+    auto   last = int(steps - 1);
+    Vec3d  ep{pos.x(), pos.y(), pos.z() + baseheight};
+    for (size_t i = 0; i < steps; ++i) {
+        double phi = i * a;
+        double x   = pos.x() + r_top * std::cos(phi);
+        double y   = pos.y() + r_top * std::sin(phi);
+        base.points.emplace_back(x, y, ep.z());
+    }
+
+    for (size_t i = 0; i < steps; ++i) {
+        double phi = i * a;
+        double x   = pos.x() + r_bottom * std::cos(phi);
+        double y   = pos.y() + r_bottom * std::sin(phi);
+        base.points.emplace_back(x, y, pos.z());
+    }
+
+    base.points.emplace_back(pos);
+    base.points.emplace_back(ep);
+
+    auto &indices = base.faces3;
+    auto  hcenter = int(base.points.size() - 1);
+    auto  lcenter = int(base.points.size() - 2);
+    auto  offs    = int(steps);
+    for (int i = 0; i < last; ++i) {
+        indices.emplace_back(i, i + offs, offs + i + 1);
+        indices.emplace_back(i, offs + i + 1, i + 1);
+        indices.emplace_back(i, i + 1, hcenter);
+        indices.emplace_back(lcenter, offs + i + 1, offs + i);
+    }
+
+    indices.emplace_back(0, last, offs);
+    indices.emplace_back(last, offs + last, offs);
+    indices.emplace_back(hcenter, last, 0);
+    indices.emplace_back(offs, offs + last, lcenter);
+
+    return base;
+}
+
+}} // namespace Slic3r::sla
diff --git a/src/libslic3r/SLA/SupportTreeMesher.hpp b/src/libslic3r/SLA/SupportTreeMesher.hpp
new file mode 100644
index 000000000..63182745d
--- /dev/null
+++ b/src/libslic3r/SLA/SupportTreeMesher.hpp
@@ -0,0 +1,117 @@
+#ifndef SUPPORTTREEMESHER_HPP
+#define SUPPORTTREEMESHER_HPP
+
+#include "libslic3r/Point.hpp"
+
+#include "libslic3r/SLA/SupportTreeBuilder.hpp"
+#include "libslic3r/SLA/Contour3D.hpp"
+
+namespace Slic3r { namespace sla {
+
+using Portion = std::tuple<double, double>;
+
+inline Portion make_portion(double a, double b)
+{
+    return std::make_tuple(a, b);
+}
+
+Contour3D sphere(double  rho,
+                 Portion portion = make_portion(0., 2. * PI),
+                 double  fa      = (2. * PI / 360.));
+
+// Down facing cylinder in Z direction with arguments:
+// r: radius
+// h: Height
+// ssteps: how many edges will create the base circle
+// sp: starting point
+Contour3D cylinder(double       r,
+                   double       h,
+                   size_t       steps = 45,
+                   const Vec3d &sp    = Vec3d::Zero());
+
+Contour3D pinhead(double r_pin, double r_back, double length, size_t steps = 45);
+
+Contour3D halfcone(double       baseheight,
+                   double       r_bottom,
+                   double       r_top,
+                   const Vec3d &pt    = Vec3d::Zero(),
+                   size_t       steps = 45);
+
+inline Contour3D get_mesh(const Head &h, size_t steps)
+{
+    Contour3D mesh = pinhead(h.r_pin_mm, h.r_back_mm, h.width_mm, steps);
+
+    for(auto& p : mesh.points) p.z() -= (h.fullwidth() - h.r_back_mm);
+
+    using Quaternion = Eigen::Quaternion<double>;
+
+    // We rotate the head to the specified direction. The head's pointing
+    // side is facing upwards so this means that it would hold a support
+    // point with a normal pointing straight down. This is the reason of
+    // the -1 z coordinate
+    auto quatern = Quaternion::FromTwoVectors(Vec3d{0, 0, -1}, h.dir);
+
+    for(auto& p : mesh.points) p = quatern * p + h.pos;
+
+    return mesh;
+}
+
+inline Contour3D get_mesh(const Pillar &p, size_t steps)
+{
+    if(p.height > EPSILON) { // Endpoint is below the starting point
+        // We just create a bridge geometry with the pillar parameters and
+        // move the data.
+        return cylinder(p.r, p.height, steps, p.endpoint());
+    }
+
+    return {};
+}
+
+inline Contour3D get_mesh(const Pedestal &p, size_t steps)
+{
+    return halfcone(p.height, p.r_bottom, p.r_top, p.pos, steps);
+}
+
+inline Contour3D get_mesh(const Junction &j, size_t steps)
+{
+    Contour3D mesh = sphere(j.r, make_portion(0, PI), 2 *PI / steps);
+    for(auto& p : mesh.points) p += j.pos;
+    return mesh;
+}
+
+inline Contour3D get_mesh(const Bridge &br, size_t steps)
+{
+    using Quaternion = Eigen::Quaternion<double>;
+    Vec3d v = (br.endp - br.startp);
+    Vec3d dir = v.normalized();
+    double d = v.norm();
+
+    Contour3D mesh = cylinder(br.r, d, steps);
+
+    auto quater = Quaternion::FromTwoVectors(Vec3d{0,0,1}, dir);
+    for(auto& p : mesh.points) p = quater * p + br.startp;
+
+    return mesh;
+}
+
+inline Contour3D get_mesh(const DiffBridge &br, size_t steps)
+{
+    double h = br.get_length();
+    Contour3D mesh = halfcone(h, br.r, br.end_r, Vec3d::Zero(), steps);
+
+    using Quaternion = Eigen::Quaternion<double>;
+
+    // We rotate the head to the specified direction. The head's pointing
+    // side is facing upwards so this means that it would hold a support
+    // point with a normal pointing straight down. This is the reason of
+    // the -1 z coordinate
+    auto quatern = Quaternion::FromTwoVectors(Vec3d{0, 0, 1}, br.get_dir());
+
+    for(auto& p : mesh.points) p = quatern * p + br.startp;
+
+    return mesh;
+}
+
+}} // namespace Slic3r::sla
+
+#endif // SUPPORTTREEMESHER_HPP
diff --git a/src/libslic3r/SLAPrint.cpp b/src/libslic3r/SLAPrint.cpp
index 2402207a8..4395bea46 100644
--- a/src/libslic3r/SLAPrint.cpp
+++ b/src/libslic3r/SLAPrint.cpp
@@ -35,13 +35,16 @@ bool is_zero_elevation(const SLAPrintObjectConfig &c)
 }
 
 // Compile the argument for support creation from the static print config.
-sla::SupportConfig make_support_cfg(const SLAPrintObjectConfig& c)
+sla::SupportTreeConfig make_support_cfg(const SLAPrintObjectConfig& c)
 {
-    sla::SupportConfig scfg;
+    sla::SupportTreeConfig scfg;
     
     scfg.enabled = c.supports_enable.getBool();
     scfg.head_front_radius_mm = 0.5*c.support_head_front_diameter.getFloat();
-    scfg.head_back_radius_mm = 0.5*c.support_pillar_diameter.getFloat();
+    double pillar_r = 0.5 * c.support_pillar_diameter.getFloat();
+    scfg.head_back_radius_mm = pillar_r;
+    scfg.head_fallback_radius_mm =
+        0.01 * c.support_small_pillar_diameter_percent.getFloat() * pillar_r;
     scfg.head_penetration_mm = c.support_head_penetration.getFloat();
     scfg.head_width_mm = c.support_head_width.getFloat();
     scfg.object_elevation_mm = is_zero_elevation(c) ?
@@ -616,7 +619,7 @@ std::string SLAPrint::validate() const
             return L("Cannot proceed without support points! "
                      "Add support points or disable support generation.");
 
-        sla::SupportConfig cfg = make_support_cfg(po->config());
+        sla::SupportTreeConfig cfg = make_support_cfg(po->config());
 
         double elv = cfg.object_elevation_mm;
         
@@ -925,6 +928,7 @@ bool SLAPrintObject::invalidate_state_by_config_options(const std::vector<t_conf
             || opt_key == "support_head_penetration"
             || opt_key == "support_head_width"
             || opt_key == "support_pillar_diameter"
+            || opt_key == "support_small_pillar_diameter_percent"
             || opt_key == "support_max_bridges_on_pillar"
             || opt_key == "support_pillar_connection_mode"
             || opt_key == "support_buildplate_only"
diff --git a/src/libslic3r/SLAPrint.hpp b/src/libslic3r/SLAPrint.hpp
index 9d41586ee..f4b220c58 100644
--- a/src/libslic3r/SLAPrint.hpp
+++ b/src/libslic3r/SLAPrint.hpp
@@ -544,7 +544,7 @@ private:
 
 bool is_zero_elevation(const SLAPrintObjectConfig &c);
 
-sla::SupportConfig make_support_cfg(const SLAPrintObjectConfig& c);
+sla::SupportTreeConfig make_support_cfg(const SLAPrintObjectConfig& c);
 
 sla::PadConfig::EmbedObject builtin_pad_cfg(const SLAPrintObjectConfig& c);
 
diff --git a/src/libslic3r/SLAPrintSteps.cpp b/src/libslic3r/SLAPrintSteps.cpp
index e421e9c1d..76bbf498d 100644
--- a/src/libslic3r/SLAPrintSteps.cpp
+++ b/src/libslic3r/SLAPrintSteps.cpp
@@ -360,18 +360,6 @@ void SLAPrint::Steps::support_points(SLAPrintObject &po)
         // removed them on purpose. No calculation will be done.
         po.m_supportdata->pts = po.transformed_support_points();
     }
-
-    // If the zero elevation mode is engaged, we have to filter out all the
-    // points that are on the bottom of the object
-    if (is_zero_elevation(po.config())) {
-        double tolerance = po.config().pad_enable.getBool() ?
-                               po.m_config.pad_wall_thickness.getFloat() :
-                               po.m_config.support_base_height.getFloat();
-
-        remove_bottom_points(po.m_supportdata->pts,
-                             po.m_supportdata->emesh.ground_level(),
-                             tolerance);
-    }
 }
 
 void SLAPrint::Steps::support_tree(SLAPrintObject &po)
@@ -382,6 +370,13 @@ void SLAPrint::Steps::support_tree(SLAPrintObject &po)
     
     if (pcfg.embed_object)
         po.m_supportdata->emesh.ground_level_offset(pcfg.wall_thickness_mm);
+
+    // If the zero elevation mode is engaged, we have to filter out all the
+    // points that are on the bottom of the object
+    if (is_zero_elevation(po.config())) {
+        remove_bottom_points(po.m_supportdata->pts,
+                             float(po.m_supportdata->emesh.ground_level() + EPSILON));
+    }
     
     po.m_supportdata->cfg = make_support_cfg(po.m_config);
 //    po.m_supportdata->emesh.load_holes(po.transformed_drainhole_points());
diff --git a/src/slic3r/GUI/ConfigManipulation.cpp b/src/slic3r/GUI/ConfigManipulation.cpp
index a0df4c659..3e301566b 100644
--- a/src/slic3r/GUI/ConfigManipulation.cpp
+++ b/src/slic3r/GUI/ConfigManipulation.cpp
@@ -353,6 +353,7 @@ void ConfigManipulation::toggle_print_sla_options(DynamicPrintConfig* config)
     toggle_field("support_head_penetration", supports_en);
     toggle_field("support_head_width", supports_en);
     toggle_field("support_pillar_diameter", supports_en);
+    toggle_field("support_small_pillar_diameter_percent", supports_en);
     toggle_field("support_max_bridges_on_pillar", supports_en);
     toggle_field("support_pillar_connection_mode", supports_en);
     toggle_field("support_buildplate_only", supports_en);
diff --git a/src/slic3r/GUI/MeshUtils.cpp b/src/slic3r/GUI/MeshUtils.cpp
index 581f50a88..ee0abe76f 100644
--- a/src/slic3r/GUI/MeshUtils.cpp
+++ b/src/slic3r/GUI/MeshUtils.cpp
@@ -134,7 +134,7 @@ bool MeshRaycaster::unproject_on_mesh(const Vec2d& mouse_pos, const Transform3d&
     Vec3d direction;
     line_from_mouse_pos(mouse_pos, trafo, camera, point, direction);
 
-    std::vector<sla::EigenMesh3D::hit_result> hits = m_emesh.query_ray_hits(point, direction);
+    std::vector<sla::IndexedMesh::hit_result> hits = m_emesh.query_ray_hits(point, direction);
 
     if (hits.empty())
         return false; // no intersection found
@@ -184,7 +184,7 @@ std::vector<unsigned> MeshRaycaster::get_unobscured_idxs(const Geometry::Transfo
 
         bool is_obscured = false;
         // Cast a ray in the direction of the camera and look for intersection with the mesh:
-        std::vector<sla::EigenMesh3D::hit_result> hits;
+        std::vector<sla::IndexedMesh::hit_result> hits;
         // Offset the start of the ray by EPSILON to account for numerical inaccuracies.
         hits = m_emesh.query_ray_hits((inverse_trafo * pt + direction_to_camera_mesh * EPSILON).cast<double>(),
                                       direction_to_camera.cast<double>());
diff --git a/src/slic3r/GUI/MeshUtils.hpp b/src/slic3r/GUI/MeshUtils.hpp
index 2758577a2..60dcb30c8 100644
--- a/src/slic3r/GUI/MeshUtils.hpp
+++ b/src/slic3r/GUI/MeshUtils.hpp
@@ -3,7 +3,7 @@
 
 #include "libslic3r/Point.hpp"
 #include "libslic3r/Geometry.hpp"
-#include "libslic3r/SLA/EigenMesh3D.hpp"
+#include "libslic3r/SLA/IndexedMesh.hpp"
 #include "admesh/stl.h"
 
 #include "slic3r/GUI/3DScene.hpp"
@@ -147,7 +147,7 @@ public:
     Vec3f get_triangle_normal(size_t facet_idx) const;
 
 private:
-    sla::EigenMesh3D m_emesh;
+    sla::IndexedMesh m_emesh;
     std::vector<stl_normal> m_normals;
 };
 
diff --git a/src/slic3r/GUI/Preset.cpp b/src/slic3r/GUI/Preset.cpp
index d810c399d..7cf3b13ac 100644
--- a/src/slic3r/GUI/Preset.cpp
+++ b/src/slic3r/GUI/Preset.cpp
@@ -496,6 +496,7 @@ const std::vector<std::string>& Preset::sla_print_options()
             "support_head_penetration",
             "support_head_width",
             "support_pillar_diameter",
+            "support_small_pillar_diameter_percent",
             "support_max_bridges_on_pillar",
             "support_pillar_connection_mode",
             "support_buildplate_only",
diff --git a/src/slic3r/GUI/Tab.cpp b/src/slic3r/GUI/Tab.cpp
index 84bc5a572..86b483a8d 100644
--- a/src/slic3r/GUI/Tab.cpp
+++ b/src/slic3r/GUI/Tab.cpp
@@ -3919,6 +3919,7 @@ void TabSLAPrint::build()
 
     optgroup = page->new_optgroup(L("Support pillar"));
     optgroup->append_single_option_line("support_pillar_diameter");
+    optgroup->append_single_option_line("support_small_pillar_diameter_percent");
     optgroup->append_single_option_line("support_max_bridges_on_pillar");
     
     optgroup->append_single_option_line("support_pillar_connection_mode");
diff --git a/tests/sla_print/CMakeLists.txt b/tests/sla_print/CMakeLists.txt
index 9d47f3ae4..f6b261fda 100644
--- a/tests/sla_print/CMakeLists.txt
+++ b/tests/sla_print/CMakeLists.txt
@@ -1,7 +1,7 @@
 get_filename_component(_TEST_NAME ${CMAKE_CURRENT_LIST_DIR} NAME)
 add_executable(${_TEST_NAME}_tests ${_TEST_NAME}_tests_main.cpp 
     sla_print_tests.cpp 
-    sla_test_utils.hpp sla_test_utils.cpp
+    sla_test_utils.hpp sla_test_utils.cpp sla_treebuilder_tests.cpp
     sla_raycast_tests.cpp)
 target_link_libraries(${_TEST_NAME}_tests test_common libslic3r)
 set_property(TARGET ${_TEST_NAME}_tests PROPERTY FOLDER "tests")
diff --git a/tests/sla_print/sla_print_tests.cpp b/tests/sla_print/sla_print_tests.cpp
index 82df2c1a6..dad2b9097 100644
--- a/tests/sla_print/sla_print_tests.cpp
+++ b/tests/sla_print/sla_print_tests.cpp
@@ -4,6 +4,8 @@
 
 #include "sla_test_utils.hpp"
 
+#include <libslic3r/SLA/SupportTreeMesher.hpp>
+
 namespace {
 
 const char *const BELOW_PAD_TEST_OBJECTS[] = {
@@ -37,9 +39,9 @@ TEST_CASE("Support point generator should be deterministic if seeded",
           "[SLASupportGeneration], [SLAPointGen]") {
     TriangleMesh mesh = load_model("A_upsidedown.obj");
     
-    sla::EigenMesh3D emesh{mesh};
+    sla::IndexedMesh emesh{mesh};
     
-    sla::SupportConfig supportcfg;
+    sla::SupportTreeConfig supportcfg;
     sla::SupportPointGenerator::Config autogencfg;
     autogencfg.head_diameter = float(2 * supportcfg.head_front_radius_mm);
     sla::SupportPointGenerator point_gen{emesh, autogencfg, [] {}, [](int) {}};
@@ -124,14 +126,14 @@ TEST_CASE("WingedPadAroundObjectIsValid", "[SLASupportGeneration]") {
 }
 
 TEST_CASE("ElevatedSupportGeometryIsValid", "[SLASupportGeneration]") {
-    sla::SupportConfig supportcfg;
+    sla::SupportTreeConfig supportcfg;
     supportcfg.object_elevation_mm = 5.;
     
     for (auto fname : SUPPORT_TEST_MODELS) test_supports(fname);
 }
 
 TEST_CASE("FloorSupportGeometryIsValid", "[SLASupportGeneration]") {
-    sla::SupportConfig supportcfg;
+    sla::SupportTreeConfig supportcfg;
     supportcfg.object_elevation_mm = 0;
     
     for (auto &fname: SUPPORT_TEST_MODELS) test_supports(fname, supportcfg);
@@ -139,7 +141,7 @@ TEST_CASE("FloorSupportGeometryIsValid", "[SLASupportGeneration]") {
 
 TEST_CASE("ElevatedSupportsDoNotPierceModel", "[SLASupportGeneration]") {
     
-    sla::SupportConfig supportcfg;
+    sla::SupportTreeConfig supportcfg;
     
     for (auto fname : SUPPORT_TEST_MODELS)
         test_support_model_collision(fname, supportcfg);
@@ -147,7 +149,7 @@ TEST_CASE("ElevatedSupportsDoNotPierceModel", "[SLASupportGeneration]") {
 
 TEST_CASE("FloorSupportsDoNotPierceModel", "[SLASupportGeneration]") {
     
-    sla::SupportConfig supportcfg;
+    sla::SupportTreeConfig supportcfg;
     supportcfg.object_elevation_mm = 0;
     
     for (auto fname : SUPPORT_TEST_MODELS)
@@ -228,3 +230,12 @@ TEST_CASE("Triangle mesh conversions should be correct", "[SLAConversions]")
         cntr.from_obj(infile);
     }
 }
+
+TEST_CASE("halfcone test", "[halfcone]") {
+    sla::DiffBridge br{Vec3d{1., 1., 1.}, Vec3d{10., 10., 10.}, 0.25, 0.5};
+
+    TriangleMesh m = sla::to_triangle_mesh(sla::get_mesh(br, 45));
+
+    m.require_shared_vertices();
+    m.WriteOBJFile("Halfcone.obj");
+}
diff --git a/tests/sla_print/sla_raycast_tests.cpp b/tests/sla_print/sla_raycast_tests.cpp
index c82e4569a..b56909280 100644
--- a/tests/sla_print/sla_raycast_tests.cpp
+++ b/tests/sla_print/sla_raycast_tests.cpp
@@ -1,7 +1,7 @@
 #include <catch2/catch.hpp>
 #include <test_utils.hpp>
 
-#include <libslic3r/SLA/EigenMesh3D.hpp>
+#include <libslic3r/SLA/IndexedMesh.hpp>
 #include <libslic3r/SLA/Hollowing.hpp>
 
 #include "sla_test_utils.hpp"
@@ -65,7 +65,7 @@ TEST_CASE("Raycaster with loaded drillholes", "[sla_raycast]")
     cube.merge(*cube_inside);
     cube.require_shared_vertices();
     
-    sla::EigenMesh3D emesh{cube};
+    sla::IndexedMesh emesh{cube};
     emesh.load_holes(holes);
     
     Vec3d s = center.cast<double>();
diff --git a/tests/sla_print/sla_test_utils.cpp b/tests/sla_print/sla_test_utils.cpp
index 1eaf796c0..8978281d8 100644
--- a/tests/sla_print/sla_test_utils.cpp
+++ b/tests/sla_print/sla_test_utils.cpp
@@ -2,13 +2,13 @@
 #include "libslic3r/SLA/AGGRaster.hpp"
 
 void test_support_model_collision(const std::string          &obj_filename,
-                                  const sla::SupportConfig   &input_supportcfg,
+                                  const sla::SupportTreeConfig   &input_supportcfg,
                                   const sla::HollowingConfig &hollowingcfg,
                                   const sla::DrainHoles      &drainholes)
 {
     SupportByproducts byproducts;
     
-    sla::SupportConfig supportcfg = input_supportcfg;
+    sla::SupportTreeConfig supportcfg = input_supportcfg;
     
     // Set head penetration to a small negative value which should ensure that
     // the supports will not touch the model body.
@@ -69,11 +69,12 @@ void export_failed_case(const std::vector<ExPolygons> &support_slices, const Sup
     m.merge(byproducts.input_mesh);
     m.repair();
     m.require_shared_vertices();
-    m.WriteOBJFile(byproducts.obj_fname.c_str());
+    m.WriteOBJFile((Catch::getResultCapture().getCurrentTestName() + "_" +
+                    byproducts.obj_fname).c_str());
 }
 
 void test_supports(const std::string          &obj_filename,
-                   const sla::SupportConfig   &supportcfg,
+                   const sla::SupportTreeConfig   &supportcfg,
                    const sla::HollowingConfig &hollowingcfg,
                    const sla::DrainHoles      &drainholes,
                    SupportByproducts          &out)
@@ -104,7 +105,7 @@ void test_supports(const std::string          &obj_filename,
     
     // Create the special index-triangle mesh with spatial indexing which
     // is the input of the support point and support mesh generators
-    sla::EigenMesh3D emesh{mesh};
+    sla::IndexedMesh emesh{mesh};
 
 #ifdef SLIC3R_HOLE_RAYCASTER
     if (hollowingcfg.enabled) 
@@ -129,8 +130,7 @@ void test_supports(const std::string          &obj_filename,
     // If there is no elevation, support points shall be removed from the
     // bottom of the object.
     if (std::abs(supportcfg.object_elevation_mm) < EPSILON) {
-        sla::remove_bottom_points(support_points, zmin,
-                                  supportcfg.base_height_mm);
+        sla::remove_bottom_points(support_points, zmin + supportcfg.base_height_mm);
     } else {
         // Should be support points at least on the bottom of the model
         REQUIRE_FALSE(support_points.empty());
@@ -141,7 +141,8 @@ void test_supports(const std::string          &obj_filename,
     
     // Generate the actual support tree
     sla::SupportTreeBuilder treebuilder;
-    treebuilder.build(sla::SupportableMesh{emesh, support_points, supportcfg});
+    sla::SupportableMesh    sm{emesh, support_points, supportcfg};
+    sla::SupportTreeBuildsteps::execute(treebuilder, sm);
     
     check_support_tree_integrity(treebuilder, supportcfg);
     
@@ -157,8 +158,8 @@ void test_supports(const std::string          &obj_filename,
     if (std::abs(supportcfg.object_elevation_mm) < EPSILON)
         allowed_zmin = zmin - 2 * supportcfg.head_back_radius_mm;
     
-    REQUIRE(obb.min.z() >= allowed_zmin);
-    REQUIRE(obb.max.z() <= zmax);
+    REQUIRE(obb.min.z() >= Approx(allowed_zmin));
+    REQUIRE(obb.max.z() <= Approx(zmax));
     
     // Move out the support tree into the byproducts, we can examine it further
     // in various tests.
@@ -168,15 +169,15 @@ void test_supports(const std::string          &obj_filename,
 }
 
 void check_support_tree_integrity(const sla::SupportTreeBuilder &stree, 
-                                  const sla::SupportConfig &cfg)
+                                  const sla::SupportTreeConfig &cfg)
 {
     double gnd  = stree.ground_level;
     double H1   = cfg.max_solo_pillar_height_mm;
     double H2   = cfg.max_dual_pillar_height_mm;
     
     for (const sla::Head &head : stree.heads()) {
-        REQUIRE((!head.is_valid() || head.pillar_id != sla::ID_UNSET ||
-                head.bridge_id != sla::ID_UNSET));
+        REQUIRE((!head.is_valid() || head.pillar_id != sla::SupportTreeNode::ID_UNSET ||
+                head.bridge_id != sla::SupportTreeNode::ID_UNSET));
     }
     
     for (const sla::Pillar &pillar : stree.pillars()) {
diff --git a/tests/sla_print/sla_test_utils.hpp b/tests/sla_print/sla_test_utils.hpp
index 3652b1f81..fdd883ed8 100644
--- a/tests/sla_print/sla_test_utils.hpp
+++ b/tests/sla_print/sla_test_utils.hpp
@@ -67,16 +67,16 @@ struct SupportByproducts
 const constexpr float CLOSING_RADIUS = 0.005f;
 
 void check_support_tree_integrity(const sla::SupportTreeBuilder &stree,
-                                  const sla::SupportConfig &cfg);
+                                  const sla::SupportTreeConfig &cfg);
 
 void test_supports(const std::string          &obj_filename,
-                   const sla::SupportConfig   &supportcfg,
+                   const sla::SupportTreeConfig   &supportcfg,
                    const sla::HollowingConfig &hollowingcfg,
                    const sla::DrainHoles      &drainholes,
                    SupportByproducts          &out);
 
 inline void test_supports(const std::string &obj_filename,
-                   const sla::SupportConfig &supportcfg,
+                   const sla::SupportTreeConfig &supportcfg,
                    SupportByproducts        &out) 
 {
     sla::HollowingConfig hcfg;
@@ -85,7 +85,7 @@ inline void test_supports(const std::string &obj_filename,
 }
 
 inline void test_supports(const std::string &obj_filename,
-                   const sla::SupportConfig &supportcfg = {})
+                   const sla::SupportTreeConfig &supportcfg = {})
 {
     SupportByproducts byproducts;
     test_supports(obj_filename, supportcfg, byproducts);
@@ -97,13 +97,13 @@ void export_failed_case(const std::vector<ExPolygons> &support_slices,
 
 void test_support_model_collision(
     const std::string          &obj_filename,
-    const sla::SupportConfig   &input_supportcfg,
+    const sla::SupportTreeConfig   &input_supportcfg,
     const sla::HollowingConfig &hollowingcfg,
     const sla::DrainHoles      &drainholes);
 
 inline void test_support_model_collision(
     const std::string        &obj_filename,
-    const sla::SupportConfig &input_supportcfg = {}) 
+    const sla::SupportTreeConfig &input_supportcfg = {}) 
 {
     sla::HollowingConfig hcfg;
     hcfg.enabled = false;
diff --git a/tests/sla_print/sla_treebuilder_tests.cpp b/tests/sla_print/sla_treebuilder_tests.cpp
new file mode 100644
index 000000000..91c2ea6f8
--- /dev/null
+++ b/tests/sla_print/sla_treebuilder_tests.cpp
@@ -0,0 +1,99 @@
+//#include <catch2/catch.hpp>
+//#include <test_utils.hpp>
+
+//#include "libslic3r/TriangleMesh.hpp"
+//#include "libslic3r/SLA/SupportTreeBuildsteps.hpp"
+//#include "libslic3r/SLA/SupportTreeMesher.hpp"
+
+//TEST_CASE("Test bridge_mesh_intersect on a cube's wall", "[SLABridgeMeshInters]") {
+//    using namespace Slic3r;
+
+//    TriangleMesh cube = make_cube(10., 10., 10.);
+
+//    sla::SupportConfig cfg = {}; // use default config
+//    sla::SupportPoints pts = {{10.f, 5.f, 5.f, float(cfg.head_front_radius_mm), false}};
+//    sla::SupportableMesh sm{cube, pts, cfg};
+
+//    size_t steps = 45;
+//    SECTION("Bridge is straight horizontal and pointing away from the cube") {
+
+//        sla::Bridge bridge(pts[0].pos.cast<double>(), Vec3d{15., 5., 5.},
+//                           pts[0].head_front_radius);
+
+//        auto hit = sla::query_hit(sm, bridge);
+
+//        REQUIRE(std::isinf(hit.distance()));
+
+//        cube.merge(sla::to_triangle_mesh(get_mesh(bridge, steps)));
+//        cube.require_shared_vertices();
+//        cube.WriteOBJFile("cube1.obj");
+//    }
+
+//    SECTION("Bridge is tilted down in 45 degrees, pointing away from the cube") {
+//        sla::Bridge bridge(pts[0].pos.cast<double>(), Vec3d{15., 5., 0.},
+//                           pts[0].head_front_radius);
+
+//        auto hit = sla::query_hit(sm, bridge);
+
+//        REQUIRE(std::isinf(hit.distance()));
+
+//        cube.merge(sla::to_triangle_mesh(get_mesh(bridge, steps)));
+//        cube.require_shared_vertices();
+//        cube.WriteOBJFile("cube2.obj");
+//    }
+//}
+
+
+//TEST_CASE("Test bridge_mesh_intersect on a sphere", "[SLABridgeMeshInters]") {
+//    using namespace Slic3r;
+
+//    TriangleMesh sphere = make_sphere(1.);
+
+//    sla::SupportConfig cfg = {}; // use default config
+//    cfg.head_back_radius_mm = cfg.head_front_radius_mm;
+//    sla::SupportPoints pts = {{1.f, 0.f, 0.f, float(cfg.head_front_radius_mm), false}};
+//    sla::SupportableMesh sm{sphere, pts, cfg};
+
+//    size_t steps = 45;
+//    SECTION("Bridge is straight horizontal and pointing away from the sphere") {
+
+//        sla::Bridge bridge(pts[0].pos.cast<double>(), Vec3d{2., 0., 0.},
+//                           pts[0].head_front_radius);
+
+//        auto hit = sla::query_hit(sm, bridge);
+
+//        sphere.merge(sla::to_triangle_mesh(get_mesh(bridge, steps)));
+//        sphere.require_shared_vertices();
+//        sphere.WriteOBJFile("sphere1.obj");
+
+//        REQUIRE(std::isinf(hit.distance()));
+//    }
+
+//    SECTION("Bridge is tilted down 45 deg and pointing away from the sphere") {
+
+//        sla::Bridge bridge(pts[0].pos.cast<double>(), Vec3d{2., 0., -2.},
+//                           pts[0].head_front_radius);
+
+//        auto hit = sla::query_hit(sm, bridge);
+
+//        sphere.merge(sla::to_triangle_mesh(get_mesh(bridge, steps)));
+//        sphere.require_shared_vertices();
+//        sphere.WriteOBJFile("sphere2.obj");
+
+//        REQUIRE(std::isinf(hit.distance()));
+//    }
+
+//    SECTION("Bridge is tilted down 90 deg and pointing away from the sphere") {
+
+//        sla::Bridge bridge(pts[0].pos.cast<double>(), Vec3d{1., 0., -2.},
+//                           pts[0].head_front_radius);
+
+//        auto hit = sla::query_hit(sm, bridge);
+
+//        sphere.merge(sla::to_triangle_mesh(get_mesh(bridge, steps)));
+//        sphere.require_shared_vertices();
+//        sphere.WriteOBJFile("sphere3.obj");
+
+//        REQUIRE(std::isinf(hit.distance()));
+//    }
+//}