Merge branch 'tm_sla_tests'

21 files changed, 4188 insertions, 4046 deletions

diff --git a/src/libslic3r/SLA/SLAAutoSupports.cpp b/src/libslic3r/SLA/SLAAutoSupports.cpp
index 36378df39..65f590143 100644
--- a/src/libslic3r/SLA/SLAAutoSupports.cpp
+++ b/src/libslic3r/SLA/SLAAutoSupports.cpp
@@ -16,6 +16,7 @@
 #include <random>
 
 namespace Slic3r {
+namespace sla {
 
 /*float SLAAutoSupports::approximate_geodesic_distance(const Vec3d& p1, const Vec3d& p2, Vec3d& n1, Vec3d& n2)
 {
@@ -48,9 +49,16 @@ float SLAAutoSupports::distance_limit(float angle) const
     return 1./(2.4*get_required_density(angle));
 }*/
 
-SLAAutoSupports::SLAAutoSupports(const TriangleMesh& mesh, const sla::EigenMesh3D& 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)
-: m_config(config), m_emesh(emesh), m_throw_on_cancel(throw_on_cancel), m_statusfn(statusfn)
+SLAAutoSupports::SLAAutoSupports(const sla::EigenMesh3D &       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)
+    : m_config(config)
+    , m_emesh(emesh)
+    , m_throw_on_cancel(throw_on_cancel)
+    , m_statusfn(statusfn)
 {
     process(slices, heights);
     project_onto_mesh(m_output);
@@ -505,6 +513,21 @@ void SLAAutoSupports::uniformly_cover(const ExPolygons& islands, Structure& stru
     }
 }
 
+void remove_bottom_points(std::vector<SupportPoint> &pts, double gnd_lvl, double tolerance)
+{
+    // 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;
+    });
+
+    // erase all elements after the new end
+    pts.erase(endit, pts.end());
+}
+
 #ifdef SLA_AUTOSUPPORTS_DEBUG
 void SLAAutoSupports::output_structures(const std::vector<Structure>& structures)
 {
@@ -533,4 +556,5 @@ void SLAAutoSupports::output_expolygons(const ExPolygons& expolys, const std::st
 }
 #endif
 
+} // namespace sla
 } // namespace Slic3r
diff --git a/src/libslic3r/SLA/SLAAutoSupports.hpp b/src/libslic3r/SLA/SLAAutoSupports.hpp
index 38f21e6cf..d2f50f0a4 100644
--- a/src/libslic3r/SLA/SLAAutoSupports.hpp
+++ b/src/libslic3r/SLA/SLAAutoSupports.hpp
@@ -11,20 +11,22 @@
 // #define SLA_AUTOSUPPORTS_DEBUG
 
 namespace Slic3r {
+namespace sla {
 
 class SLAAutoSupports {
 public:
     struct Config {
-            float density_relative;
-            float minimal_distance;
-            float head_diameter;
+            float density_relative {1.f};
+            float minimal_distance {1.f};
+            float head_diameter {0.4f};
             ///////////////
             inline float support_force() const { return 7.7f / density_relative; } // a force one point can support       (arbitrary force unit)
             inline float tear_pressure() const { return 1.f; }  // pressure that the display exerts    (the force unit per mm2)
         };
 
-    SLAAutoSupports(const TriangleMesh& mesh, const sla::EigenMesh3D& emesh, const std::vector<ExPolygons>& slices,
+    SLAAutoSupports(const sla::EigenMesh3D& 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);
+    
     const std::vector<sla::SupportPoint>& output() { return m_output; }
 
 	struct MyLayer;
@@ -199,7 +201,9 @@ private:
     std::function<void(int)>  m_statusfn;
 };
 
+void remove_bottom_points(std::vector<SupportPoint> &pts, double gnd_lvl, double tolerance);
 
+} // namespace sla
 } // namespace Slic3r
 
 
diff --git a/src/libslic3r/SLA/SLABasePool.cpp b/src/libslic3r/SLA/SLABasePool.cpp
deleted file mode 100644
index 53dfef404..000000000
--- a/src/libslic3r/SLA/SLABasePool.cpp
+++ /dev/null
@@ -1,922 +0,0 @@
-#include "SLABasePool.hpp"
-#include "SLABoilerPlate.hpp"
-
-#include "boost/log/trivial.hpp"
-#include "SLABoostAdapter.hpp"
-#include "ClipperUtils.hpp"
-#include "Tesselate.hpp"
-#include "MTUtils.hpp"
-
-// For debugging:
-// #include <fstream>
-// #include <libnest2d/tools/benchmark.h>
-// #include "SVG.hpp"
-
-namespace Slic3r { namespace sla {
-
-/// This function will return a triangulation of a sheet connecting an upper
-/// and a lower plate given as input polygons. It will not triangulate the
-/// plates themselves only the sheet. The caller has to specify the lower and
-/// upper z levels in world coordinates as well as the offset difference
-/// between the sheets. If the lower_z_mm is higher than upper_z_mm or the
-/// offset difference is negative, the resulting triangle orientation will be
-/// reversed.
-///
-/// IMPORTANT: This is not a universal triangulation algorithm. It assumes
-/// that the lower and upper polygons are offsetted versions of the same
-/// original polygon. In general, it assumes that one of the polygons is
-/// completely inside the other. The offset difference is the reference
-/// distance from the inner polygon's perimeter to the outer polygon's
-/// perimeter. The real distance will be variable as the clipper offset has
-/// different strategies (rounding, etc...). This algorithm should have
-/// O(2n + 3m) complexity where n is the number of upper vertices and m is the
-/// number of lower vertices.
-Contour3D walls(const Polygon& lower, const Polygon& upper,
-                double lower_z_mm, double upper_z_mm,
-                double offset_difference_mm, ThrowOnCancel thr)
-{
-    Contour3D ret;
-
-    if(upper.points.size() < 3 || lower.size() < 3) return ret;
-
-    // The concept of the algorithm is relatively simple. It will try to find
-    // the closest vertices from the upper and the lower polygon and use those
-    // as starting points. Then it will create the triangles sequentially using
-    // an edge from the upper polygon and a vertex from the lower or vice versa,
-    // depending on the resulting triangle's quality.
-    // The quality is measured by a scalar value. So far it looks like it is
-    // enough to derive it from the slope of the triangle's two edges connecting
-    // the upper and the lower part. A reference slope is calculated from the
-    // height and the offset difference.
-
-    // Offset in the index array for the ceiling
-    const auto offs = upper.points.size();
-
-    // Shorthand for the vertex arrays
-    auto& upoints = upper.points, &lpoints = lower.points;
-    auto& rpts = ret.points; auto& ind = ret.indices;
-
-    // If the Z levels are flipped, or the offset difference is negative, we
-    // will interpret that as the triangles normals should be inverted.
-    bool inverted = upper_z_mm < lower_z_mm || offset_difference_mm < 0;
-
-    // Copy the points into the mesh, convert them from 2D to 3D
-    rpts.reserve(upoints.size() + lpoints.size());
-    ind.reserve(2 * upoints.size() + 2 * lpoints.size());
-    for (auto &p : upoints)
-        rpts.emplace_back(unscaled(p.x()), unscaled(p.y()), upper_z_mm);
-    for (auto &p : lpoints)
-        rpts.emplace_back(unscaled(p.x()), unscaled(p.y()), lower_z_mm);
-
-    // Create pointing indices into vertex arrays. u-upper, l-lower
-    size_t uidx = 0, lidx = offs, unextidx = 1, lnextidx = offs + 1;
-
-    // Simple squared distance calculation.
-    auto distfn = [](const Vec3d& p1, const Vec3d& p2) {
-        auto p = p1 - p2; return p.transpose() * p;
-    };
-
-    // We need to find the closest point on lower polygon to the first point on
-    // the upper polygon. These will be our starting points.
-    double distmin = std::numeric_limits<double>::max();
-    for(size_t l = lidx; l < rpts.size(); ++l) {
-        thr();
-        double d = distfn(rpts[l], rpts[uidx]);
-        if(d < distmin) { lidx = l; distmin = d; }
-    }
-
-    // Set up lnextidx to be ahead of lidx in cyclic mode
-    lnextidx = lidx + 1;
-    if(lnextidx == rpts.size()) lnextidx = offs;
-
-    // This will be the flip switch to toggle between upper and lower triangle
-    // creation mode
-    enum class Proceed {
-        UPPER, // A segment from the upper polygon and one vertex from the lower
-        LOWER  // A segment from the lower polygon and one vertex from the upper
-    } proceed = Proceed::UPPER;
-
-    // Flags to help evaluating loop termination.
-    bool ustarted = false, lstarted = false;
-
-    // The variables for the fitness values, one for the actual and one for the
-    // previous.
-    double current_fit = 0, prev_fit = 0;
-
-    // Every triangle of the wall has two edges connecting the upper plate with
-    // the lower plate. From the length of these two edges and the zdiff we
-    // can calculate the momentary squared offset distance at a particular
-    // position on the wall. The average of the differences from the reference
-    // (squared) offset distance will give us the driving fitness value.
-    const double offsdiff2 = std::pow(offset_difference_mm, 2);
-    const double zdiff2 = std::pow(upper_z_mm - lower_z_mm, 2);
-
-    // Mark the current vertex iterator positions. If the iterators return to
-    // the same position, the loop can be terminated.
-    size_t uendidx = uidx, lendidx = lidx;
-
-    do { thr();  // check throw if canceled
-
-        prev_fit = current_fit;
-
-        switch(proceed) {   // proceed depending on the current state
-        case Proceed::UPPER:
-            if(!ustarted || uidx != uendidx) { // there are vertices remaining
-                // Get the 3D vertices in order
-                const Vec3d& p_up1 = rpts[uidx];
-                const Vec3d& p_low = rpts[lidx];
-                const Vec3d& p_up2 = rpts[unextidx];
-
-                // Calculate fitness: the average of the two connecting edges
-                double a = offsdiff2 - (distfn(p_up1, p_low) - zdiff2);
-                double b = offsdiff2 - (distfn(p_up2, p_low) - zdiff2);
-                current_fit = (std::abs(a) + std::abs(b)) / 2;
-
-                if(current_fit > prev_fit) { // fit is worse than previously
-                    proceed = Proceed::LOWER;
-                } else {    // good to go, create the triangle
-                    inverted
-                        ? ind.emplace_back(int(unextidx), int(lidx), int(uidx))
-                        : ind.emplace_back(int(uidx), int(lidx), int(unextidx));
-
-                    // Increment the iterators, rotate if necessary
-                    ++uidx; ++unextidx;
-                    if(unextidx == offs) unextidx = 0;
-                    if(uidx == offs) uidx = 0;
-
-                    ustarted = true;    // mark the movement of the iterators
-                    // so that the comparison to uendidx can be made correctly
-                }
-            } else proceed = Proceed::LOWER;
-
-            break;
-        case Proceed::LOWER:
-            // Mode with lower segment, upper vertex. Same structure:
-            if(!lstarted || lidx != lendidx) {
-                const Vec3d& p_low1 = rpts[lidx];
-                const Vec3d& p_low2 = rpts[lnextidx];
-                const Vec3d& p_up   = rpts[uidx];
-
-                double a = offsdiff2 - (distfn(p_up, p_low1) - zdiff2);
-                double b = offsdiff2 - (distfn(p_up, p_low2) - zdiff2);
-                current_fit = (std::abs(a) + std::abs(b)) / 2;
-
-                if(current_fit > prev_fit) {
-                    proceed = Proceed::UPPER;
-                } else {
-                    inverted
-                        ? ind.emplace_back(int(uidx), int(lnextidx), int(lidx))
-                        : ind.emplace_back(int(lidx), int(lnextidx), int(uidx));
-
-                    ++lidx; ++lnextidx;
-                    if(lnextidx == rpts.size()) lnextidx = offs;
-                    if(lidx == rpts.size()) lidx = offs;
-
-                    lstarted = true;
-                }
-            } else proceed = Proceed::UPPER;
-
-            break;
-        } // end of switch
-    } while(!ustarted || !lstarted || uidx != uendidx || lidx != lendidx);
-
-    return ret;
-}
-
-/// Offsetting with clipper and smoothing the edges into a curvature.
-void offset(ExPolygon& sh, coord_t distance, bool edgerounding = true) {
-    using ClipperLib::ClipperOffset;
-    using ClipperLib::jtRound;
-    using ClipperLib::jtMiter;
-    using ClipperLib::etClosedPolygon;
-    using ClipperLib::Paths;
-    using ClipperLib::Path;
-
-    auto&& ctour = Slic3rMultiPoint_to_ClipperPath(sh.contour);
-    auto&& holes = Slic3rMultiPoints_to_ClipperPaths(sh.holes);
-
-    // If the input is not at least a triangle, we can not do this algorithm
-    if(ctour.size() < 3 ||
-       std::any_of(holes.begin(), holes.end(),
-                   [](const Path& p) { return p.size() < 3; })
-            ) {
-        BOOST_LOG_TRIVIAL(error) << "Invalid geometry for offsetting!";
-        return;
-    }
-
-    auto jointype = edgerounding? jtRound : jtMiter;
-    
-    ClipperOffset offs;
-    offs.ArcTolerance = scaled<double>(0.01);
-    Paths result;
-    offs.AddPath(ctour, jointype, etClosedPolygon);
-    offs.AddPaths(holes, jointype, etClosedPolygon);
-    offs.Execute(result, static_cast<double>(distance));
-
-    // Offsetting reverts the orientation and also removes the last vertex
-    // so boost will not have a closed polygon.
-
-    bool found_the_contour = false;
-    sh.holes.clear();
-    for(auto& r : result) {
-        if(ClipperLib::Orientation(r)) {
-            // We don't like if the offsetting generates more than one contour
-            // but throwing would be an overkill. Instead, we should warn the
-            // caller about the inability to create correct geometries
-            if(!found_the_contour) {
-                auto rr = ClipperPath_to_Slic3rPolygon(r);
-                sh.contour.points.swap(rr.points);
-                found_the_contour = true;
-            } else {
-                BOOST_LOG_TRIVIAL(warning)
-                        << "Warning: offsetting result is invalid!";
-            }
-        } else {
-            // TODO If there are multiple contours we can't be sure which hole
-            // belongs to the first contour. (But in this case the situation is
-            // bad enough to let it go...)
-            sh.holes.emplace_back(ClipperPath_to_Slic3rPolygon(r));
-        }
-    }
-}
-
-void offset(Polygon &sh, coord_t distance, bool edgerounding = true)
-{
-    using ClipperLib::ClipperOffset;
-    using ClipperLib::jtRound;
-    using ClipperLib::jtMiter;
-    using ClipperLib::etClosedPolygon;
-    using ClipperLib::Paths;
-    using ClipperLib::Path;
-
-    auto &&ctour = Slic3rMultiPoint_to_ClipperPath(sh);
-
-    // If the input is not at least a triangle, we can not do this algorithm
-    if (ctour.size() < 3) {
-        BOOST_LOG_TRIVIAL(error) << "Invalid geometry for offsetting!";
-        return;
-    }
-
-    ClipperOffset offs;
-    offs.ArcTolerance = 0.01 * scaled(1.);
-    Paths result;
-    offs.AddPath(ctour, edgerounding ? jtRound : jtMiter, etClosedPolygon);
-    offs.Execute(result, static_cast<double>(distance));
-
-    // Offsetting reverts the orientation and also removes the last vertex
-    // so boost will not have a closed polygon.
-
-    bool found_the_contour = false;
-    for (auto &r : result) {
-        if (ClipperLib::Orientation(r)) {
-            // We don't like if the offsetting generates more than one contour
-            // but throwing would be an overkill. Instead, we should warn the
-            // caller about the inability to create correct geometries
-            if (!found_the_contour) {
-                auto rr = ClipperPath_to_Slic3rPolygon(r);
-                sh.points.swap(rr.points);
-                found_the_contour = true;
-            } else {
-                BOOST_LOG_TRIVIAL(warning)
-                    << "Warning: offsetting result is invalid!";
-            }
-        }
-    }
-}
-
-/// Unification of polygons (with clipper) preserving holes as well.
-ExPolygons unify(const ExPolygons& shapes) {
-    using ClipperLib::ptSubject;
-
-    ExPolygons retv;
-
-    bool closed = true;
-    bool valid = true;
-
-    ClipperLib::Clipper clipper;
-
-    for(auto& path : shapes) {
-        auto clipperpath = Slic3rMultiPoint_to_ClipperPath(path.contour);
-
-        if(!clipperpath.empty())
-            valid &= clipper.AddPath(clipperpath, ptSubject, closed);
-
-        auto clipperholes = Slic3rMultiPoints_to_ClipperPaths(path.holes);
-
-        for(auto& hole : clipperholes) {
-            if(!hole.empty())
-                valid &= clipper.AddPath(hole, ptSubject, closed);
-        }
-    }
-
-    if(!valid) BOOST_LOG_TRIVIAL(warning) << "Unification of invalid shapes!";
-
-    ClipperLib::PolyTree result;
-    clipper.Execute(ClipperLib::ctUnion, result, ClipperLib::pftNonZero);
-
-    retv.reserve(static_cast<size_t>(result.Total()));
-
-    // Now we will recursively traverse the polygon tree and serialize it
-    // into an ExPolygon with holes. The polygon tree has the clipper-ish
-    // PolyTree structure which alternates its nodes as contours and holes
-
-    // A "declaration" of function for traversing leafs which are holes
-    std::function<void(ClipperLib::PolyNode*, ExPolygon&)> processHole;
-
-    // Process polygon which calls processHoles which than calls processPoly
-    // again until no leafs are left.
-    auto processPoly = [&retv, &processHole](ClipperLib::PolyNode *pptr) {
-        ExPolygon poly;
-        poly.contour.points = ClipperPath_to_Slic3rPolygon(pptr->Contour);
-        for(auto h : pptr->Childs) { processHole(h, poly); }
-        retv.push_back(poly);
-    };
-
-    // Body of the processHole function
-    processHole = [&processPoly](ClipperLib::PolyNode *pptr, ExPolygon& poly)
-    {
-        poly.holes.emplace_back();
-        poly.holes.back().points = ClipperPath_to_Slic3rPolygon(pptr->Contour);
-        for(auto c : pptr->Childs) processPoly(c);
-    };
-
-    // Wrapper for traversing.
-    auto traverse = [&processPoly] (ClipperLib::PolyNode *node)
-    {
-        for(auto ch : node->Childs) {
-            processPoly(ch);
-        }
-    };
-
-    // Here is the actual traverse
-    traverse(&result);
-
-    return retv;
-}
-
-Polygons unify(const Polygons& shapes) {
-    using ClipperLib::ptSubject;
-    
-    bool closed = true;
-    bool valid = true;
-
-    ClipperLib::Clipper clipper;
-
-    for(auto& path : shapes) {
-        auto clipperpath = Slic3rMultiPoint_to_ClipperPath(path);
-
-        if(!clipperpath.empty())
-            valid &= clipper.AddPath(clipperpath, ptSubject, closed);
-    }
-
-    if(!valid) BOOST_LOG_TRIVIAL(warning) << "Unification of invalid shapes!";
-
-    ClipperLib::Paths result;
-    clipper.Execute(ClipperLib::ctUnion, result, ClipperLib::pftNonZero);
-
-    Polygons ret;
-    for (ClipperLib::Path &p : result) {
-        Polygon pp = ClipperPath_to_Slic3rPolygon(p);
-        if (!pp.is_clockwise()) ret.emplace_back(std::move(pp));
-    }
-    
-    return ret;
-}
-
-// Function to cut tiny connector cavities for a given polygon. The input poly
-// will be offsetted by "padding" and small rectangle shaped cavities will be
-// inserted along the perimeter in every "stride" distance. The stick rectangles
-// will have a with about "stick_width". The input dimensions are in world 
-// measure, not the scaled clipper units.
-void breakstick_holes(ExPolygon& poly,
-                      double padding,
-                      double stride,
-                      double stick_width,
-                      double penetration)
-{   
-    // SVG svg("bridgestick_plate.svg");
-    // svg.draw(poly);
-
-    auto transf = [stick_width, penetration, padding, stride](Points &pts) {
-        // The connector stick will be a small rectangle with dimensions
-        // stick_width x (penetration + padding) to have some penetration
-        // into the input polygon.
-
-        Points out;
-        out.reserve(2 * pts.size()); // output polygon points
-
-        // stick bottom and right edge dimensions
-        double sbottom = scaled(stick_width);
-        double sright  = scaled(penetration + padding);
-
-        // scaled stride distance
-        double sstride = scaled(stride);
-        double t       = 0;
-
-        // process pairs of vertices as an edge, start with the last and
-        // first point
-        for (size_t i = pts.size() - 1, j = 0; j < pts.size(); i = j, ++j) {
-            // Get vertices and the direction vectors
-            const Point &a = pts[i], &b = pts[j];
-            Vec2d        dir = b.cast<double>() - a.cast<double>();
-            double       nrm = dir.norm();
-            dir /= nrm;
-            Vec2d dirp(-dir(Y), dir(X));
-
-            // Insert start point
-            out.emplace_back(a);
-
-            // dodge the start point, do not make sticks on the joins
-            while (t < sbottom) t += sbottom;
-            double tend = nrm - sbottom;
-
-            while (t < tend) { // insert the stick on the polygon perimeter
-
-                // calculate the stick rectangle vertices and insert them
-                // into the output.
-                Point p1 = a + (t * dir).cast<coord_t>();
-                Point p2 = p1 + (sright * dirp).cast<coord_t>();
-                Point p3 = p2 + (sbottom * dir).cast<coord_t>();
-                Point p4 = p3 + (sright * -dirp).cast<coord_t>();
-                out.insert(out.end(), {p1, p2, p3, p4});
-
-                // continue along the perimeter
-                t += sstride;
-            }
-
-            t = t - nrm;
-
-            // Insert edge endpoint
-            out.emplace_back(b);
-        }
-        
-        // move the new points
-        out.shrink_to_fit();
-        pts.swap(out);
-    };
-    
-    if(stride > 0.0 && stick_width > 0.0 && padding > 0.0) {
-        transf(poly.contour.points);
-        for (auto &h : poly.holes) transf(h.points);
-    }
-    
-    // svg.draw(poly);
-    // svg.Close();
-}
-
-/// This method will create a rounded edge around a flat polygon in 3d space.
-/// 'base_plate' parameter is the target plate.
-/// 'radius' is the radius of the edges.
-/// 'degrees' is tells how much of a circle should be created as the rounding.
-///     It should be in degrees, not radians.
-/// 'ceilheight_mm' is the Z coordinate of the flat polygon in 3D space.
-/// 'dir' Is the direction of the round edges: inward or outward
-/// 'thr' Throws if a cancel signal was received
-/// 'last_offset' An auxiliary output variable to save the last offsetted
-///     version of 'base_plate'
-/// 'last_height' An auxiliary output to save the last z coordinate of the
-/// offsetted base_plate. In other words, where the rounded edges end.
-Contour3D round_edges(const ExPolygon& base_plate,
-                      double radius_mm,
-                      double degrees,
-                      double ceilheight_mm,
-                      bool dir,
-                      ThrowOnCancel thr,
-                      ExPolygon& last_offset, double& last_height)
-{
-    auto ob = base_plate;
-    auto ob_prev = ob;
-    double wh = ceilheight_mm, wh_prev = wh;
-    Contour3D curvedwalls;
-
-    int steps = 30;
-    double stepx = radius_mm / steps;
-    coord_t s = dir? 1 : -1;
-    degrees = std::fmod(degrees, 180);
-
-    // we use sin for x distance because we interpret the angle starting from
-    // PI/2
-    int tos = degrees < 90?
-            int(radius_mm*std::cos(degrees * PI / 180 - PI/2) / stepx) : steps;
-
-    for(int i = 1; i <= tos; ++i) {
-        thr();
-
-        ob = base_plate;
-
-        double r2 = radius_mm * radius_mm;
-        double xx = i*stepx;
-        double x2 = xx*xx;
-        double stepy = std::sqrt(r2 - x2);
-
-        offset(ob, s * scaled(xx));
-        wh = ceilheight_mm - radius_mm + stepy;
-
-        Contour3D pwalls;
-        double prev_x = xx - (i - 1) * stepx;
-        pwalls = walls(ob.contour, ob_prev.contour, wh, wh_prev, s*prev_x, thr);
-
-        curvedwalls.merge(pwalls);
-        ob_prev = ob;
-        wh_prev = wh;
-    }
-
-    if(degrees > 90) {
-        double tox = radius_mm - radius_mm*std::cos(degrees * PI / 180 - PI/2);
-        int tos = int(tox / stepx);
-
-        for(int i = 1; i <= tos; ++i) {
-            thr();
-            ob = base_plate;
-
-            double r2 = radius_mm * radius_mm;
-            double xx = radius_mm - i*stepx;
-            double x2 = xx*xx;
-            double stepy = std::sqrt(r2 - x2);
-            offset(ob, s * scaled(xx));
-            wh = ceilheight_mm - radius_mm - stepy;
-
-            Contour3D pwalls;
-            double prev_x = xx - radius_mm + (i - 1)*stepx;
-            pwalls =
-                walls(ob_prev.contour, ob.contour, wh_prev, wh, s*prev_x, thr);
-
-            curvedwalls.merge(pwalls);
-            ob_prev = ob;
-            wh_prev = wh;
-        }
-    }
-
-    last_offset = std::move(ob);
-    last_height = wh;
-
-    return curvedwalls;
-}
-
-inline Point centroid(Points& pp) {
-    Point c;
-    switch(pp.size()) {
-    case 0: break;
-    case 1: c = pp.front(); break;
-    case 2: c = (pp[0] + pp[1]) / 2; break;
-    default: {
-        auto MAX = std::numeric_limits<Point::coord_type>::max();
-        auto MIN = std::numeric_limits<Point::coord_type>::min();
-        Point min = {MAX, MAX}, max = {MIN, MIN};
-
-        for(auto& p : pp) {
-            if(p(0) < min(0)) min(0) = p(0);
-            if(p(1) < min(1)) min(1) = p(1);
-            if(p(0) > max(0)) max(0) = p(0);
-            if(p(1) > max(1)) max(1) = p(1);
-        }
-        c(0) = min(0) + (max(0) - min(0)) / 2;
-        c(1) = min(1) + (max(1) - min(1)) / 2;
-
-        // TODO: fails for non convex cluster
-//        c = std::accumulate(pp.begin(), pp.end(), Point{0, 0});
-//        x(c) /= coord_t(pp.size()); y(c) /= coord_t(pp.size());
-        break;
-    }
-    }
-
-    return c;
-}
-
-inline Point centroid(const Polygon& poly) {
-    return poly.centroid();
-}
-
-/// A fake concave hull that is constructed by connecting separate shapes
-/// with explicit bridges. Bridges are generated from each shape's centroid
-/// to the center of the "scene" which is the centroid calculated from the shape
-/// centroids (a star is created...)
-Polygons concave_hull(const Polygons& polys, double maxd_mm, ThrowOnCancel thr)
-{
-    namespace bgi = boost::geometry::index;
-    using SpatElement = std::pair<Point, unsigned>;
-    using SpatIndex = bgi::rtree< SpatElement, bgi::rstar<16, 4> >;
-
-    if(polys.empty()) return Polygons();
-    
-    const double max_dist = scaled(maxd_mm);
-
-    Polygons punion = unify(polys);   // could be redundant
-
-    if(punion.size() == 1) return punion;
-
-    // We get the centroids of all the islands in the 2D slice
-    Points centroids; centroids.reserve(punion.size());
-    std::transform(punion.begin(), punion.end(), std::back_inserter(centroids),
-                   [](const Polygon& poly) { return centroid(poly); });
-
-    SpatIndex ctrindex;
-    unsigned  idx = 0;
-    for(const Point &ct : centroids) ctrindex.insert(std::make_pair(ct, idx++));
-    
-    // Centroid of the centroids of islands. This is where the additional
-    // connector sticks are routed.
-    Point cc = centroid(centroids);
-
-    punion.reserve(punion.size() + centroids.size());
-
-    idx = 0;
-    std::transform(centroids.begin(), centroids.end(),
-                   std::back_inserter(punion),
-                   [&centroids, &ctrindex, cc, max_dist, &idx, thr]
-                   (const Point& c)
-    {
-        thr();
-        double dx = x(c) - x(cc), dy = y(c) - y(cc);
-        double l = std::sqrt(dx * dx + dy * dy);
-        double nx = dx / l, ny = dy / l;
-        
-        Point& ct = centroids[idx];
-        
-        std::vector<SpatElement> result;
-        ctrindex.query(bgi::nearest(ct, 2), std::back_inserter(result));
-
-        double dist = max_dist;
-        for (const SpatElement &el : result)
-            if (el.second != idx) {
-                dist = Line(el.first, ct).length();
-                break;
-            }
-        
-        idx++;
-        
-        if (dist >= max_dist) return Polygon();
-        
-        Polygon r;
-        auto& ctour = r.points;
-
-        ctour.reserve(3);
-        ctour.emplace_back(cc);
-
-        Point d(scaled(nx), scaled(ny));
-        ctour.emplace_back(c + Point( -y(d),  x(d) ));
-        ctour.emplace_back(c + Point(  y(d), -x(d) ));
-        offset(r, scaled(1.));
-
-        return r;
-    });
-
-    // This is unavoidable...
-    punion = unify(punion);
-
-    return punion;
-}
-
-void base_plate(const TriangleMesh &      mesh,
-                ExPolygons &              output,
-                const std::vector<float> &heights,
-                ThrowOnCancel             thrfn)
-{
-    if (mesh.empty()) return;
-    //    m.require_shared_vertices(); // TriangleMeshSlicer needs this    
-    TriangleMeshSlicer slicer(&mesh);
-    
-    std::vector<ExPolygons> out; out.reserve(heights.size());
-    slicer.slice(heights, 0.f, &out, thrfn);
-    
-    size_t count = 0; for(auto& o : out) count += o.size();
-    
-    // Now we have to unify all slice layers which can be an expensive operation
-    // so we will try to simplify the polygons
-    ExPolygons tmp; tmp.reserve(count);
-    for(ExPolygons& o : out)
-        for(ExPolygon& e : o) {
-            auto&& exss = e.simplify(scaled<double>(0.1));
-            for(ExPolygon& ep : exss) tmp.emplace_back(std::move(ep));
-        }
-    
-    ExPolygons utmp = unify(tmp);
-    
-    for(auto& o : utmp) {
-        auto&& smp = o.simplify(scaled<double>(0.1));
-        output.insert(output.end(), smp.begin(), smp.end());
-    }
-}
-
-void base_plate(const TriangleMesh &mesh,
-                ExPolygons &        output,
-                float               h,
-                float               layerh,
-                ThrowOnCancel       thrfn)
-{
-    auto bb = mesh.bounding_box();
-    float gnd = float(bb.min(Z));
-    std::vector<float> heights = {float(bb.min(Z))};
-    
-    for(float hi = gnd + layerh; hi <= gnd + h; hi += layerh)
-        heights.emplace_back(hi);
-    
-    base_plate(mesh, output, heights, thrfn);
-}
-
-Contour3D create_base_pool(const Polygons &ground_layer, 
-                           const ExPolygons &obj_self_pad = {},
-                           const PoolConfig& cfg = PoolConfig()) 
-{
-    // for debugging:
-    // Benchmark bench;
-    // bench.start();
-
-    double mergedist = 2*(1.8*cfg.min_wall_thickness_mm + 4*cfg.edge_radius_mm)+
-                       cfg.max_merge_distance_mm;
-
-    // Here we get the base polygon from which the pad has to be generated.
-    // We create an artificial concave hull from this polygon and that will
-    // serve as the bottom plate of the pad. We will offset this concave hull
-    // and then offset back the result with clipper with rounding edges ON. This
-    // trick will create a nice rounded pad shape.
-    Polygons concavehs = concave_hull(ground_layer, mergedist, cfg.throw_on_cancel);
-
-    const double thickness      = cfg.min_wall_thickness_mm;
-    const double wingheight     = cfg.min_wall_height_mm;
-    const double fullheight     = wingheight + thickness;
-    const double slope          = cfg.wall_slope;
-    const double wingdist       = wingheight / std::tan(slope);
-    const double bottom_offs    = (thickness + wingheight) / std::tan(slope);
-
-    // scaled values
-    const coord_t s_thickness   = scaled(thickness);
-    const coord_t s_eradius     = scaled(cfg.edge_radius_mm);
-    const coord_t s_safety_dist = 2*s_eradius + coord_t(0.8*s_thickness);
-    const coord_t s_wingdist    = scaled(wingdist);
-    const coord_t s_bottom_offs = scaled(bottom_offs);
-
-    auto& thrcl = cfg.throw_on_cancel;
-
-    Contour3D pool;
-
-    for(Polygon& concaveh : concavehs) {
-        if(concaveh.points.empty()) return pool;
-
-        // Here lies the trick that does the smoothing only with clipper offset
-        // calls. The offset is configured to round edges. Inner edges will
-        // be rounded because we offset twice: ones to get the outer (top) plate
-        // and again to get the inner (bottom) plate
-        auto outer_base = concaveh;
-        offset(outer_base, s_safety_dist + s_wingdist + s_thickness);
-
-        ExPolygon bottom_poly; bottom_poly.contour = outer_base;
-        offset(bottom_poly, -s_bottom_offs);
-
-        // Punching a hole in the top plate for the cavity
-        ExPolygon top_poly;
-        ExPolygon middle_base;
-        ExPolygon inner_base;
-        top_poly.contour = outer_base;
-
-        if(wingheight > 0) {
-            inner_base.contour = outer_base;
-            offset(inner_base, -(s_thickness + s_wingdist + s_eradius));
-
-            middle_base.contour = outer_base;
-            offset(middle_base, -s_thickness);
-            top_poly.holes.emplace_back(middle_base.contour);
-            auto& tph = top_poly.holes.back().points;
-            std::reverse(tph.begin(), tph.end());
-        }
-
-        ExPolygon ob; ob.contour = outer_base; double wh = 0;
-
-        // now we will calculate the angle or portion of the circle from
-        // pi/2 that will connect perfectly with the bottom plate.
-        // this is a tangent point calculation problem and the equation can
-        // be found for example here:
-        // http://www.ambrsoft.com/TrigoCalc/Circles2/CirclePoint/CirclePointDistance.htm
-        // the y coordinate would be:
-        // y = cy + (r^2*py - r*px*sqrt(px^2 + py^2 - r^2) / (px^2 + py^2)
-        // where px and py are the coordinates of the point outside the circle
-        // cx and cy are the circle center, r is the radius
-        // We place the circle center to (0, 0) in the calculation the make
-        // things easier.
-        // to get the angle we use arcsin function and subtract 90 degrees then
-        // flip the sign to get the right input to the round_edge function.
-        double r = cfg.edge_radius_mm;
-        double cy = 0;
-        double cx = 0;
-        double px = thickness + wingdist;
-        double py = r - fullheight;
-
-        double pxcx = px - cx;
-        double pycy = py - cy;
-        double b_2 = pxcx*pxcx + pycy*pycy;
-        double r_2 = r*r;
-        double D = std::sqrt(b_2 - r_2);
-        double vy = (r_2*pycy - r*pxcx*D) / b_2;
-        double phi = -(std::asin(vy/r) * 180 / PI - 90);
-
-
-        // Generate the smoothed edge geometry
-        if(s_eradius > 0) pool.merge(round_edges(ob,
-                               r,
-                               phi,
-                               0,    // z position of the input plane
-                               true,
-                               thrcl,
-                               ob, wh));
-
-        // Now that we have the rounded edge connecting the top plate with
-        // the outer side walls, we can generate and merge the sidewall geometry
-        pool.merge(walls(ob.contour, bottom_poly.contour, wh, -fullheight,
-                         bottom_offs, thrcl));
-
-        if(wingheight > 0) {
-            // Generate the smoothed edge geometry
-            wh = 0;
-            ob = middle_base;
-            if(s_eradius) pool.merge(round_edges(middle_base,
-                                   r,
-                                   phi - 90, // from tangent lines
-                                   0,  // z position of the input plane
-                                   false,
-                                   thrcl,
-                                   ob, wh));
-
-            // Next is the cavity walls connecting to the top plate's
-            // artificially created hole.
-            pool.merge(walls(inner_base.contour, ob.contour, -wingheight,
-                             wh, -wingdist, thrcl));
-        }
-
-        if (cfg.embed_object) {
-            ExPolygons bttms = diff_ex(to_polygons(bottom_poly),
-                                       to_polygons(obj_self_pad));
-            
-            assert(!bttms.empty());
-            
-            std::sort(bttms.begin(), bttms.end(),
-                      [](const ExPolygon& e1, const ExPolygon& e2) {
-                          return e1.contour.area() > e2.contour.area();
-                      });
-            
-            if(wingheight > 0) inner_base.holes = bttms.front().holes;
-            else top_poly.holes = bttms.front().holes;
-
-            auto straight_walls =
-                [&pool](const Polygon &cntr, coord_t z_low, coord_t z_high) {
-                    
-                auto lines = cntr.lines();
-                
-                for (auto &l : lines) {
-                    auto s = coord_t(pool.points.size());
-                    auto& pts = pool.points;
-                    pts.emplace_back(unscale(l.a.x(), l.a.y(), z_low));
-                    pts.emplace_back(unscale(l.b.x(), l.b.y(), z_low));
-                    pts.emplace_back(unscale(l.a.x(), l.a.y(), z_high));
-                    pts.emplace_back(unscale(l.b.x(), l.b.y(), z_high));
-                    
-                    pool.indices.emplace_back(s, s + 1, s + 3);
-                    pool.indices.emplace_back(s, s + 3, s + 2);
-                }
-            };
-            
-            coord_t z_lo = -scaled(fullheight), z_hi = -scaled(wingheight);
-            for (ExPolygon &ep : bttms) {
-                pool.merge(triangulate_expolygon_3d(ep, -fullheight, true));
-                for (auto &h : ep.holes) straight_walls(h, z_lo, z_hi);
-            }
-            
-            // Skip the outer contour, triangulate the holes
-            for (auto it = std::next(bttms.begin()); it != bttms.end(); ++it) {
-                pool.merge(triangulate_expolygon_3d(*it, -wingheight));
-                straight_walls(it->contour, z_lo, z_hi);
-            }
-            
-        } else {
-            // Now we need to triangulate the top and bottom plates as well as
-            // the cavity bottom plate which is the same as the bottom plate
-            // but it is elevated by the thickness.
-            
-            pool.merge(triangulate_expolygon_3d(bottom_poly, -fullheight, true));
-        }
-        
-        pool.merge(triangulate_expolygon_3d(top_poly));
-
-        if(wingheight > 0)
-            pool.merge(triangulate_expolygon_3d(inner_base, -wingheight));
-
-    }
-    
-    return pool;
-}
-
-void create_base_pool(const Polygons &ground_layer, TriangleMesh& out,
-                      const ExPolygons &holes, const PoolConfig& cfg)
-{
-    
-
-    // For debugging:
-    // bench.stop();
-    // std::cout << "Pad creation time: " << bench.getElapsedSec() << std::endl;
-    // std::fstream fout("pad_debug.obj", std::fstream::out);
-    // if(fout.good()) pool.to_obj(fout);
-
-    out.merge(mesh(create_base_pool(ground_layer, holes, cfg)));
-}
-
-}
-}
diff --git a/src/libslic3r/SLA/SLABasePool.hpp b/src/libslic3r/SLA/SLABasePool.hpp
deleted file mode 100644
index eec426bbf..000000000
--- a/src/libslic3r/SLA/SLABasePool.hpp
+++ /dev/null
@@ -1,92 +0,0 @@
-#ifndef SLABASEPOOL_HPP
-#define SLABASEPOOL_HPP
-
-#include <vector>
-#include <functional>
-#include <cmath>
-
-namespace Slic3r {
-
-class ExPolygon;
-class Polygon;
-using ExPolygons = std::vector<ExPolygon>;
-using Polygons = std::vector<Polygon>;
-
-class TriangleMesh;
-
-namespace sla {
-
-using ThrowOnCancel = std::function<void(void)>;
-
-/// Calculate the polygon representing the silhouette from the specified height
-void base_plate(const TriangleMesh& mesh,       // input mesh
-                ExPolygons& output,             // Output will be merged with
-                float samplingheight = 0.1f,    // The height range to sample
-                float layerheight = 0.05f,      // The sampling height
-                ThrowOnCancel thrfn = [](){});  // Will be called frequently
-
-void base_plate(const TriangleMesh& mesh,       // input mesh
-                ExPolygons& output,             // Output will be merged with
-                const std::vector<float>&,      // Exact Z levels to sample
-                ThrowOnCancel thrfn = [](){});  // Will be called frequently
-
-// Function to cut tiny connector cavities for a given polygon. The input poly
-// will be offsetted by "padding" and small rectangle shaped cavities will be
-// inserted along the perimeter in every "stride" distance. The stick rectangles
-// will have a with about "stick_width". The input dimensions are in world 
-// measure, not the scaled clipper units.
-void breakstick_holes(ExPolygon &poly,
-                      double     padding,
-                      double     stride,
-                      double     stick_width,
-                      double     penetration = 0.0);
-
-Polygons concave_hull(const Polygons& polys, double max_dist_mm = 50,
-                      ThrowOnCancel throw_on_cancel = [](){});
-
-struct PoolConfig {
-    double min_wall_thickness_mm = 2;
-    double min_wall_height_mm = 5;
-    double max_merge_distance_mm = 50;
-    double edge_radius_mm = 1;
-    double wall_slope = std::atan(1.0);          // Universal constant for Pi/4
-    struct EmbedObject {
-        double object_gap_mm = 0.5;
-        double stick_stride_mm = 10;
-        double stick_width_mm = 0.3;
-        double stick_penetration_mm = 0.1;
-        bool enabled = false;
-        operator bool() const { return enabled; }
-    } embed_object;
-
-    ThrowOnCancel throw_on_cancel = [](){};
-
-    inline PoolConfig() {}
-    inline PoolConfig(double wt, double wh, double md, double er, double slope):
-        min_wall_thickness_mm(wt),
-        min_wall_height_mm(wh),
-        max_merge_distance_mm(md),
-        edge_radius_mm(er),
-        wall_slope(slope) {}
-};
-
-/// Calculate the pool for the mesh for SLA printing
-void create_base_pool(const Polygons& base_plate,
-                      TriangleMesh& output_mesh,
-                      const ExPolygons& holes,
-                      const PoolConfig& = PoolConfig());
-
-/// Returns the elevation needed for compensating the pad.
-inline double get_pad_elevation(const PoolConfig& cfg) {
-    return cfg.min_wall_thickness_mm;
-}
-
-inline double get_pad_fullheight(const PoolConfig& cfg) {
-    return cfg.min_wall_height_mm + cfg.min_wall_thickness_mm;
-}
-
-}
-
-}
-
-#endif // SLABASEPOOL_HPP
diff --git a/src/libslic3r/SLA/SLABoilerPlate.hpp b/src/libslic3r/SLA/SLABoilerPlate.hpp
index 86e90f3b7..d7ce26bb2 100644
--- a/src/libslic3r/SLA/SLABoilerPlate.hpp
+++ b/src/libslic3r/SLA/SLABoilerPlate.hpp
@@ -8,35 +8,19 @@
 #include <libslic3r/ExPolygon.hpp>
 #include <libslic3r/TriangleMesh.hpp>
 
+#include "SLACommon.hpp"
+#include "SLASpatIndex.hpp"
+
 namespace Slic3r {
 namespace sla {
 
-/// Get x and y coordinates (because we are eigenizing...)
-inline coord_t x(const Point& p) { return p(0); }
-inline coord_t y(const Point& p) { return p(1); }
-inline coord_t& x(Point& p) { return p(0); }
-inline coord_t& y(Point& p) { return p(1); }
-
-inline coordf_t x(const Vec3d& p) { return p(0); }
-inline coordf_t y(const Vec3d& p) { return p(1); }
-inline coordf_t z(const Vec3d& p) { return p(2); }
-inline coordf_t& x(Vec3d& p) { return p(0); }
-inline coordf_t& y(Vec3d& p) { return p(1); }
-inline coordf_t& z(Vec3d& p) { return p(2); }
-
-inline coord_t& x(Vec3crd& p) { return p(0); }
-inline coord_t& y(Vec3crd& p) { return p(1); }
-inline coord_t& z(Vec3crd& p) { return p(2); }
-inline coord_t x(const Vec3crd& p) { return p(0); }
-inline coord_t y(const Vec3crd& p) { return p(1); }
-inline coord_t z(const Vec3crd& p) { return p(2); }
-
 /// Intermediate struct for a 3D mesh
 struct Contour3D {
     Pointf3s points;
     std::vector<Vec3i> indices;
 
-    void merge(const Contour3D& ctr) {
+    Contour3D& merge(const Contour3D& ctr)
+    {
         auto s3 = coord_t(points.size());
         auto s = indices.size();
 
@@ -44,21 +28,27 @@ struct Contour3D {
         indices.insert(indices.end(), ctr.indices.begin(), ctr.indices.end());
 
         for(size_t n = s; n < indices.size(); n++) {
-            auto& idx = indices[n]; x(idx) += s3; y(idx) += s3; z(idx) += s3;
+            auto& idx = indices[n]; idx.x() += s3; idx.y() += s3; idx.z() += s3;
         }
+        
+        return *this;
     }
 
-    void merge(const Pointf3s& triangles) {
+    Contour3D& merge(const Pointf3s& triangles)
+    {
         const size_t offs = points.size();
         points.insert(points.end(), triangles.begin(), triangles.end());
         indices.reserve(indices.size() + points.size() / 3);
-
-        for(int i = (int)offs; i < (int)points.size(); i += 3)
+        
+        for(int i = int(offs); i < int(points.size()); i += 3)
             indices.emplace_back(i, i + 1, i + 2);
+        
+        return *this;
     }
 
     // Write the index triangle structure to OBJ file for debugging purposes.
-    void to_obj(std::ostream& stream) {
+    void to_obj(std::ostream& stream)
+    {
         for(auto& p : points) {
             stream << "v " << p.transpose() << "\n";
         }
@@ -72,6 +62,31 @@ struct Contour3D {
 using ClusterEl = std::vector<unsigned>;
 using ClusteredPoints = std::vector<ClusterEl>;
 
+// Clustering a set of points by the given distance.
+ClusteredPoints cluster(const std::vector<unsigned>& indices,
+                        std::function<Vec3d(unsigned)> pointfn,
+                        double dist,
+                        unsigned max_points);
+
+ClusteredPoints cluster(const PointSet& points,
+                        double dist,
+                        unsigned max_points);
+
+ClusteredPoints cluster(
+    const std::vector<unsigned>& indices,
+    std::function<Vec3d(unsigned)> pointfn,
+    std::function<bool(const PointIndexEl&, const PointIndexEl&)> predicate,
+    unsigned max_points);
+
+
+// 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& mesh,
+                 double eps = 0.05,  // min distance from edges
+                 std::function<void()> throw_on_cancel = [](){},
+                 const std::vector<unsigned>& selected_points = {});
+
 /// Mesh from an existing contour.
 inline TriangleMesh mesh(const Contour3D& ctour) {
     return {ctour.points, ctour.indices};
diff --git a/src/libslic3r/SLA/SLACommon.hpp b/src/libslic3r/SLA/SLACommon.hpp
index d8e258035..97b459676 100644
--- a/src/libslic3r/SLA/SLACommon.hpp
+++ b/src/libslic3r/SLA/SLACommon.hpp
@@ -1,8 +1,9 @@
 #ifndef SLACOMMON_HPP
 #define SLACOMMON_HPP
 
-#include <Eigen/Geometry>
 #include <memory>
+#include <vector>
+#include <Eigen/Geometry>
 
 // #define SLIC3R_SLA_NEEDS_WINDTREE
 
@@ -69,6 +70,8 @@ struct SupportPoint
     }
 };
 
+using SupportPoints = std::vector<SupportPoint>;
+
 /// An index-triangle structure for libIGL functions. Also serves as an
 /// alternative (raw) input format for the SLASupportTree
 class EigenMesh3D {
@@ -175,6 +178,8 @@ public:
     }
 };
 
+using PointSet = Eigen::MatrixXd;
+
 } // namespace sla
 } // namespace Slic3r
 
diff --git a/src/libslic3r/SLA/SLAConcurrency.hpp b/src/libslic3r/SLA/SLAConcurrency.hpp
new file mode 100644
index 000000000..4beb2aead
--- /dev/null
+++ b/src/libslic3r/SLA/SLAConcurrency.hpp
@@ -0,0 +1,56 @@
+#ifndef SLACONCURRENCY_H
+#define SLACONCURRENCY_H
+
+#include <tbb/spin_mutex.h>
+#include <tbb/mutex.h>
+#include <tbb/parallel_for.h>
+
+namespace Slic3r {
+namespace sla {
+
+// Set this to true to enable full parallelism in this module.
+// Only the well tested parts will be concurrent if this is set to false.
+const constexpr bool USE_FULL_CONCURRENCY = true;
+
+template<bool> struct _ccr {};
+
+template<> struct _ccr<true>
+{
+    using SpinningMutex = tbb::spin_mutex;
+    using BlockingMutex  = tbb::mutex;
+    
+    template<class It, class Fn>
+    static inline void enumerate(It from, It to, Fn fn)
+    {
+        auto   iN = to - from;
+        size_t N  = iN < 0 ? 0 : size_t(iN);
+        
+        tbb::parallel_for(size_t(0), N, [from, fn](size_t n) {
+            fn(*(from + decltype(iN)(n)), n);
+        });
+    }
+};
+
+template<> struct _ccr<false>
+{
+private:
+    struct _Mtx { inline void lock() {} inline void unlock() {} };
+    
+public:
+    using SpinningMutex = _Mtx;
+    using BlockingMutex = _Mtx;
+    
+    template<class It, class Fn>
+    static inline void enumerate(It from, It to, Fn fn)
+    {
+        for (auto it = from; it != to; ++it) fn(*it, size_t(it - from));
+    }
+};
+
+using ccr = _ccr<USE_FULL_CONCURRENCY>;
+using ccr_seq = _ccr<false>;
+using ccr_par = _ccr<true>;
+
+}} // namespace Slic3r::sla
+
+#endif // SLACONCURRENCY_H
diff --git a/src/libslic3r/SLA/SLAPad.cpp b/src/libslic3r/SLA/SLAPad.cpp
new file mode 100644
index 000000000..71f8b1c7f
--- /dev/null
+++ b/src/libslic3r/SLA/SLAPad.cpp
@@ -0,0 +1,870 @@
+#include "SLAPad.hpp"
+#include "SLABoilerPlate.hpp"
+#include "SLASpatIndex.hpp"
+
+#include "boost/log/trivial.hpp"
+#include "SLABoostAdapter.hpp"
+#include "ClipperUtils.hpp"
+#include "Tesselate.hpp"
+#include "MTUtils.hpp"
+
+// For debugging:
+// #include <fstream>
+// #include <libnest2d/tools/benchmark.h>
+#include "SVG.hpp"
+
+#include "I18N.hpp"
+#include <boost/log/trivial.hpp>
+
+//! macro used to mark string used at localization,
+//! return same string
+#define L(s) Slic3r::I18N::translate(s)
+
+namespace Slic3r { namespace sla {
+
+namespace {
+
+/// This function will return a triangulation of a sheet connecting an upper
+/// and a lower plate given as input polygons. It will not triangulate the
+/// plates themselves only the sheet. The caller has to specify the lower and
+/// upper z levels in world coordinates as well as the offset difference
+/// between the sheets. If the lower_z_mm is higher than upper_z_mm or the
+/// offset difference is negative, the resulting triangle orientation will be
+/// reversed.
+///
+/// IMPORTANT: This is not a universal triangulation algorithm. It assumes
+/// that the lower and upper polygons are offsetted versions of the same
+/// original polygon. In general, it assumes that one of the polygons is
+/// completely inside the other. The offset difference is the reference
+/// distance from the inner polygon's perimeter to the outer polygon's
+/// perimeter. The real distance will be variable as the clipper offset has
+/// different strategies (rounding, etc...). This algorithm should have
+/// O(2n + 3m) complexity where n is the number of upper vertices and m is the
+/// number of lower vertices.
+Contour3D walls(
+    const Polygon &lower,
+    const Polygon &upper,
+    double         lower_z_mm,
+    double         upper_z_mm,
+    double         offset_difference_mm,
+    ThrowOnCancel  thr = [] {})
+{
+    Contour3D ret;
+
+    if(upper.points.size() < 3 || lower.size() < 3) return ret;
+
+    // The concept of the algorithm is relatively simple. It will try to find
+    // the closest vertices from the upper and the lower polygon and use those
+    // as starting points. Then it will create the triangles sequentially using
+    // an edge from the upper polygon and a vertex from the lower or vice versa,
+    // depending on the resulting triangle's quality.
+    // The quality is measured by a scalar value. So far it looks like it is
+    // enough to derive it from the slope of the triangle's two edges connecting
+    // the upper and the lower part. A reference slope is calculated from the
+    // height and the offset difference.
+
+    // Offset in the index array for the ceiling
+    const auto offs = upper.points.size();
+
+    // Shorthand for the vertex arrays
+    auto& upts = upper.points, &lpts = lower.points;
+    auto& rpts = ret.points; auto& ind = ret.indices;
+
+    // If the Z levels are flipped, or the offset difference is negative, we
+    // will interpret that as the triangles normals should be inverted.
+    bool inverted = upper_z_mm < lower_z_mm || offset_difference_mm < 0;
+
+    // Copy the points into the mesh, convert them from 2D to 3D
+    rpts.reserve(upts.size() + lpts.size());
+    ind.reserve(2 * upts.size() + 2 * lpts.size());
+    for (auto &p : upts)
+        rpts.emplace_back(unscaled(p.x()), unscaled(p.y()), upper_z_mm);
+    for (auto &p : lpts)
+        rpts.emplace_back(unscaled(p.x()), unscaled(p.y()), lower_z_mm);
+
+    // Create pointing indices into vertex arrays. u-upper, l-lower
+    size_t uidx = 0, lidx = offs, unextidx = 1, lnextidx = offs + 1;
+
+    // Simple squared distance calculation.
+    auto distfn = [](const Vec3d& p1, const Vec3d& p2) {
+        auto p = p1 - p2; return p.transpose() * p;
+    };
+
+    // We need to find the closest point on lower polygon to the first point on
+    // the upper polygon. These will be our starting points.
+    double distmin = std::numeric_limits<double>::max();
+    for(size_t l = lidx; l < rpts.size(); ++l) {
+        thr();
+        double d = distfn(rpts[l], rpts[uidx]);
+        if(d < distmin) { lidx = l; distmin = d; }
+    }
+
+    // Set up lnextidx to be ahead of lidx in cyclic mode
+    lnextidx = lidx + 1;
+    if(lnextidx == rpts.size()) lnextidx = offs;
+
+    // This will be the flip switch to toggle between upper and lower triangle
+    // creation mode
+    enum class Proceed {
+        UPPER, // A segment from the upper polygon and one vertex from the lower
+        LOWER  // A segment from the lower polygon and one vertex from the upper
+    } proceed = Proceed::UPPER;
+
+    // Flags to help evaluating loop termination.
+    bool ustarted = false, lstarted = false;
+
+    // The variables for the fitness values, one for the actual and one for the
+    // previous.
+    double current_fit = 0, prev_fit = 0;
+
+    // Every triangle of the wall has two edges connecting the upper plate with
+    // the lower plate. From the length of these two edges and the zdiff we
+    // can calculate the momentary squared offset distance at a particular
+    // position on the wall. The average of the differences from the reference
+    // (squared) offset distance will give us the driving fitness value.
+    const double offsdiff2 = std::pow(offset_difference_mm, 2);
+    const double zdiff2 = std::pow(upper_z_mm - lower_z_mm, 2);
+
+    // Mark the current vertex iterator positions. If the iterators return to
+    // the same position, the loop can be terminated.
+    size_t uendidx = uidx, lendidx = lidx;
+
+    do { thr();  // check throw if canceled
+
+        prev_fit = current_fit;
+
+        switch(proceed) {   // proceed depending on the current state
+        case Proceed::UPPER:
+            if(!ustarted || uidx != uendidx) { // there are vertices remaining
+                // Get the 3D vertices in order
+                const Vec3d& p_up1 = rpts[uidx];
+                const Vec3d& p_low = rpts[lidx];
+                const Vec3d& p_up2 = rpts[unextidx];
+
+                // Calculate fitness: the average of the two connecting edges
+                double a = offsdiff2 - (distfn(p_up1, p_low) - zdiff2);
+                double b = offsdiff2 - (distfn(p_up2, p_low) - zdiff2);
+                current_fit = (std::abs(a) + std::abs(b)) / 2;
+
+                if(current_fit > prev_fit) { // fit is worse than previously
+                    proceed = Proceed::LOWER;
+                } else {    // good to go, create the triangle
+                    inverted
+                        ? ind.emplace_back(int(unextidx), int(lidx), int(uidx))
+                        : ind.emplace_back(int(uidx), int(lidx), int(unextidx));
+
+                    // Increment the iterators, rotate if necessary
+                    ++uidx; ++unextidx;
+                    if(unextidx == offs) unextidx = 0;
+                    if(uidx == offs) uidx = 0;
+
+                    ustarted = true;    // mark the movement of the iterators
+                    // so that the comparison to uendidx can be made correctly
+                }
+            } else proceed = Proceed::LOWER;
+
+            break;
+        case Proceed::LOWER:
+            // Mode with lower segment, upper vertex. Same structure:
+            if(!lstarted || lidx != lendidx) {
+                const Vec3d& p_low1 = rpts[lidx];
+                const Vec3d& p_low2 = rpts[lnextidx];
+                const Vec3d& p_up   = rpts[uidx];
+
+                double a = offsdiff2 - (distfn(p_up, p_low1) - zdiff2);
+                double b = offsdiff2 - (distfn(p_up, p_low2) - zdiff2);
+                current_fit = (std::abs(a) + std::abs(b)) / 2;
+
+                if(current_fit > prev_fit) {
+                    proceed = Proceed::UPPER;
+                } else {
+                    inverted
+                        ? ind.emplace_back(int(uidx), int(lnextidx), int(lidx))
+                        : ind.emplace_back(int(lidx), int(lnextidx), int(uidx));
+
+                    ++lidx; ++lnextidx;
+                    if(lnextidx == rpts.size()) lnextidx = offs;
+                    if(lidx == rpts.size()) lidx = offs;
+
+                    lstarted = true;
+                }
+            } else proceed = Proceed::UPPER;
+
+            break;
+        } // end of switch
+    } while(!ustarted || !lstarted || uidx != uendidx || lidx != lendidx);
+
+    return ret;
+}
+
+// Same as walls() but with identical higher and lower polygons.
+Contour3D inline straight_walls(const Polygon &plate,
+                                double         lo_z,
+                                double         hi_z,
+                                ThrowOnCancel  thr)
+{
+    return walls(plate, plate, lo_z, hi_z, .0 /*offset_diff*/, thr);
+}
+
+// As it shows, the current offset_ex in ClipperUtils hangs if used in jtRound
+// mode
+ClipperLib::Paths fast_offset(const ClipperLib::Paths &paths,
+                              coord_t                  delta,
+                              ClipperLib::JoinType     jointype)
+{
+    using ClipperLib::ClipperOffset;
+    using ClipperLib::etClosedPolygon;
+    using ClipperLib::Paths;
+    using ClipperLib::Path;
+
+    ClipperOffset offs;
+    offs.ArcTolerance = scaled<double>(0.01);
+
+    for (auto &p : paths)
+        // If the input is not at least a triangle, we can not do this algorithm
+        if(p.size() < 3) {
+            BOOST_LOG_TRIVIAL(error) << "Invalid geometry for offsetting!";
+            return {};
+        }
+
+    offs.AddPaths(paths, jointype, etClosedPolygon);
+
+    Paths result;
+    offs.Execute(result, static_cast<double>(delta));
+
+    return result;
+}
+
+
+// Function to cut tiny connector cavities for a given polygon. The input poly
+// will be offsetted by "padding" and small rectangle shaped cavities will be
+// inserted along the perimeter in every "stride" distance. The stick rectangles
+// will have a with about "stick_width". The input dimensions are in world
+// measure, not the scaled clipper units.
+void breakstick_holes(Points& pts,
+                      double padding,
+                      double stride,
+                      double stick_width,
+                      double penetration)
+{
+    if(stride <= EPSILON || stick_width <= EPSILON || padding <= EPSILON)
+        return;
+
+    // SVG svg("bridgestick_plate.svg");
+    // svg.draw(poly);
+
+    // The connector stick will be a small rectangle with dimensions
+    // stick_width x (penetration + padding) to have some penetration
+    // into the input polygon.
+
+    Points out;
+    out.reserve(2 * pts.size()); // output polygon points
+
+    // stick bottom and right edge dimensions
+    double sbottom = scaled(stick_width);
+    double sright  = scaled(penetration + padding);
+
+    // scaled stride distance
+    double sstride = scaled(stride);
+    double t       = 0;
+
+    // process pairs of vertices as an edge, start with the last and
+    // first point
+    for (size_t i = pts.size() - 1, j = 0; j < pts.size(); i = j, ++j) {
+        // Get vertices and the direction vectors
+        const Point &a = pts[i], &b = pts[j];
+        Vec2d        dir = b.cast<double>() - a.cast<double>();
+        double       nrm = dir.norm();
+        dir /= nrm;
+        Vec2d dirp(-dir(Y), dir(X));
+
+        // Insert start point
+        out.emplace_back(a);
+
+        // dodge the start point, do not make sticks on the joins
+        while (t < sbottom) t += sbottom;
+        double tend = nrm - sbottom;
+
+        while (t < tend) { // insert the stick on the polygon perimeter
+
+            // calculate the stick rectangle vertices and insert them
+            // into the output.
+            Point p1 = a + (t * dir).cast<coord_t>();
+            Point p2 = p1 + (sright * dirp).cast<coord_t>();
+            Point p3 = p2 + (sbottom * dir).cast<coord_t>();
+            Point p4 = p3 + (sright * -dirp).cast<coord_t>();
+            out.insert(out.end(), {p1, p2, p3, p4});
+
+            // continue along the perimeter
+            t += sstride;
+        }
+
+        t = t - nrm;
+
+        // Insert edge endpoint
+        out.emplace_back(b);
+    }
+
+    // move the new points
+    out.shrink_to_fit();
+    pts.swap(out);
+}
+
+template<class...Args>
+ExPolygons breakstick_holes(const ExPolygons &input, Args...args)
+{
+    ExPolygons ret = input;
+    for (ExPolygon &p : ret) {
+        breakstick_holes(p.contour.points, args...);
+        for (auto &h : p.holes) breakstick_holes(h.points, args...);
+    }
+
+    return ret;
+}
+
+/// A fake concave hull that is constructed by connecting separate shapes
+/// with explicit bridges. Bridges are generated from each shape's centroid
+/// to the center of the "scene" which is the centroid calculated from the shape
+/// centroids (a star is created...)
+class ConcaveHull {
+    Polygons m_polys;
+
+    Point centroid(const Points& pp) const
+    {
+        Point c;
+        switch(pp.size()) {
+        case 0: break;
+        case 1: c = pp.front(); break;
+        case 2: c = (pp[0] + pp[1]) / 2; break;
+        default: {
+            auto MAX = std::numeric_limits<Point::coord_type>::max();
+            auto MIN = std::numeric_limits<Point::coord_type>::min();
+            Point min = {MAX, MAX}, max = {MIN, MIN};
+
+            for(auto& p : pp) {
+                if(p(0) < min(0)) min(0) = p(0);
+                if(p(1) < min(1)) min(1) = p(1);
+                if(p(0) > max(0)) max(0) = p(0);
+                if(p(1) > max(1)) max(1) = p(1);
+            }
+            c(0) = min(0) + (max(0) - min(0)) / 2;
+            c(1) = min(1) + (max(1) - min(1)) / 2;
+            break;
+        }
+        }
+
+        return c;
+    }
+
+    inline Point centroid(const Polygon &poly) const { return poly.centroid(); }
+
+    Points calculate_centroids() const
+    {
+        // We get the centroids of all the islands in the 2D slice
+        Points centroids = reserve_vector<Point>(m_polys.size());
+        std::transform(m_polys.begin(), m_polys.end(),
+                       std::back_inserter(centroids),
+                       [this](const Polygon &poly) { return centroid(poly); });
+
+        return centroids;
+    }
+
+    void merge_polygons() { m_polys = union_(m_polys); }
+
+    void add_connector_rectangles(const Points &centroids,
+                                  coord_t       max_dist,
+                                  ThrowOnCancel thr)
+    {
+        namespace bgi = boost::geometry::index;
+        using PointIndexElement = std::pair<Point, unsigned>;
+        using PointIndex = bgi::rtree<PointIndexElement, bgi::rstar<16, 4>>;
+
+        // Centroid of the centroids of islands. This is where the additional
+        // connector sticks are routed.
+        Point cc = centroid(centroids);
+
+        PointIndex ctrindex;
+        unsigned  idx = 0;
+        for(const Point &ct : centroids)
+            ctrindex.insert(std::make_pair(ct, idx++));
+
+        m_polys.reserve(m_polys.size() + centroids.size());
+
+        idx = 0;
+        for (const Point &c : centroids) {
+            thr();
+
+            double dx = c.x() - cc.x(), dy = c.y() - cc.y();
+            double l  = std::sqrt(dx * dx + dy * dy);
+            double nx = dx / l, ny = dy / l;
+
+            const Point &ct = centroids[idx];
+
+            std::vector<PointIndexElement> result;
+            ctrindex.query(bgi::nearest(ct, 2), std::back_inserter(result));
+
+            double dist = max_dist;
+            for (const PointIndexElement &el : result)
+                if (el.second != idx) {
+                    dist = Line(el.first, ct).length();
+                    break;
+                }
+
+            idx++;
+
+            if (dist >= max_dist) return;
+
+            Polygon r;
+            r.points.reserve(3);
+            r.points.emplace_back(cc);
+
+            Point d(scaled(nx), scaled(ny));
+            r.points.emplace_back(c + Point(-d.y(), d.x()));
+            r.points.emplace_back(c + Point(d.y(), -d.x()));
+            offset(r, scaled<float>(1.));
+
+            m_polys.emplace_back(r);
+        }
+    }
+
+public:
+
+    ConcaveHull(const ExPolygons& polys, double merge_dist, ThrowOnCancel thr)
+        : ConcaveHull{to_polygons(polys), merge_dist, thr} {}
+
+    ConcaveHull(const Polygons& polys, double mergedist, ThrowOnCancel thr)
+    {
+        if(polys.empty()) return;
+
+        m_polys = polys;
+        merge_polygons();
+
+        if(m_polys.size() == 1) return;
+
+        Points centroids = calculate_centroids();
+
+        add_connector_rectangles(centroids, scaled(mergedist), thr);
+
+        merge_polygons();
+    }
+
+    // const Polygons & polygons() const { return m_polys; }
+
+    ExPolygons to_expolygons() const
+    {
+        auto ret = reserve_vector<ExPolygon>(m_polys.size());
+        for (const Polygon &p : m_polys) ret.emplace_back(ExPolygon(p));
+        return ret;
+    }
+
+    void offset_waffle_style(coord_t delta) {
+        ClipperLib::Paths paths = Slic3rMultiPoints_to_ClipperPaths(m_polys);
+        paths = fast_offset(paths, 2 * delta, ClipperLib::jtRound);
+        paths = fast_offset(paths, -delta, ClipperLib::jtRound);
+        m_polys = ClipperPaths_to_Slic3rPolygons(paths);
+    }
+
+    static inline coord_t get_waffle_offset(const PadConfig &c)
+    {
+        return scaled(c.brim_size_mm + c.wing_distance());
+    }
+
+    static inline double get_merge_distance(const PadConfig &c)
+    {
+        return 2. * (1.8 * c.wall_thickness_mm) + c.max_merge_dist_mm;
+    }
+};
+
+// Part of the pad configuration that is used for 3D geometry generation
+struct PadConfig3D {
+    double thickness, height, wing_height, slope;
+
+    explicit PadConfig3D(const PadConfig &cfg2d)
+        : thickness{cfg2d.wall_thickness_mm}
+        , height{cfg2d.full_height()}
+        , wing_height{cfg2d.wall_height_mm}
+        , slope{cfg2d.wall_slope}
+    {}
+
+    inline double bottom_offset() const
+    {
+        return (thickness + wing_height) / std::tan(slope);
+    }
+};
+
+// Outer part of the skeleton is used to generate the waffled edges of the pad.
+// Inner parts will not be waffled or offsetted. Inner parts are only used if
+// pad is generated around the object and correspond to holes and inner polygons
+// in the model blueprint.
+struct PadSkeleton { ExPolygons inner, outer; };
+
+PadSkeleton divide_blueprint(const ExPolygons &bp)
+{
+    ClipperLib::PolyTree ptree = union_pt(bp);
+
+    PadSkeleton ret;
+    ret.inner.reserve(size_t(ptree.Total()));
+    ret.outer.reserve(size_t(ptree.Total()));
+
+    for (ClipperLib::PolyTree::PolyNode *node : ptree.Childs) {
+        ExPolygon poly(ClipperPath_to_Slic3rPolygon(node->Contour));
+        for (ClipperLib::PolyTree::PolyNode *child : node->Childs) {
+            if (child->IsHole()) {
+                poly.holes.emplace_back(
+                    ClipperPath_to_Slic3rPolygon(child->Contour));
+
+                traverse_pt_unordered(child->Childs, &ret.inner);
+            }
+            else traverse_pt_unordered(child, &ret.inner);
+        }
+
+        ret.outer.emplace_back(poly);
+    }
+
+    return ret;
+}
+
+// A helper class for storing polygons and maintaining a spatial index of their
+// bounding boxes.
+class Intersector {
+    BoxIndex       m_index;
+    ExPolygons     m_polys;
+
+public:
+
+    // Add a new polygon to the index
+    void add(const ExPolygon &ep)
+    {
+        m_polys.emplace_back(ep);
+        m_index.insert(BoundingBox{ep}, unsigned(m_index.size()));
+    }
+
+    // Check an arbitrary polygon for intersection with the indexed polygons
+    bool intersects(const ExPolygon &poly)
+    {
+        // Create a suitable query bounding box.
+        auto bb = poly.contour.bounding_box();
+
+        std::vector<BoxIndexEl> qres = m_index.query(bb, BoxIndex::qtIntersects);
+
+        // Now check intersections on the actual polygons (not just the boxes)
+        bool is_overlap = false;
+        auto qit        = qres.begin();
+        while (!is_overlap && qit != qres.end())
+            is_overlap = is_overlap || poly.overlaps(m_polys[(qit++)->second]);
+
+        return is_overlap;
+    }
+};
+
+// This dummy intersector to implement the "force pad everywhere" feature
+struct DummyIntersector
+{
+    inline void add(const ExPolygon &) {}
+    inline bool intersects(const ExPolygon &) { return true; }
+};
+
+template<class _Intersector>
+class _AroundPadSkeleton : public PadSkeleton
+{
+    // A spatial index used to be able to efficiently find intersections of
+    // support polygons with the model polygons.
+    _Intersector m_intersector;
+
+public:
+    _AroundPadSkeleton(const ExPolygons &support_blueprint,
+                       const ExPolygons &model_blueprint,
+                       const PadConfig & cfg,
+                       ThrowOnCancel     thr)
+    {
+        // We need to merge the support and the model contours in a special
+        // way in which the model contours have to be substracted from the
+        // support contours. The pad has to have a hole in which the model can
+        // fit perfectly (thus the substraction -- diff_ex). Also, the pad has
+        // to be eliminated from areas where there is no need for a pad, due
+        // to missing supports.
+
+        add_supports_to_index(support_blueprint);
+
+        auto model_bp_offs =
+            offset_ex(model_blueprint,
+                      scaled<float>(cfg.embed_object.object_gap_mm),
+                      ClipperLib::jtMiter, 1);
+
+        ConcaveHull fullcvh =
+            wafflized_concave_hull(support_blueprint, model_bp_offs, cfg, thr);
+
+        auto model_bp_sticks =
+            breakstick_holes(model_bp_offs, cfg.embed_object.object_gap_mm,
+                             cfg.embed_object.stick_stride_mm,
+                             cfg.embed_object.stick_width_mm,
+                             cfg.embed_object.stick_penetration_mm);
+
+        ExPolygons fullpad = diff_ex(fullcvh.to_expolygons(), model_bp_sticks);
+
+        remove_redundant_parts(fullpad);
+
+        PadSkeleton divided = divide_blueprint(fullpad);
+        outer = std::move(divided.outer);
+        inner = std::move(divided.inner);
+    }
+
+private:
+
+    // Add the support blueprint to the search index to be queried later
+    void add_supports_to_index(const ExPolygons &supp_bp)
+    {
+        for (auto &ep : supp_bp) m_intersector.add(ep);
+    }
+
+    // Create the wafflized pad around all object in the scene. This pad doesnt
+    // have any holes yet.
+    ConcaveHull wafflized_concave_hull(const ExPolygons &supp_bp,
+                                       const ExPolygons &model_bp,
+                                       const PadConfig  &cfg,
+                                       ThrowOnCancel     thr)
+    {
+        auto allin = reserve_vector<ExPolygon>(supp_bp.size() + model_bp.size());
+
+        for (auto &ep : supp_bp) allin.emplace_back(ep.contour);
+        for (auto &ep : model_bp) allin.emplace_back(ep.contour);
+
+        ConcaveHull ret{allin, ConcaveHull::get_merge_distance(cfg), thr};
+        ret.offset_waffle_style(ConcaveHull::get_waffle_offset(cfg));
+
+        return ret;
+    }
+
+    // To remove parts of the pad skeleton which do not host any supports
+    void remove_redundant_parts(ExPolygons &parts)
+    {
+        auto endit = std::remove_if(parts.begin(), parts.end(),
+                                    [this](const ExPolygon &p) {
+                                        return !m_intersector.intersects(p);
+                                    });
+
+        parts.erase(endit, parts.end());
+    }
+};
+
+using AroundPadSkeleton = _AroundPadSkeleton<Intersector>;
+using BrimPadSkeleton   = _AroundPadSkeleton<DummyIntersector>;
+
+class BelowPadSkeleton : public PadSkeleton
+{
+public:
+    BelowPadSkeleton(const ExPolygons &support_blueprint,
+                     const ExPolygons &model_blueprint,
+                     const PadConfig & cfg,
+                     ThrowOnCancel     thr)
+    {
+        outer.reserve(support_blueprint.size() + model_blueprint.size());
+
+        for (auto &ep : support_blueprint) outer.emplace_back(ep.contour);
+        for (auto &ep : model_blueprint) outer.emplace_back(ep.contour);
+
+        ConcaveHull ochull{outer, ConcaveHull::get_merge_distance(cfg), thr};
+
+        ochull.offset_waffle_style(ConcaveHull::get_waffle_offset(cfg));
+        outer = ochull.to_expolygons();
+    }
+};
+
+// Offset the contour only, leave the holes untouched
+template<class...Args>
+ExPolygon offset_contour_only(const ExPolygon &poly, coord_t delta, Args...args)
+{
+    ExPolygons tmp = offset_ex(poly.contour, float(delta), args...);
+
+    if (tmp.empty()) return {};
+
+    Polygons holes = poly.holes;
+    for (auto &h : holes) h.reverse();
+
+    tmp = diff_ex(to_polygons(tmp), holes);
+
+    if (tmp.empty()) return {};
+
+    return tmp.front();
+}
+
+bool add_cavity(Contour3D &pad, ExPolygon &top_poly, const PadConfig3D &cfg,
+                ThrowOnCancel thr)
+{
+    auto logerr = []{BOOST_LOG_TRIVIAL(error)<<"Could not create pad cavity";};
+
+    double    wing_distance = cfg.wing_height / std::tan(cfg.slope);
+    coord_t   delta_inner   = -scaled(cfg.thickness + wing_distance);
+    coord_t   delta_middle  = -scaled(cfg.thickness);
+    ExPolygon inner_base    = offset_contour_only(top_poly, delta_inner);
+    ExPolygon middle_base   = offset_contour_only(top_poly, delta_middle);
+
+    if (inner_base.empty() || middle_base.empty()) { logerr(); return false; }
+
+    ExPolygons pdiff = diff_ex(top_poly, middle_base.contour);
+
+    if (pdiff.size() != 1) { logerr(); return false; }
+
+    top_poly = pdiff.front();
+
+    double z_min = -cfg.wing_height, z_max = 0;
+    double offset_difference = -wing_distance;
+    pad.merge(walls(inner_base.contour, middle_base.contour, z_min, z_max,
+                    offset_difference, thr));
+
+    pad.merge(triangulate_expolygon_3d(inner_base, z_min, NORMALS_UP));
+
+    return true;
+}
+
+Contour3D create_outer_pad_geometry(const ExPolygons & skeleton,
+                                    const PadConfig3D &cfg,
+                                    ThrowOnCancel      thr)
+{
+    Contour3D ret;
+
+    for (const ExPolygon &pad_part : skeleton) {
+        ExPolygon top_poly{pad_part};
+        ExPolygon bottom_poly =
+            offset_contour_only(pad_part, -scaled(cfg.bottom_offset()));
+
+        if (bottom_poly.empty()) continue;
+
+        double z_min = -cfg.height, z_max = 0;
+        ret.merge(walls(top_poly.contour, bottom_poly.contour, z_max, z_min,
+                        cfg.bottom_offset(), thr));
+
+        if (cfg.wing_height > 0. && add_cavity(ret, top_poly, cfg, thr))
+            z_max = -cfg.wing_height;
+
+        for (auto &h : bottom_poly.holes)
+            ret.merge(straight_walls(h, z_max, z_min, thr));
+
+        ret.merge(triangulate_expolygon_3d(bottom_poly, z_min, NORMALS_DOWN));
+        ret.merge(triangulate_expolygon_3d(top_poly, NORMALS_UP));
+    }
+
+    return ret;
+}
+
+Contour3D create_inner_pad_geometry(const ExPolygons & skeleton,
+                                    const PadConfig3D &cfg,
+                                    ThrowOnCancel      thr)
+{
+    Contour3D ret;
+
+    double z_max = 0., z_min = -cfg.height;
+    for (const ExPolygon &pad_part : skeleton) {
+        ret.merge(straight_walls(pad_part.contour, z_max, z_min,thr));
+
+        for (auto &h : pad_part.holes)
+            ret.merge(straight_walls(h, z_max, z_min, thr));
+
+        ret.merge(triangulate_expolygon_3d(pad_part, z_min, NORMALS_DOWN));
+        ret.merge(triangulate_expolygon_3d(pad_part, z_max, NORMALS_UP));
+    }
+
+    return ret;
+}
+
+Contour3D create_pad_geometry(const PadSkeleton &skelet,
+                              const PadConfig &  cfg,
+                              ThrowOnCancel      thr)
+{
+#ifndef NDEBUG
+    SVG svg("pad_skeleton.svg");
+    svg.draw(skelet.outer, "green");
+    svg.draw(skelet.inner, "blue");
+    svg.Close();
+#endif
+
+    PadConfig3D cfg3d(cfg);
+    return create_outer_pad_geometry(skelet.outer, cfg3d, thr)
+        .merge(create_inner_pad_geometry(skelet.inner, cfg3d, thr));
+}
+
+Contour3D create_pad_geometry(const ExPolygons &supp_bp,
+                              const ExPolygons &model_bp,
+                              const PadConfig & cfg,
+                              ThrowOnCancel thr)
+{
+    PadSkeleton skelet;
+
+    if (cfg.embed_object.enabled) {
+        if (cfg.embed_object.everywhere)
+            skelet = BrimPadSkeleton(supp_bp, model_bp, cfg, thr);
+        else
+            skelet = AroundPadSkeleton(supp_bp, model_bp, cfg, thr);
+    } else
+        skelet = BelowPadSkeleton(supp_bp, model_bp, cfg, thr);
+
+    return create_pad_geometry(skelet, cfg, thr);
+}
+
+} // namespace
+
+void pad_blueprint(const TriangleMesh &      mesh,
+                   ExPolygons &              output,
+                   const std::vector<float> &heights,
+                   ThrowOnCancel             thrfn)
+{
+    if (mesh.empty()) return;
+    TriangleMeshSlicer slicer(&mesh);
+
+    auto out = reserve_vector<ExPolygons>(heights.size());
+    slicer.slice(heights, 0.f, &out, thrfn);
+
+    size_t count = 0;
+    for(auto& o : out) count += o.size();
+
+    // Unification is expensive, a simplify also speeds up the pad generation
+    auto tmp = reserve_vector<ExPolygon>(count);
+    for(ExPolygons& o : out)
+        for(ExPolygon& e : o) {
+            auto&& exss = e.simplify(scaled<double>(0.1));
+            for(ExPolygon& ep : exss) tmp.emplace_back(std::move(ep));
+        }
+
+    ExPolygons utmp = union_ex(tmp);
+
+    for(auto& o : utmp) {
+        auto&& smp = o.simplify(scaled<double>(0.1));
+        output.insert(output.end(), smp.begin(), smp.end());
+    }
+}
+
+void pad_blueprint(const TriangleMesh &mesh,
+                   ExPolygons &        output,
+                   float               h,
+                   float               layerh,
+                   ThrowOnCancel       thrfn)
+{
+    float gnd = float(mesh.bounding_box().min(Z));
+
+    std::vector<float> slicegrid = grid(gnd, gnd + h, layerh);
+    pad_blueprint(mesh, output, slicegrid, thrfn);
+}
+
+void create_pad(const ExPolygons &sup_blueprint,
+                const ExPolygons &model_blueprint,
+                TriangleMesh &    out,
+                const PadConfig & cfg,
+                ThrowOnCancel thr)
+{
+    Contour3D t = create_pad_geometry(sup_blueprint, model_blueprint, cfg, thr);
+    out.merge(mesh(std::move(t)));
+}
+
+std::string PadConfig::validate() const
+{
+    static const double constexpr MIN_BRIM_SIZE_MM = .1;
+
+    if (brim_size_mm < MIN_BRIM_SIZE_MM ||
+        bottom_offset() > brim_size_mm + wing_distance() ||
+        ConcaveHull::get_waffle_offset(*this) <= MIN_BRIM_SIZE_MM)
+        return L("Pad brim size is too small for the current configuration.");
+
+    return "";
+}
+
+}} // namespace Slic3r::sla
diff --git a/src/libslic3r/SLA/SLAPad.hpp b/src/libslic3r/SLA/SLAPad.hpp
new file mode 100644
index 000000000..4abcdd281
--- /dev/null
+++ b/src/libslic3r/SLA/SLAPad.hpp
@@ -0,0 +1,94 @@
+#ifndef SLABASEPOOL_HPP
+#define SLABASEPOOL_HPP
+
+#include <vector>
+#include <functional>
+#include <cmath>
+#include <string>
+
+namespace Slic3r {
+
+class ExPolygon;
+class Polygon;
+using ExPolygons = std::vector<ExPolygon>;
+using Polygons = std::vector<Polygon>;
+
+class TriangleMesh;
+
+namespace sla {
+
+using ThrowOnCancel = std::function<void(void)>;
+
+/// Calculate the polygon representing the silhouette.
+void pad_blueprint(
+    const TriangleMesh &mesh,       // input mesh
+    ExPolygons &        output,     // Output will be merged with
+    const std::vector<float> &,     // Exact Z levels to sample
+    ThrowOnCancel thrfn = [] {}); // Function that throws if cancel was requested
+
+void pad_blueprint(
+    const TriangleMesh &mesh,
+    ExPolygons &        output,
+    float               samplingheight = 0.1f,  // The height range to sample
+    float               layerheight    = 0.05f, // The sampling height
+    ThrowOnCancel       thrfn = [] {});
+
+struct PadConfig {
+    double wall_thickness_mm = 1.;
+    double wall_height_mm = 1.;
+    double max_merge_dist_mm = 50;
+    double wall_slope = std::atan(1.0);          // Universal constant for Pi/4
+    double brim_size_mm = 1.6;
+
+    struct EmbedObject {
+        double object_gap_mm = 1.;
+        double stick_stride_mm = 10.;
+        double stick_width_mm = 0.5;
+        double stick_penetration_mm = 0.1;
+        bool enabled = false;
+        bool everywhere = false;
+        operator bool() const { return enabled; }
+    } embed_object;
+
+    inline PadConfig() = default;
+    inline PadConfig(double thickness,
+                     double height,
+                     double mergedist,
+                     double slope)
+        : wall_thickness_mm(thickness)
+        , wall_height_mm(height)
+        , max_merge_dist_mm(mergedist)
+        , wall_slope(slope)
+    {}
+
+    inline double bottom_offset() const
+    {
+        return (wall_thickness_mm + wall_height_mm) / std::tan(wall_slope);
+    }
+
+    inline double wing_distance() const
+    {
+        return wall_height_mm / std::tan(wall_slope);
+    }
+
+    inline double full_height() const
+    {
+        return wall_height_mm + wall_thickness_mm;
+    }
+
+    /// Returns the elevation needed for compensating the pad.
+    inline double required_elevation() const { return wall_thickness_mm; }
+
+    std::string validate() const;
+};
+
+void create_pad(const ExPolygons &support_contours,
+                const ExPolygons &model_contours,
+                TriangleMesh &    output_mesh,
+                const PadConfig & = PadConfig(),
+                ThrowOnCancel throw_on_cancel = []{});
+
+} // namespace sla
+} // namespace Slic3r
+
+#endif // SLABASEPOOL_HPP
diff --git a/src/libslic3r/SLA/SLARaster.cpp b/src/libslic3r/SLA/SLARaster.cpp
index 32a88b1b5..091cadd23 100644
--- a/src/libslic3r/SLA/SLARaster.cpp
+++ b/src/libslic3r/SLA/SLARaster.cpp
@@ -5,6 +5,7 @@
 
 #include "SLARaster.hpp"
 #include "libslic3r/ExPolygon.hpp"
+#include "libslic3r/MTUtils.hpp"
 #include <libnest2d/backends/clipper/clipper_polygon.hpp>
 
 // For rasterizing
@@ -32,25 +33,30 @@ inline const ClipperLib::Paths& holes(const ClipperLib::Polygon& p) { return p.H
 
 namespace sla {
 
-class Raster::Impl {
-public:
-    using TPixelRenderer = agg::pixfmt_gray8; // agg::pixfmt_rgb24;
-    using TRawRenderer = agg::renderer_base<TPixelRenderer>;
-    using TPixel = TPixelRenderer::color_type;
-    using TRawBuffer = agg::rendering_buffer;
+const Raster::TMirroring Raster::NoMirror = {false, false};
+const Raster::TMirroring Raster::MirrorX  = {true, false};
+const Raster::TMirroring Raster::MirrorY  = {false, true};
+const Raster::TMirroring Raster::MirrorXY = {true, true};
+
 
-    using TBuffer = std::vector<TPixelRenderer::pixel_type>;
+using TPixelRenderer = agg::pixfmt_gray8; // agg::pixfmt_rgb24;
+using TRawRenderer = agg::renderer_base<TPixelRenderer>;
+using TPixel = TPixelRenderer::color_type;
+using TRawBuffer = agg::rendering_buffer;
+using TBuffer = std::vector<TPixelRenderer::pixel_type>;
 
-    using TRendererAA = agg::renderer_scanline_aa_solid<TRawRenderer>;
+using TRendererAA = agg::renderer_scanline_aa_solid<TRawRenderer>;
+
+class Raster::Impl {
+public:
 
     static const TPixel ColorWhite;
     static const TPixel ColorBlack;
 
-    using Format = Raster::Format;
+    using Format = Raster::RawData;
 
 private:
     Raster::Resolution m_resolution;
-//    Raster::PixelDim m_pxdim;
     Raster::PixelDim m_pxdim_scaled;    // used for scaled coordinate polygons
     TBuffer m_buf;
     TRawBuffer m_rbuf;
@@ -59,74 +65,49 @@ private:
     TRendererAA m_renderer;
     
     std::function<double(double)> m_gammafn;
-    std::array<bool, 2> m_mirror;
-    Format m_fmt = Format::PNG;
+    Trafo m_trafo;
     
     inline void flipy(agg::path_storage& path) const {
-        path.flip_y(0, m_resolution.height_px);
+        path.flip_y(0, double(m_resolution.height_px));
     }
     
     inline void flipx(agg::path_storage& path) const {
-        path.flip_x(0, m_resolution.width_px);
+        path.flip_x(0, double(m_resolution.width_px));
     }
 
 public:
-
-    inline Impl(const Raster::Resolution& res, const Raster::PixelDim &pd,
-                const std::array<bool, 2>& mirror, double gamma = 1.0):
-        m_resolution(res), 
-//        m_pxdim(pd), 
-        m_pxdim_scaled(SCALING_FACTOR / pd.w_mm, SCALING_FACTOR / pd.h_mm),
-        m_buf(res.pixels()),
-        m_rbuf(reinterpret_cast<TPixelRenderer::value_type*>(m_buf.data()),
-              res.width_px, res.height_px,
-              int(res.width_px*TPixelRenderer::num_components)),
-        m_pixfmt(m_rbuf),
-        m_raw_renderer(m_pixfmt),
-        m_renderer(m_raw_renderer),
-        m_mirror(mirror)
+    inline Impl(const Raster::Resolution & res,
+                const Raster::PixelDim &   pd,
+                const Trafo &trafo)
+        : m_resolution(res)
+        , m_pxdim_scaled(SCALING_FACTOR / pd.w_mm, SCALING_FACTOR / pd.h_mm)
+        , m_buf(res.pixels())
+        , m_rbuf(reinterpret_cast<TPixelRenderer::value_type *>(m_buf.data()),
+                 unsigned(res.width_px),
+                 unsigned(res.height_px),
+                 int(res.width_px * TPixelRenderer::num_components))
+        , m_pixfmt(m_rbuf)
+        , m_raw_renderer(m_pixfmt)
+        , m_renderer(m_raw_renderer)
+        , m_trafo(trafo)
     {
         m_renderer.color(ColorWhite);
         
-        if(gamma > 0) m_gammafn = agg::gamma_power(gamma);
+        if (trafo.gamma > 0) m_gammafn = agg::gamma_power(trafo.gamma);
         else m_gammafn = agg::gamma_threshold(0.5);
         
         clear();
     }
-    
-    inline Impl(const Raster::Resolution& res, 
-                const Raster::PixelDim &pd,
-                Format fmt, 
-                double gamma = 1.0): 
-        Impl(res, pd, {false, false}, gamma) 
-    {
-        switch (fmt) {
-        case Format::PNG: m_mirror = {false, true}; break;
-        case Format::RAW: m_mirror = {false, false}; break;
-        }
-        m_fmt = fmt;
-    }
 
     template<class P> void draw(const P &poly) {
         agg::rasterizer_scanline_aa<> ras;
         agg::scanline_p8 scanlines;
         
         ras.gamma(m_gammafn);
-
-        auto&& path = to_path(contour(poly));
         
-        if(m_mirror[X]) flipx(path);
-        if(m_mirror[Y]) flipy(path);
-
-        ras.add_path(path);
-
-        for(auto& h : holes(poly)) {
-            auto&& holepath = to_path(h);
-            if(m_mirror[X]) flipx(holepath);
-            if(m_mirror[Y]) flipy(holepath);
-            ras.add_path(holepath);
-        }
-
+        ras.add_path(to_path(contour(poly)));
+        for(auto& h : holes(poly)) ras.add_path(to_path(h));
+        
         agg::render_scanlines(ras, scanlines, m_renderer);
     }
 
@@ -135,11 +116,16 @@ public:
     }
 
     inline TBuffer& buffer()  { return m_buf; }
+    inline const TBuffer& buffer() const { return m_buf; }
     
-    inline Format format() const { return m_fmt; }
 
     inline const Raster::Resolution resolution() { return m_resolution; }
-   
+    inline const Raster::PixelDim   pixdim()
+    {
+        return {SCALING_FACTOR / m_pxdim_scaled.w_mm,
+                SCALING_FACTOR / m_pxdim_scaled.h_mm};
+    }
+
 private:
     inline double getPx(const Point& p) {
         return p(0) * m_pxdim_scaled.w_mm;
@@ -162,49 +148,67 @@ private:
         return p.Y * m_pxdim_scaled.h_mm;
     }
 
-    template<class PointVec> agg::path_storage to_path(const PointVec& poly)
+    template<class PointVec> agg::path_storage _to_path(const PointVec& v)
     {
         agg::path_storage path;
         
-        auto it = poly.begin();
+        auto it = v.begin();
         path.move_to(getPx(*it), getPy(*it));
+        while(++it != v.end()) path.line_to(getPx(*it), getPy(*it));
+        path.line_to(getPx(v.front()), getPy(v.front()));
+        
+        return path;
+    }
+   
+    template<class PointVec> agg::path_storage _to_path_flpxy(const PointVec& v)
+    {
+        agg::path_storage path;
+        
+        auto it = v.begin();
+        path.move_to(getPy(*it), getPx(*it));
+        while(++it != v.end()) path.line_to(getPy(*it), getPx(*it));
+        path.line_to(getPy(v.front()), getPx(v.front()));
+        
+        return path;
+    }
+    
+    template<class PointVec> agg::path_storage to_path(const PointVec &v)
+    {
+        auto path = m_trafo.flipXY ? _to_path_flpxy(v) : _to_path(v);
+        
+        path.translate_all_paths(m_trafo.origin_x * m_pxdim_scaled.w_mm,
+                                 m_trafo.origin_y * m_pxdim_scaled.h_mm);
+        
+        if(m_trafo.mirror_x) flipx(path);
+        if(m_trafo.mirror_y) flipy(path);
         
-        while(++it != poly.end())
-            path.line_to(getPx(*it), getPy(*it));
-
-        path.line_to(getPx(poly.front()), getPy(poly.front()));
         return path;
     }
 
 };
 
-const Raster::Impl::TPixel Raster::Impl::ColorWhite = Raster::Impl::TPixel(255);
-const Raster::Impl::TPixel Raster::Impl::ColorBlack = Raster::Impl::TPixel(0);
-
-template<> Raster::Raster() { reset(); };
-Raster::~Raster() = default;
-
-// Raster::Raster(Raster &&m) = default;
-// Raster& Raster::operator=(Raster&&) = default;
+const TPixel Raster::Impl::ColorWhite = TPixel(255);
+const TPixel Raster::Impl::ColorBlack = TPixel(0);
 
-// FIXME: remove after migrating to higher version of windows compiler
-Raster::Raster(Raster &&m): m_impl(std::move(m.m_impl)) {}
-Raster& Raster::operator=(Raster &&m) {
-    m_impl = std::move(m.m_impl); return *this;
-}
+Raster::Raster() { reset(); }
 
-void Raster::reset(const Raster::Resolution &r, const Raster::PixelDim &pd,
-                   Format fmt, double gamma)
+Raster::Raster(const Raster::Resolution &r,
+               const Raster::PixelDim &  pd,
+               const Raster::Trafo &     tr)
 {
-    m_impl.reset();
-    m_impl.reset(new Impl(r, pd, fmt, gamma));
+    reset(r, pd, tr);
 }
 
+Raster::~Raster() = default;
+
+Raster::Raster(Raster &&m) = default;
+Raster &Raster::operator=(Raster &&) = default;
+
 void Raster::reset(const Raster::Resolution &r, const Raster::PixelDim &pd,
-                   const std::array<bool, 2>& mirror, double gamma)
+                   const Trafo &trafo)
 {
     m_impl.reset();
-    m_impl.reset(new Impl(r, pd, mirror, gamma));
+    m_impl.reset(new Impl(r, pd, trafo));
 }
 
 void Raster::reset()
@@ -214,9 +218,16 @@ void Raster::reset()
 
 Raster::Resolution Raster::resolution() const
 {
-    if(m_impl) return m_impl->resolution();
+    if (m_impl) return m_impl->resolution();
+    
+    return Resolution{0, 0};
+}
 
-    return Resolution(0, 0);
+Raster::PixelDim Raster::pixel_dimensions() const
+{
+    if (m_impl) return m_impl->pixdim();
+    
+    return PixelDim{0., 0.};
 }
 
 void Raster::clear()
@@ -227,103 +238,83 @@ void Raster::clear()
 
 void Raster::draw(const ExPolygon &expoly)
 {
+    assert(m_impl);
     m_impl->draw(expoly);
 }
 
 void Raster::draw(const ClipperLib::Polygon &poly)
 {
+    assert(m_impl);
     m_impl->draw(poly);
 }
 
-void Raster::save(std::ostream& stream, Format fmt)
+uint8_t Raster::read_pixel(size_t x, size_t y) const
 {
-    assert(m_impl);
-    if(!stream.good()) return;
-
-    switch(fmt) {
-    case Format::PNG: {
-        auto& b = m_impl->buffer();
-        size_t out_len = 0;
-        void * rawdata = tdefl_write_image_to_png_file_in_memory(
-                    b.data(),
-                    int(resolution().width_px),
-                    int(resolution().height_px), 1, &out_len);
-
-        if(rawdata == nullptr) break;
-
-        stream.write(static_cast<const char*>(rawdata),
-                     std::streamsize(out_len));
-
-        MZ_FREE(rawdata);
-
-        break;
-    }
-    case Format::RAW: {
-        stream << "P5 "
-               << m_impl->resolution().width_px << " "
-               << m_impl->resolution().height_px << " "
-               << "255 ";
-
-        auto sz = m_impl->buffer().size()*sizeof(Impl::TBuffer::value_type);
-        stream.write(reinterpret_cast<const char*>(m_impl->buffer().data()),
-                     std::streamsize(sz));
-    }
-    }
+    assert (m_impl);
+    TPixel::value_type px;
+    m_impl->buffer()[y * resolution().width_px + x].get(px);
+    return px;
 }
 
-void Raster::save(std::ostream &stream)
+PNGImage & PNGImage::serialize(const Raster &raster)
 {
-    save(stream, m_impl->format());
+    size_t s = 0;
+    m_buffer.clear();
+    
+    void *rawdata = tdefl_write_image_to_png_file_in_memory(
+        get_internals(raster).buffer().data(),
+        int(raster.resolution().width_px),
+        int(raster.resolution().height_px), 1, &s);
+    
+    // On error, data() will return an empty vector. No other info can be
+    // retrieved from miniz anyway...
+    if (rawdata == nullptr) return *this;
+    
+    auto ptr = static_cast<std::uint8_t*>(rawdata);
+    
+    m_buffer.reserve(s);
+    std::copy(ptr, ptr + s, std::back_inserter(m_buffer));
+    
+    MZ_FREE(rawdata);
+    return *this;
 }
 
-RawBytes Raster::save(Format fmt)
+std::ostream &operator<<(std::ostream &stream, const Raster::RawData &bytes)
 {
-    assert(m_impl);
-
-    std::vector<std::uint8_t> data; size_t s = 0;
-
-    switch(fmt) {
-    case Format::PNG: {
-        void *rawdata = tdefl_write_image_to_png_file_in_memory(
-                    m_impl->buffer().data(),
-                    int(resolution().width_px),
-                    int(resolution().height_px), 1, &s);
+    stream.write(reinterpret_cast<const char *>(bytes.data()),
+                 std::streamsize(bytes.size()));
+    
+    return stream;
+}
 
-        if(rawdata == nullptr) break;
-        auto ptr = static_cast<std::uint8_t*>(rawdata);
-        
-        data.reserve(s); std::copy(ptr, ptr + s, std::back_inserter(data));
-        
-        MZ_FREE(rawdata);
-        break;
-    }
-    case Format::RAW: {
-        auto header = std::string("P5 ") +
-                std::to_string(m_impl->resolution().width_px) + " " +
-                std::to_string(m_impl->resolution().height_px) + " " + "255 ";
+Raster::RawData::~RawData() = default;
 
-        auto sz = m_impl->buffer().size()*sizeof(Impl::TBuffer::value_type);
-        s = sz + header.size();
-        
-        data.reserve(s);
-        
-        auto buff = reinterpret_cast<std::uint8_t*>(m_impl->buffer().data());
-        std::copy(header.begin(), header.end(), std::back_inserter(data));
-        std::copy(buff, buff+sz, std::back_inserter(data));
-        
-        break;
-    }
-    }
+PPMImage & PPMImage::serialize(const Raster &raster)
+{
+    auto header = std::string("P5 ") +
+            std::to_string(raster.resolution().width_px) + " " +
+            std::to_string(raster.resolution().height_px) + " " + "255 ";
+    
+    const auto &impl = get_internals(raster);
+    auto sz = impl.buffer().size() * sizeof(TBuffer::value_type);
+    size_t s = sz + header.size();
+    
+    m_buffer.clear();
+    m_buffer.reserve(s);
 
-    return {std::move(data)};
+    auto buff = reinterpret_cast<const std::uint8_t*>(impl.buffer().data());
+    std::copy(header.begin(), header.end(), std::back_inserter(m_buffer));
+    std::copy(buff, buff+sz, std::back_inserter(m_buffer));
+    
+    return *this;
 }
 
-RawBytes Raster::save()
+const Raster::Impl &Raster::RawData::get_internals(const Raster &raster)
 {
-    return save(m_impl->format());
+    return *raster.m_impl;
 }
 
-}
-}
+} // namespace sla
+} // namespace Slic3r
 
 #endif // SLARASTER_CPP
diff --git a/src/libslic3r/SLA/SLARaster.hpp b/src/libslic3r/SLA/SLARaster.hpp
index 8b27fd153..b3d73536b 100644
--- a/src/libslic3r/SLA/SLARaster.hpp
+++ b/src/libslic3r/SLA/SLARaster.hpp
@@ -8,45 +8,13 @@
 #include <utility>
 #include <cstdint>
 
-namespace ClipperLib { struct Polygon; }
-
-namespace Slic3r { 
+#include <libslic3r/ExPolygon.hpp>
 
-class ExPolygon;
+namespace ClipperLib { struct Polygon; }
 
+namespace Slic3r {
 namespace sla {
 
-// Raw byte buffer paired with its size. Suitable for compressed PNG data.
-class RawBytes {
-
-    std::vector<std::uint8_t> m_buffer;
-public:
-
-    RawBytes() = default;
-    RawBytes(std::vector<std::uint8_t>&& data): m_buffer(std::move(data)) {}
-    
-    size_t size() const { return m_buffer.size(); }
-    const uint8_t * data() { return m_buffer.data(); }
-    
-    RawBytes(const RawBytes&) = delete;
-    RawBytes& operator=(const RawBytes&) = delete;
-
-    // /////////////////////////////////////////////////////////////////////////
-    // FIXME: the following is needed for MSVC2013 compatibility
-    // /////////////////////////////////////////////////////////////////////////
-
-    // RawBytes(RawBytes&&) = default;
-    // RawBytes& operator=(RawBytes&&) = default;
-
-    RawBytes(RawBytes&& mv) : m_buffer(std::move(mv.m_buffer)) {}
-    RawBytes& operator=(RawBytes&& mv) {
-        m_buffer = std::move(mv.m_buffer);
-        return *this;
-    }
-
-    // /////////////////////////////////////////////////////////////////////////
-};
-
 /**
  * @brief Raster captures an anti-aliased monochrome canvas where vectorial
  * polygons can be rasterized. Fill color is always white and the background is
@@ -60,10 +28,28 @@ class Raster {
     std::unique_ptr<Impl> m_impl;
 public:
 
-    /// Supported compression types
-    enum class Format {
-        RAW,    //!> Uncompressed pixel data
-        PNG     //!> PNG compression
+    // Raw byte buffer paired with its size. Suitable for compressed image data.
+    class RawData
+    {
+    protected:
+        std::vector<std::uint8_t> m_buffer;
+        const Impl& get_internals(const Raster& raster);
+    public:
+        RawData() = default;
+        RawData(std::vector<std::uint8_t>&& data): m_buffer(std::move(data)) {}
+        virtual ~RawData();
+
+        RawData(const RawData &) = delete;
+        RawData &operator=(const RawData &) = delete;
+
+        RawData(RawData &&) = default;
+        RawData &operator=(RawData &&) = default;
+
+        size_t size() const { return m_buffer.size(); }
+        const uint8_t * data() const { return m_buffer.data(); }
+
+        virtual RawData& serialize(const Raster &/*raster*/)  { return *this; }
+        virtual std::string get_file_extension() const = 0;
     };
 
     /// Type that represents a resolution in pixels.
@@ -86,11 +72,36 @@ public:
             w_mm(px_width_mm), h_mm(px_height_mm) {}
     };
 
-    /// Constructor taking the resolution and the pixel dimension.
-    template <class...Args> Raster(Args...args) { 
-        reset(std::forward<Args>(args)...); 
-    }
-    
+    enum Orientation { roLandscape, roPortrait };
+
+    using TMirroring = std::array<bool, 2>;
+    static const TMirroring NoMirror;
+    static const TMirroring MirrorX;
+    static const TMirroring MirrorY;
+    static const TMirroring MirrorXY;
+
+    struct Trafo {
+        bool mirror_x = false, mirror_y = false, flipXY = false;
+        coord_t origin_x = 0, origin_y = 0;
+
+        // If gamma is zero, thresholding will be performed which disables AA.
+        double gamma = 1.;
+
+        // Portrait orientation will make sure the drawed polygons are rotated
+        // by 90 degrees.
+        Trafo(Orientation o = roLandscape, const TMirroring &mirror = NoMirror)
+            // XY flipping implicitly does an X mirror
+            : mirror_x(o == roPortrait ? !mirror[0] : mirror[0])
+            , mirror_y(!mirror[1]) // Makes raster origin to be top left corner
+            , flipXY(o == roPortrait)
+        {}
+    };
+
+    Raster();
+    Raster(const Resolution &r,
+           const PixelDim &  pd,
+           const Trafo &     tr = {});
+
     Raster(const Raster& cpy) = delete;
     Raster& operator=(const Raster& cpy) = delete;
     Raster(Raster&& m);
@@ -98,18 +109,10 @@ public:
     ~Raster();
 
     /// Reallocated everything for the given resolution and pixel dimension.
-    /// The third parameter is either the X, Y mirroring or a supported format 
-    /// for which the correct mirroring will be configured.
-    void reset(const Resolution&, 
-               const PixelDim&, 
-               const std::array<bool, 2>& mirror, 
-               double gamma = 1.0);
-    
-    void reset(const Resolution& r, 
-               const PixelDim& pd, 
-               Format o, 
-               double gamma = 1.0);
-    
+    void reset(const Resolution& r,
+               const PixelDim& pd,
+               const Trafo &tr = {});
+
     /**
      * Release the allocated resources. Drawing in this state ends in
      * unspecified behavior.
@@ -118,6 +121,7 @@ public:
 
     /// Get the resolution of the raster.
     Resolution resolution() const;
+    PixelDim   pixel_dimensions() const;
 
     /// Clear the raster with black color.
     void clear();
@@ -126,24 +130,28 @@ public:
     void draw(const ExPolygon& poly);
     void draw(const ClipperLib::Polygon& poly);
 
-    // Saving the raster: 
-    // It is possible to override the format given in the constructor but
-    // be aware that the mirroring will not be modified.
-    
-    /// Save the raster on the specified stream.
-    void save(std::ostream& stream, Format);
-    void save(std::ostream& stream);
-
-    /// Save into a continuous byte stream which is returned.
-    RawBytes save(Format fmt);
-    RawBytes save();
+    uint8_t read_pixel(size_t w, size_t h) const;
+
+    inline bool empty() const { return ! bool(m_impl); }
+
 };
 
-// This prevents the duplicate default constructor warning on MSVC2013
-template<> Raster::Raster();
+class PNGImage: public Raster::RawData {
+public:
+    PNGImage& serialize(const Raster &raster) override;
+    std::string get_file_extension() const override { return "png"; }
+};
 
+class PPMImage: public Raster::RawData {
+public:
+    PPMImage& serialize(const Raster &raster) override;
+    std::string get_file_extension() const override { return "ppm"; }
+};
+
+std::ostream& operator<<(std::ostream &stream, const Raster::RawData &bytes);
 
 } // sla
 } // Slic3r
 
+
 #endif // SLARASTER_HPP
diff --git a/src/libslic3r/SLA/SLARasterWriter.cpp b/src/libslic3r/SLA/SLARasterWriter.cpp
index 425f4c3c2..f80ce01ab 100644
--- a/src/libslic3r/SLA/SLARasterWriter.cpp
+++ b/src/libslic3r/SLA/SLARasterWriter.cpp
@@ -10,7 +10,7 @@
 
 namespace Slic3r { namespace sla {
 
-std::string SLARasterWriter::createIniContent(const std::string& projectname) const 
+std::string RasterWriter::createIniContent(const std::string& projectname) const 
 {
     std::string out("action = print\njobDir = ");
     out += projectname + "\n";
@@ -21,39 +21,14 @@ std::string SLARasterWriter::createIniContent(const std::string& projectname) co
     return out;
 }
 
-void SLARasterWriter::flpXY(ClipperLib::Polygon &poly)
-{
-    for(auto& p : poly.Contour) std::swap(p.X, p.Y);
-    std::reverse(poly.Contour.begin(), poly.Contour.end());
-    
-    for(auto& h : poly.Holes) {
-        for(auto& p : h) std::swap(p.X, p.Y);
-        std::reverse(h.begin(), h.end());
-    }
-}
-
-void SLARasterWriter::flpXY(ExPolygon &poly)
-{
-    for(auto& p : poly.contour.points) p = Point(p.y(), p.x());
-    std::reverse(poly.contour.points.begin(), poly.contour.points.end());
-    
-    for(auto& h : poly.holes) {
-        for(auto& p : h.points) p = Point(p.y(), p.x());
-        std::reverse(h.points.begin(), h.points.end());
-    }
-}
-
-SLARasterWriter::SLARasterWriter(const Raster::Resolution  &res,
-                                 const Raster::PixelDim    &pixdim,
-                                 const std::array<bool, 2> &mirror,
-                                 double gamma)
-    : m_res(res), m_pxdim(pixdim), m_mirror(mirror), m_gamma(gamma)
-{
-    // PNG raster will implicitly do an Y mirror
-    m_mirror[1] = !m_mirror[1];
-}
+RasterWriter::RasterWriter(const Raster::Resolution &res,
+                           const Raster::PixelDim &  pixdim,
+                           const Raster::Trafo &     trafo,
+                           double                    gamma)
+    : m_res(res), m_pxdim(pixdim), m_trafo(trafo), m_gamma(gamma)
+{}
 
-void SLARasterWriter::save(const std::string &fpath, const std::string &prjname)
+void RasterWriter::save(const std::string &fpath, const std::string &prjname)
 {
     try {
         Zipper zipper(fpath); // zipper with no compression
@@ -103,7 +78,7 @@ std::string get_cfg_value(const DynamicPrintConfig &cfg, const std::string &key)
 
 } // namespace
 
-void SLARasterWriter::set_config(const DynamicPrintConfig &cfg)
+void RasterWriter::set_config(const DynamicPrintConfig &cfg)
 {
     m_config["layerHeight"]    = get_cfg_value(cfg, "layer_height");
     m_config["expTime"]        = get_cfg_value(cfg, "exposure_time");
@@ -118,7 +93,7 @@ void SLARasterWriter::set_config(const DynamicPrintConfig &cfg)
     m_config["prusaSlicerVersion"]    = SLIC3R_BUILD_ID;
 }
 
-void SLARasterWriter::set_statistics(const PrintStatistics &stats)
+void RasterWriter::set_statistics(const PrintStatistics &stats)
 {
     m_config["usedMaterial"] = std::to_string(stats.used_material);
     m_config["numFade"]      = std::to_string(stats.num_fade);
diff --git a/src/libslic3r/SLA/SLARasterWriter.hpp b/src/libslic3r/SLA/SLARasterWriter.hpp
index b9202c464..c231655d2 100644
--- a/src/libslic3r/SLA/SLARasterWriter.hpp
+++ b/src/libslic3r/SLA/SLARasterWriter.hpp
@@ -15,20 +15,17 @@
 
 namespace Slic3r { namespace sla {
 
-// Implementation for PNG raster output
+// API to write the zipped sla output layers and metadata.
+// Implementation uses PNG raster output.
 // Be aware that if a large number of layers are allocated, it can very well
 // exhaust the available memory especially on 32 bit platform.
 // This class is designed to be used in parallel mode. Layers have an ID and
 // each layer can be written and compressed independently (in parallel).
 // At the end when all layers where written, the save method can be used to 
 // write out the result into a zipped archive.
-class SLARasterWriter
+class RasterWriter
 {
 public:
-    enum Orientation {
-        roLandscape,
-        roPortrait
-    };
     
     // Used for addressing parameters of set_statistics()
     struct PrintStatistics
@@ -45,7 +42,7 @@ private:
     // A struct to bind the raster image data and its compressed bytes together.
     struct Layer {
         Raster raster;
-        RawBytes rawbytes;
+        PNGImage rawbytes;
 
         Layer() = default;
         
@@ -61,70 +58,55 @@ private:
     // parallel. Later we can write every layer to the disk sequentially.
     std::vector<Layer> m_layers_rst;
     Raster::Resolution m_res;
-    Raster::PixelDim m_pxdim;
-    std::array<bool, 2> m_mirror;
-    double m_gamma;
-    
+    Raster::PixelDim   m_pxdim;
+    Raster::Trafo      m_trafo;
+    double             m_gamma;
+
     std::map<std::string, std::string> m_config;
     
     std::string createIniContent(const std::string& projectname) const;
-    
-    static void flpXY(ClipperLib::Polygon& poly);
-    static void flpXY(ExPolygon& poly);
 
 public:
-    SLARasterWriter(const Raster::Resolution  &res,
-                    const Raster::PixelDim    &pixdim,
-                    const std::array<bool, 2> &mirror,
-                    double gamma = 1.);
+    
+    // SLARasterWriter is using Raster in custom mirroring mode
+    RasterWriter(const Raster::Resolution &res,
+                 const Raster::PixelDim &  pixdim,
+                 const Raster::Trafo &     trafo,
+                 double                    gamma = 1.);
 
-    SLARasterWriter(const SLARasterWriter& ) = delete;
-    SLARasterWriter& operator=(const SLARasterWriter&) = delete;
-    SLARasterWriter(SLARasterWriter&& m) = default;
-    SLARasterWriter& operator=(SLARasterWriter&&) = default;
+    RasterWriter(const RasterWriter& ) = delete;
+    RasterWriter& operator=(const RasterWriter&) = delete;
+    RasterWriter(RasterWriter&& m) = default;
+    RasterWriter& operator=(RasterWriter&&) = default;
 
     inline void layers(unsigned cnt) { if(cnt > 0) m_layers_rst.resize(cnt); }
     inline unsigned layers() const { return unsigned(m_layers_rst.size()); }
     
-    template<class Poly> void draw_polygon(const Poly& p, unsigned lyr,
-                      Orientation o = roPortrait)
+    template<class Poly> void draw_polygon(const Poly& p, unsigned lyr)
     {
         assert(lyr < m_layers_rst.size());
-        
-        switch (o) {
-        case roPortrait: {
-            Poly poly(p);
-            flpXY(poly);
-            m_layers_rst[lyr].raster.draw(poly);
-            break;
-        }
-        case roLandscape:
-            m_layers_rst[lyr].raster.draw(p);
-            break;
-        }
+        m_layers_rst[lyr].raster.draw(p);
     }
 
     inline void begin_layer(unsigned lyr) {
         if(m_layers_rst.size() <= lyr) m_layers_rst.resize(lyr+1);
-        m_layers_rst[lyr].raster.reset(m_res, m_pxdim, m_mirror, m_gamma);
+        m_layers_rst[lyr].raster.reset(m_res, m_pxdim, m_trafo);
     }
 
     inline void begin_layer() {
         m_layers_rst.emplace_back();
-        m_layers_rst.front().raster.reset(m_res, m_pxdim, m_mirror, m_gamma);
+        m_layers_rst.front().raster.reset(m_res, m_pxdim, m_trafo);
     }
 
     inline void finish_layer(unsigned lyr_id) {
         assert(lyr_id < m_layers_rst.size());
-        m_layers_rst[lyr_id].rawbytes =
-                m_layers_rst[lyr_id].raster.save(Raster::Format::PNG);
+        m_layers_rst[lyr_id].rawbytes.serialize(m_layers_rst[lyr_id].raster);
         m_layers_rst[lyr_id].raster.reset();
     }
 
     inline void finish_layer() {
         if(!m_layers_rst.empty()) {
-            m_layers_rst.back().rawbytes =
-                    m_layers_rst.back().raster.save(Raster::Format::PNG);
+            m_layers_rst.back().rawbytes.serialize(m_layers_rst.back().raster);
             m_layers_rst.back().raster.reset();
         }
     }
diff --git a/src/libslic3r/SLA/SLASpatIndex.hpp b/src/libslic3r/SLA/SLASpatIndex.hpp
index 90dcdc362..20b6fcd58 100644
--- a/src/libslic3r/SLA/SLASpatIndex.hpp
+++ b/src/libslic3r/SLA/SLASpatIndex.hpp
@@ -39,14 +39,19 @@ public:
         insert(std::make_pair(v, unsigned(idx)));
     }
 
-    std::vector<PointIndexEl> query(std::function<bool(const PointIndexEl&)>);
-    std::vector<PointIndexEl> nearest(const Vec3d&, unsigned k);
+    std::vector<PointIndexEl> query(std::function<bool(const PointIndexEl&)>) const;
+    std::vector<PointIndexEl> nearest(const Vec3d&, unsigned k) const;
+    std::vector<PointIndexEl> query(const Vec3d &v, unsigned k) const // wrapper
+    {
+        return nearest(v, k);
+    }
 
     // For testing
     size_t size() const;
     bool empty() const { return size() == 0; }
 
     void foreach(std::function<void(const PointIndexEl& el)> fn);
+    void foreach(std::function<void(const PointIndexEl& el)> fn) const;
 };
 
 using BoxIndexEl = std::pair<Slic3r::BoundingBox, unsigned>;
diff --git a/src/libslic3r/SLA/SLASupportTree.cpp b/src/libslic3r/SLA/SLASupportTree.cpp
index 99f7bc8b3..fea8bf731 100644
--- a/src/libslic3r/SLA/SLASupportTree.cpp
+++ b/src/libslic3r/SLA/SLASupportTree.cpp
@@ -7,7 +7,7 @@
 #include "SLASupportTree.hpp"
 #include "SLABoilerPlate.hpp"
 #include "SLASpatIndex.hpp"
-#include "SLABasePool.hpp"
+#include "SLASupportTreeBuilder.hpp"
 
 #include <libslic3r/MTUtils.hpp>
 #include <libslic3r/ClipperUtils.hpp>
@@ -17,51 +17,14 @@
 #include <libnest2d/optimizers/nlopt/subplex.hpp>
 #include <boost/log/trivial.hpp>
 #include <tbb/parallel_for.h>
+#include <tbb/mutex.h>
+#include <tbb/spin_mutex.h>
 #include <libslic3r/I18N.hpp>
 
 //! macro used to mark string used at localization,
 //! return same string
 #define L(s) Slic3r::I18N::translate(s)
 
-/**
- * Terminology:
- *
- * Support point:
- * The point on the model surface that needs support.
- *
- * Pillar:
- * A thick column that spans from a support point to the ground and has
- * a thick cone shaped base where it touches the ground.
- *
- * Ground facing support point:
- * A support point that can be directly connected with the ground with a pillar
- * that does not collide or cut through the model.
- *
- * Non ground facing support point:
- * A support point that cannot be directly connected with the ground (only with
- * the model surface).
- *
- * Head:
- * The pinhead that connects to the model surface with the sharp end end
- * to a pillar or bridge stick with the dull end.
- *
- * Headless support point:
- * A support point on the model surface for which there is not enough place for
- * the head. It is either in a hole or there is some barrier that would collide
- * with the head geometry. The headless support point can be ground facing and
- * non ground facing as well.
- *
- * Bridge:
- * A stick that connects two pillars or a head with a pillar.
- *
- * Junction:
- * A small ball in the intersection of two or more sticks (pillar, bridge, ...)
- *
- * CompactBridge:
- * A bridge that connects a headless support point with the model surface or a
- * nearby pillar.
- */
-
 namespace Slic3r {
 namespace sla {
 
@@ -80,2527 +43,16 @@ const unsigned SupportConfig::optimizer_max_iterations = 1000;
 const unsigned SupportConfig::pillar_cascade_neighbors = 3;
 const unsigned SupportConfig::max_bridges_on_pillar = 3;
 
-using Coordf = double;
-using Portion = std::tuple<double, double>;
-
-// Set this to true to enable full parallelism in this module.
-// Only the well tested parts will be concurrent if this is set to false.
-const constexpr bool USE_FULL_CONCURRENCY = false;
-
-template<bool> struct _ccr {};
-
-template<> struct _ccr<true>
-{
-    using Mutex = SpinMutex;
-
-    template<class It, class Fn>
-    static inline void enumerate(It from, It to, Fn fn)
-    {
-        using TN = size_t;
-        auto iN  = to - from;
-        TN   N   = iN < 0 ? 0 : TN(iN);
-
-        tbb::parallel_for(TN(0), N, [from, fn](TN n) { fn(*(from + n), n); });
-    }
-};
-
-template<> struct _ccr<false>
-{
-    struct Mutex { inline void lock() {} inline void unlock() {} };
-
-    template<class It, class Fn>
-    static inline void enumerate(It from, It to, Fn fn)
-    {
-        for (auto it = from; it != to; ++it) fn(*it, it - from);
-    }
-};
-
-using ccr = _ccr<USE_FULL_CONCURRENCY>;
-using ccr_seq = _ccr<false>;
-using ccr_par = _ccr<true>;
-
-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);
-}
-
-template<class Vec> double distance(const Vec& pp1, const Vec& pp2) {
-    auto p = pp2 - pp1;
-    return distance(p);
-}
-
-Contour3D sphere(double rho, Portion portion = make_portion(0.0, 2.0*PI),
-                 double fa=(2*PI/360)) {
-
-    Contour3D ret;
-
-    // prohibit close to zero radius
-    if(rho <= 1e-6 && rho >= -1e-6) return ret;
-
-    auto& vertices = ret.points;
-    auto& facets = ret.indices;
-
-    // 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)
-        facets.emplace_back((i == 0) ? Vec3crd(coord_t(ring.size()), 0, 1) :
-                                       Vec3crd(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(Vec3crd(id - 1, id,
-                                            id + coord_t(ring.size() - 1)));
-                facets.emplace_back(Vec3crd(id - 1, id_ringsize, id));
-            } else {
-                facets.emplace_back(Vec3crd(id_ringsize - 1, id_ringsize, id));
-                facets.emplace_back(Vec3crd(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(Vec3crd(id - 1, id_ringsize, id));
-            } else {
-                auto ci = coord_t(id_ringsize + coord_t(i));
-                facets.emplace_back(Vec3crd(ci - 1, ci, id));
-            }
-        }
-    }
-    id++;
-
-    return ret;
-}
-
-// 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, const Vec3d sp = {0,0,0})
-{
-    Contour3D ret;
-
-    auto steps = int(ssteps);
-    auto& points = ret.points;
-    auto& indices = ret.indices;
-    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;
-}
-
-struct Head {
-    Contour3D mesh;
-
-    size_t steps = 45;
-    Vec3d dir = {0, 0, -1};
-    Vec3d tr = {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 = -1;
-
-    // If there is a pillar connecting to this head, then the id will be set.
-    long pillar_id = -1;
-
-    inline void invalidate() { id = -1; }
-    inline bool is_valid() const { return id >= 0; }
-
-    Head(double r_big_mm,
-         double r_small_mm,
-         double length_mm,
-         double penetration,
-         Vec3d direction = {0, 0, -1},    // direction (normal to the dull end )
-         Vec3d offset = {0, 0, 0},        // displacement
-         const size_t circlesteps = 45):
-            steps(circlesteps), dir(direction), tr(offset),
-            r_back_mm(r_big_mm), r_pin_mm(r_small_mm), width_mm(length_mm),
-            penetration_mm(penetration)
-    {
-
-        // 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) z(p) += 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.indices.emplace_back(i1s1, i2s1, i2s2);
-            mesh.indices.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.indices.emplace_back(i2s2, i2s1, i1s1);
-        mesh.indices.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) z(p) -= (h + r_small_mm - penetration_mm);
-    }
-
-    void transform()
-    {
-        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;
-    }
-
-    double fullwidth() const {
-        return 2 * r_pin_mm + width_mm + 2*r_back_mm - penetration_mm;
-    }
-
-    static double fullwidth(const SupportConfig& cfg) {
-        return 2 * cfg.head_front_radius_mm + cfg.head_width_mm +
-               2 * cfg.head_back_radius_mm - cfg.head_penetration_mm;
-    }
-
-    Vec3d junction_point() const {
-        return tr + ( 2 * r_pin_mm + width_mm + r_back_mm - penetration_mm)*dir;
-    }
-
-    double request_pillar_radius(double radius) const {
-        const double rmax = r_back_mm;
-        return radius > 0 && radius < rmax ? radius : rmax;
-    }
-};
-
-struct Junction {
-    Contour3D mesh;
-    double r = 1;
-    size_t steps = 45;
-    Vec3d pos;
-
-    long id = -1;
-
-    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;
-    }
-};
-
-struct Pillar {
-    Contour3D mesh;
-    Contour3D base;
-    double r = 1;
-    size_t steps = 0;
-    Vec3d endpt;
-    double height = 0;
-
-    long id = -1;
-
-    // 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
-    long start_junction_id = -1;
-
-    // How many bridges are connected to this pillar
-    unsigned bridges = 0;
-
-    // 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):
-        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.indices.swap(body.indices);
-        }
-    }
-
-    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 {
-        return {endpt(X), endpt(Y), endpt(Z) + height};
-    }
-
-    inline const Vec3d& endpoint() const { return endpt; }
-
-    Pillar& add_base(double baseheight = 3, double radius = 2) {
-        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.indices;
-        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;
-    }
-
-    bool has_base() const { return !base.points.empty(); }
-};
-
-// A Bridge between two pillars (with junction endpoints)
-struct Bridge {
-    Contour3D mesh;
-    double r = 0.8;
-
-    long id = -1;
-    long start_jid = -1;
-    long end_jid = -1;
-
-    // We should reduce the radius a tiny bit to help the convex hull algorithm
-    Bridge(const Vec3d& j1, const Vec3d& j2,
-           double r_mm = 0.8, size_t steps = 45):
-        r(r_mm)
-    {
-        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;
-    }
-
-    Bridge(const Junction& j1, const Junction& j2, double r_mm = 0.8):
-        Bridge(j1.pos, j2.pos, r_mm, j1.steps) {}
-
-};
-
-// 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 = -1;
-
-    CompactBridge(const Vec3d& sp,
-                  const Vec3d& ep,
-                  const Vec3d& n,
-                  double r,
-                  bool endball = true,
-                  size_t steps = 45)
-    {
-        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);
-    }
-};
-
-// A wrapper struct around the base pool (pad)
-struct Pad {
-    TriangleMesh tmesh;
-    PoolConfig cfg;
-    double zlevel = 0;
-
-    Pad() = default;
-
-    Pad(const TriangleMesh& support_mesh,
-        const ExPolygons& modelbase,
-        double ground_level,
-        const PoolConfig& pcfg) :
-        cfg(pcfg),
-        zlevel(ground_level +
-               sla::get_pad_fullheight(pcfg) -
-               sla::get_pad_elevation(pcfg))
-    {
-        Polygons basep;
-        auto &thr = cfg.throw_on_cancel;
-
-        thr();
-
-        // Get a sample for the pad from the support mesh
-        {
-            ExPolygons platetmp;
-
-            float zstart = float(zlevel);
-            float zend   = zstart + float(get_pad_fullheight(pcfg) + EPSILON);
-
-            base_plate(support_mesh, platetmp, grid(zstart, zend, 0.1f), thr);
-
-            // We don't need no... holes control...
-            for (const ExPolygon &bp : platetmp)
-                basep.emplace_back(std::move(bp.contour));
-        }
-
-        if(pcfg.embed_object) {
-
-            // If the zero elevation mode is ON, we need to process the model
-            // base silhouette. Create the offsetted version and punch the
-            // breaksticks across its perimeter.
-
-            ExPolygons modelbase_offs = modelbase;
-
-            if (pcfg.embed_object.object_gap_mm > 0.0)
-                modelbase_offs
-                    = offset_ex(modelbase_offs,
-                                float(scaled(pcfg.embed_object.object_gap_mm)));
-
-            // Create a spatial index of the support silhouette polygons.
-            // This will be used to check for intersections with the model
-            // silhouette polygons. If there is no intersection, then a certain
-            // part of the pad is redundant as it does not host any supports.
-            BoxIndex bindex;
-            {
-                unsigned idx = 0;
-                for(auto &bp : basep) {
-                    auto bb = bp.bounding_box();
-                    bb.offset(float(scaled(pcfg.min_wall_thickness_mm)));
-                    bindex.insert(bb, idx++);
-                }
-            }
-
-            ExPolygons concaveh = offset_ex(
-                concave_hull(basep, pcfg.max_merge_distance_mm, thr),
-                scaled<float>(pcfg.min_wall_thickness_mm));
-
-            // Punching the breaksticks across the offsetted polygon perimeters
-            auto pad_stickholes = reserve_vector<ExPolygon>(modelbase.size());
-            for(auto& poly : modelbase_offs) {
-
-                bool overlap = false;
-                for (const ExPolygon &p : concaveh)
-                    overlap = overlap || poly.overlaps(p);
-
-                auto bb = poly.contour.bounding_box();
-                bb.offset(scaled<float>(pcfg.min_wall_thickness_mm));
-
-                std::vector<BoxIndexEl> qres =
-                    bindex.query(bb, BoxIndex::qtIntersects);
-
-                if (!qres.empty() || overlap) {
-
-                    // The model silhouette polygon 'poly' HAS an intersection
-                    // with the support silhouettes. Include this polygon
-                    // in the pad holes with the breaksticks and merge the
-                    // original (offsetted) version with the rest of the pad
-                    // base plate.
-
-                    basep.emplace_back(poly.contour);
-
-                    // The holes of 'poly' will become positive parts of the
-                    // pad, so they has to be checked for intersections as well
-                    // and erased if there is no intersection with the supports
-                    auto it = poly.holes.begin();
-                    while(it != poly.holes.end()) {
-                        if (bindex.query(it->bounding_box(),
-                                         BoxIndex::qtIntersects).empty())
-                            it = poly.holes.erase(it);
-                        else
-                            ++it;
-                    }
-
-                    // Punch the breaksticks
-                    sla::breakstick_holes(
-                        poly,
-                        pcfg.embed_object.object_gap_mm,   // padding
-                        pcfg.embed_object.stick_stride_mm,
-                        pcfg.embed_object.stick_width_mm,
-                        pcfg.embed_object.stick_penetration_mm);
-
-                    pad_stickholes.emplace_back(poly);
-                }
-            }
-
-            create_base_pool(basep, tmesh, pad_stickholes, cfg);
-        } else {
-            for (const ExPolygon &bp : modelbase) basep.emplace_back(bp.contour);
-            create_base_pool(basep, tmesh, {}, cfg);
-        }
-
-        tmesh.translate(0, 0, float(zlevel));
-        if (!tmesh.empty()) tmesh.require_shared_vertices();
-    }
-
-    bool empty() const { return tmesh.facets_count() == 0; }
-};
-
-// The minimum distance for two support points to remain valid.
-static const double /*constexpr*/ D_SP   = 0.1;
-
-enum { // For indexing Eigen vectors as v(X), v(Y), v(Z) instead of numbers
-  X, Y, Z
-};
-
-// 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& mesh,
-                 double eps = 0.05,  // min distance from edges
-                 std::function<void()> throw_on_cancel = [](){},
-                 const std::vector<unsigned>& selected_points = {});
-
-inline Vec2d to_vec2(const Vec3d& v3) {
-    return {v3(X), v3(Y)};
-}
-
-bool operator==(const PointIndexEl& e1, const PointIndexEl& e2) {
-    return e1.second == e2.second;
-}
-
-// Clustering a set of points by the given distance.
-ClusteredPoints cluster(const std::vector<unsigned>& indices,
-                        std::function<Vec3d(unsigned)> pointfn,
-                        double dist,
-                        unsigned max_points);
-
-ClusteredPoints cluster(const PointSet& points,
-                        double dist,
-                        unsigned max_points);
-
-ClusteredPoints cluster(
-        const std::vector<unsigned>& indices,
-        std::function<Vec3d(unsigned)> pointfn,
-        std::function<bool(const PointIndexEl&, const PointIndexEl&)> predicate,
-        unsigned max_points);
-
-// This class will hold the support tree meshes with some additional bookkeeping
-// as well. Various parts of the support geometry are stored separately and are
-// merged when the caller queries the merged mesh. The merged result is cached
-// for fast subsequent delivery of the merged mesh which can be quite complex.
-// An object of this class will be used as the result type during the support
-// generation algorithm. Parts will be added with the appropriate methods such
-// as add_head or add_pillar which forwards the constructor arguments and fills
-// the IDs of these substructures. The IDs are basically indices into the arrays
-// of the appropriate type (heads, pillars, etc...). One can later query e.g. a
-// pillar for a specific head...
-//
-// The support pad is considered an auxiliary geometry and is not part of the
-// merged mesh. It can be retrieved using a dedicated method (pad())
-class SLASupportTree::Impl {
-    // 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<CompactBridge> m_compact_bridges;
-    Controller m_ctl;
-
-    Pad m_pad;
-
-    using Mutex = ccr::Mutex;
-
-    mutable Mutex m_mutex;
-    mutable TriangleMesh meshcache; mutable bool meshcache_valid = false;
-    mutable double model_height = 0; // the full height of the model
-
-public:
-    double ground_level = 0;
-
-    Impl() = default;
-    inline Impl(const Controller& ctl): m_ctl(ctl) {}
-
-    const Controller& ctl() const { return m_ctl; }
-
-    template<class...Args> Head& add_head(unsigned id, Args&&... args)
-    {
-        std::lock_guard<Mutex> lk(m_mutex);
-        m_heads.emplace_back(std::forward<Args>(args)...);
-        m_heads.back().id = id;
-
-        if (id >= m_head_indices.size()) m_head_indices.resize(id + 1);
-        m_head_indices[id] = m_heads.size() - 1;
-
-        meshcache_valid = false;
-        return m_heads.back();
-    }
-
-    template<class...Args> Pillar& add_pillar(unsigned headid, Args&&... args)
-    {
-        std::lock_guard<Mutex> lk(m_mutex);
-
-        assert(headid < m_head_indices.size());
-        Head &head = m_heads[m_head_indices[headid]];
-
-        m_pillars.emplace_back(head, std::forward<Args>(args)...);
-        Pillar& pillar = m_pillars.back();
-        pillar.id = long(m_pillars.size() - 1);
-        head.pillar_id = pillar.id;
-        pillar.start_junction_id = head.id;
-        pillar.starts_from_head = true;
-
-        meshcache_valid = false;
-        return m_pillars.back();
-    }
-
-    void increment_bridges(const Pillar& pillar)
-    {
-        std::lock_guard<Mutex> lk(m_mutex);
-        assert(pillar.id >= 0 && size_t(pillar.id) < m_pillars.size());
-
-        if(pillar.id >= 0 && size_t(pillar.id) < m_pillars.size())
-            m_pillars[size_t(pillar.id)].bridges++;
-    }
-
-    void increment_links(const Pillar& pillar)
-    {
-        std::lock_guard<Mutex> lk(m_mutex);
-        assert(pillar.id >= 0 && size_t(pillar.id) < m_pillars.size());
-
-        if(pillar.id >= 0 && size_t(pillar.id) < m_pillars.size())
-            m_pillars[size_t(pillar.id)].links++;
-    }
-
-    template<class...Args> Pillar& add_pillar(Args&&...args)
-    {
-        std::lock_guard<Mutex> lk(m_mutex);
-        m_pillars.emplace_back(std::forward<Args>(args)...);
-        Pillar& pillar = m_pillars.back();
-        pillar.id = long(m_pillars.size() - 1);
-        pillar.starts_from_head = false;
-        meshcache_valid = false;
-        return m_pillars.back();
-    }
-
-    const Head& pillar_head(long pillar_id) const
-    {
-        std::lock_guard<Mutex> lk(m_mutex);
-        assert(pillar_id >= 0 && pillar_id < long(m_pillars.size()));
-
-        const Pillar& p = m_pillars[size_t(pillar_id)];
-        assert(p.starts_from_head && p.start_junction_id >= 0);
-        assert(size_t(p.start_junction_id) < m_head_indices.size());
-
-        return m_heads[m_head_indices[p.start_junction_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);
-        m_junctions.emplace_back(std::forward<Args>(args)...);
-        m_junctions.back().id = long(m_junctions.size() - 1);
-        meshcache_valid = false;
-        return m_junctions.back();
-    }
-
-    template<class...Args> const Bridge& add_bridge(Args&&... args)
-    {
-        std::lock_guard<Mutex> lk(m_mutex);
-        m_bridges.emplace_back(std::forward<Args>(args)...);
-        m_bridges.back().id = long(m_bridges.size() - 1);
-        meshcache_valid = false;
-        return m_bridges.back();
-    }
-
-    template<class...Args> const CompactBridge& add_compact_bridge(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);
-        meshcache_valid = false;
-        return m_compact_bridges.back();
-    }
-
-    Head &head(unsigned id)
-    {
-        std::lock_guard<Mutex> lk(m_mutex);
-        assert(id < m_head_indices.size());
-
-        meshcache_valid = false;
-        return m_heads[m_head_indices[id]];
-    }
-
-    inline size_t pillarcount() const {
-        std::lock_guard<Mutex> lk(m_mutex);
-        return m_pillars.size();
-    }
-
-    template<class T> inline IntegerOnly<T, const Pillar&> pillar(T id) const
-    {
-        std::lock_guard<Mutex> lk(m_mutex);
-        assert(id >= 0 && size_t(id) < m_pillars.size() &&
-               size_t(id) < std::numeric_limits<size_t>::max());
-
-        return m_pillars[size_t(id)];
-    }
-
-    const Pad &create_pad(const TriangleMesh &object_supports,
-                          const ExPolygons &  modelbase,
-                          const PoolConfig &  cfg)
-    {
-        m_pad = Pad(object_supports, modelbase, ground_level, cfg);
-        return m_pad;
-    }
-
-    void remove_pad() { m_pad = Pad(); }
-
-    const Pad& pad() const { return m_pad; }
-
-    // WITHOUT THE PAD!!!
-    const TriangleMesh &merged_mesh() const
-    {
-        if (meshcache_valid) return meshcache;
-
-        Contour3D merged;
-
-        for (auto &head : m_heads) {
-            if (m_ctl.stopcondition()) break;
-            if (head.is_valid()) merged.merge(head.mesh);
-        }
-
-        for (auto &stick : m_pillars) {
-            if (m_ctl.stopcondition()) break;
-            merged.merge(stick.mesh);
-            merged.merge(stick.base);
-        }
-
-        for (auto &j : m_junctions) {
-            if (m_ctl.stopcondition()) break;
-            merged.merge(j.mesh);
-        }
-
-        for (auto &cb : m_compact_bridges) {
-            if (m_ctl.stopcondition()) break;
-            merged.merge(cb.mesh);
-        }
-
-        for (auto &bs : m_bridges) {
-            if (m_ctl.stopcondition()) break;
-            merged.merge(bs.mesh);
-        }
-
-        if (m_ctl.stopcondition()) {
-            // In case of failure we have to return an empty mesh
-            meshcache = TriangleMesh();
-            return meshcache;
-        }
-
-        meshcache = mesh(merged);
-
-        // The mesh will be passed by const-pointer to TriangleMeshSlicer,
-        // which will need this.
-        if (!meshcache.empty()) meshcache.require_shared_vertices();
-
-        BoundingBoxf3 &&bb = meshcache.bounding_box();
-        model_height       = bb.max(Z) - bb.min(Z);
-
-        meshcache_valid = true;
-        return meshcache;
-    }
-
-    // WITH THE PAD
-    double full_height() const
-    {
-        if (merged_mesh().empty() && !pad().empty())
-            return get_pad_fullheight(pad().cfg);
-
-        double h = mesh_height();
-        if (!pad().empty()) h += sla::get_pad_elevation(pad().cfg);
-        return h;
-    }
-
-    // WITHOUT THE PAD!!!
-    double mesh_height() const
-    {
-        if (!meshcache_valid) merged_mesh();
-        return model_height;
-    }
-
-    // Intended to be called after the generation is fully complete
-    void merge_and_cleanup()
-    {
-        merged_mesh(); // in case the mesh is not generated, it should be...
-
-        // Doing clear() does not garantee to release the memory.
-        m_heads = {};
-        m_head_indices = {};
-        m_pillars = {};
-        m_junctions = {};
-        m_bridges = {};
-        m_compact_bridges = {};
-    }
-};
-
-// 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,
-                      std::function<Vec3d(size_t)> 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());
-}
-
-inline Vec3d dirv(const Vec3d& startp, const Vec3d& endp) {
-    return (endp - startp).normalized();
-}
-
-class SLASupportTree::Algorithm {
-    const SupportConfig& m_cfg;
-    const EigenMesh3D& m_mesh;
-    const std::vector<SupportPoint>& m_support_pts;
-
-    using PtIndices = std::vector<unsigned>;
-
-    PtIndices m_iheads;            // support points with pinhead
-    PtIndices m_iheadless;         // headless support points
-
-    // supp. pts. connecting to model: point index and the ray hit data
-    std::vector<std::pair<unsigned, EigenMesh3D::hit_result>> m_iheads_onmodel;
-
-    // normals for support points from model faces.
-    PointSet  m_support_nmls;
-
-    // Clusters of points which can reach the ground directly and can be
-    // bridged to one central pillar
-    std::vector<PtIndices> m_pillar_clusters;
-
-    // This algorithm uses the Impl class as its output stream. It will be
-    // filled gradually with support elements (heads, pillars, bridges, ...)
-    using Result = SLASupportTree::Impl;
-
-    Result& m_result;
-
-    // support points in Eigen/IGL format
-    PointSet m_points;
-
-    // throw if canceled: It will be called many times so a shorthand will
-    // come in handy.
-    ThrowOnCancel m_thr;
-
-    // A spatial index to easily find strong pillars to connect to.
-    PointIndex m_pillar_index;
-
-    inline double ray_mesh_intersect(const Vec3d& s,
-                                     const Vec3d& dir)
-    {
-        return m_mesh.query_ray_hit(s, dir).distance();
-    }
-
-    // This function will test if a future pinhead would not collide with the
-    // model geometry. It does not take a 'Head' object because those are
-    // created after this test. Parameters: s: The touching point on the model
-    // surface. dir: This is the direction of the head from the pin to the back
-    // r_pin, r_back: the radiuses of the pin and the back sphere width: This
-    // is the full width from the pin center to the back center m: The object
-    // mesh.
-    // The return value is the hit result from the ray casting. If the starting
-    // 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(
-            const Vec3d& s,
-            const Vec3d& dir,
-            double r_pin,
-            double r_back,
-            double width)
-    {
-        static const size_t SAMPLES = 8;
-
-        // method based on:
-        // https://math.stackexchange.com/questions/73237/parametric-equation-of-a-circle-in-3d-space
-
-        // 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.
-
-        // Move away slightly from the touching point to avoid raycasting on the
-        // inner surface of the mesh.
-        Vec3d v = dir;     // Our direction (axis)
-        Vec3d c = s + width * dir;
-        const double& sd = m_cfg.safety_distance_mm;
-
-        // 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.
-        Vec3d a(0, 1, 0), b;
-
-        // The portions of the circle (the head-back circle) for which we will
-        // shoot rays.
-        std::array<double, SAMPLES> phis;
-        for(size_t i = 0; i < phis.size(); ++i) phis[i] = i*2*PI/phis.size();
-
-        auto& m = m_mesh;
-        using HitResult = EigenMesh3D::hit_result;
-
-        // Hit results
-        std::array<HitResult, SAMPLES> hits;
-
-        // 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'
-        auto chk1 = [] (double val) {
-            return std::abs(std::abs(val) - 1) < 1e-20;
-        };
-
-        if(chk1(v(X)) || chk1(v(Y)) || chk1(v(Z))) {
-            a = {v(Z), v(X), v(Y)};
-            b = {v(Y), v(Z), v(X)};
-        }
-        else {
-            a(Z) = -(v(Y)*a(Y)) / v(Z); a.normalize();
-            b = a.cross(v);
-        }
-
-        // Now a and b vectors are perpendicular to v and to each other.
-        // Together they define the plane where we have to iterate with the
-        // given angles in the 'phis' vector
-        ccr_par::enumerate(phis.begin(), phis.end(),
-                           [&hits, &m, sd, r_pin, r_back, s, a, b, c]
-                           (double phi, size_t i)
-        {
-            double sinphi = std::sin(phi);
-            double cosphi = std::cos(phi);
-
-            // Let's have a safety coefficient for the radiuses.
-            double rpscos = (sd + r_pin) * cosphi;
-            double rpssin = (sd + r_pin) * sinphi;
-            double rpbcos = (sd + r_back) * cosphi;
-            double rpbsin = (sd + r_back) * sinphi;
-
-            // Point on the circle on the pin sphere
-            Vec3d ps(s(X) + rpscos * a(X) + rpssin * b(X),
-                     s(Y) + rpscos * a(Y) + rpssin * b(Y),
-                     s(Z) + rpscos * a(Z) + rpssin * b(Z));
-
-            // 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).
-
-            // This is the point on the circle on the back sphere
-            Vec3d p(c(X) + rpbcos * a(X) + rpbsin * b(X),
-                    c(Y) + rpbcos * a(Y) + rpbsin * b(Y),
-                    c(Z) + rpbcos * a(Z) + rpbsin * b(Z));
-
-            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() > r_pin + sd)  {
-                    // If we are inside the model and the hit distance is bigger
-                    // than our pin circle diameter, it probably indicates that
-                    // the support point was already inside the model, or there
-                    // is really no space around the point. We will assign a
-                    // zero hit distance to these cases which will enforce the
-                    // function return value to be an invalid ray with zero hit
-                    // distance. (see min_element at the end)
-                    hits[i] = HitResult(0.0);
-                }
-                else {
-                    // re-cast the ray from the outside of the object.
-                    // The starting point has an offset of 2*safety_distance
-                    // because the original ray has also had an offset
-                    auto q2 = m.query_ray_hit(ps + (q.distance() + 2*sd)*n, n);
-                    hits[i] = q2;
-                }
-            } else hits[i] = q;
-        });
-
-        auto mit = std::min_element(hits.begin(), hits.end());
-
-        return *mit;
-    }
-
-    // Checking bridge (pillar and stick as well) intersection with the model.
-    // If the function is used for headless sticks, the ins_check parameter
-    // have to be true as the beginning of the stick might be inside the model
-    // geometry.
-    // The return value is the hit result from the ray casting. If the starting
-    // 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(
-            const Vec3d& s,
-            const Vec3d& dir,
-            double r,
-            bool ins_check = false)
-    {
-        static const size_t SAMPLES = 8;
-
-        // helper vector calculations
-        Vec3d a(0, 1, 0), b;
-        const double& sd = m_cfg.safety_distance_mm;
-
-        // INFO: for explanation of the method used here, see the previous
-        // method's comments.
-
-        auto chk1 = [] (double val) {
-            return std::abs(std::abs(val) - 1) < 1e-20;
-        };
-
-        if(chk1(dir(X)) || chk1(dir(Y)) || chk1(dir(Z))) {
-            a = {dir(Z), dir(X), dir(Y)};
-            b = {dir(Y), dir(Z), dir(X)};
-        }
-        else {
-            a(Z) = -(dir(Y)*a(Y)) / dir(Z); a.normalize();
-            b = a.cross(dir);
-        }
-
-        // circle portions
-        std::array<double, SAMPLES> phis;
-        for(size_t i = 0; i < phis.size(); ++i) phis[i] = i*2*PI/phis.size();
-
-        auto& m = m_mesh;
-        using HitResult = EigenMesh3D::hit_result;
-
-        // Hit results
-        std::array<HitResult, SAMPLES> hits;
-
-        ccr_par::enumerate(phis.begin(), phis.end(),
-                           [&m, a, b, sd, dir, r, s, ins_check, &hits]
-                           (double phi, size_t i)
-        {
-            double sinphi = std::sin(phi);
-            double cosphi = std::cos(phi);
-
-            // Let's have a safety coefficient for the radiuses.
-            double rcos = (sd + r) * cosphi;
-            double rsin = (sd + r) * sinphi;
-
-            // Point on the circle on the pin sphere
-            Vec3d p (s(X) + rcos * a(X) + rsin * b(X),
-                     s(Y) + rcos * a(Y) + rsin * b(Y),
-                     s(Z) + rcos * a(Z) + rsin * b(Z));
-
-            auto hr = m.query_ray_hit(p + sd*dir, dir);
-
-            if(ins_check && hr.is_inside()) {
-                if(hr.distance() > 2 * r + sd) hits[i] = HitResult(0.0);
-                else {
-                    // re-cast the ray from the outside of the object
-                    auto hr2 =
-                           m.query_ray_hit(p + (hr.distance() + 2*sd)*dir, dir);
-
-                    hits[i] = hr2;
-                }
-            } else hits[i] = hr;
-        });
-
-        auto mit = std::min_element(hits.begin(), hits.end());
-
-        return *mit;
-    }
-
-    // Helper function for interconnecting two pillars with zig-zag bridges.
-    bool interconnect(const Pillar& pillar, const Pillar& nextpillar)
-    {
-        // We need to get the starting point of the zig-zag pattern. We have to
-        // be aware that the two head junctions are at different heights. We
-        // may start from the lowest junction and call it a day but this
-        // strategy would leave unconnected a lot of pillar duos where the
-        // 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_result.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_intersect(sj,
-                                     dirv(sj, ej),
-                                     pillar.r) >= bridge_distance)
-            {
-                m_result.add_bridge(sj, ej, pillar.r);
-                was_connected = true;
-            }
-
-            // double bridging: (crosses)
-            if(docrosses) {
-                Vec3d sjback(ej(X), ej(Y), sj(Z));
-                Vec3d ejback(sj(X), sj(Y), ej(Z));
-                if(sjback(Z) <= slower(Z) && ejback(Z) >= eupper(Z) &&
-                   bridge_mesh_intersect(sjback,
-                                         dirv(sjback, ejback),
-                                         pillar.r) >= bridge_distance)
-                {
-                    // need to check collision for the cross stick
-                    m_result.add_bridge(sjback, ejback, pillar.r);
-                    was_connected = true;
-                }
-            }
-
-            sj.swap(ej);
-            ej(Z) = sj(Z) + zstep;
-        }
-
-        return was_connected;
-    }
-
-    // For connecting a head to a nearby pillar.
-    bool connect_to_nearpillar(const Head& head, long nearpillar_id) {
-
-        auto nearpillar = [this, nearpillar_id]() {
-            return m_result.pillar(nearpillar_id);
-        };
-
-        if (nearpillar().bridges > 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_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_intersect(headjp, {0,0,-1}, 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_result.ground_level + 2 * m_cfg.head_width_mm;
-        if(bridgeend(Z) < minz) return false;
-
-        double t = bridge_mesh_intersect(bridgestart,
-                                         dirv(bridgestart, bridgeend), r);
-
-        // Cannot insert the bridge. (further search might not worth the hassle)
-        if(t < distance(bridgestart, bridgeend)) return false;
-
-        // A partial pillar is needed under the starting head.
-        if(zdiff > 0) {
-            m_result.add_pillar(unsigned(head.id), bridgestart, r);
-            m_result.add_junction(bridgestart, r);
-        }
-
-        m_result.add_bridge(bridgestart, bridgeend, r);
-        m_result.increment_bridges(nearpillar());
-
-        return true;
-    }
-
-    bool search_pillar_and_connect(const Head& head) {
-        PointIndex spindex = m_pillar_index;
-
-        long nearest_id = -1;
-
-        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_result.ground_level);
-            auto qres = spindex.nearest(qp, 1);
-            if(qres.empty()) break;
-
-            auto ne = qres.front();
-            nearest_id = ne.second;
-
-            if(nearest_id >= 0) {
-                auto nearpillarID = unsigned(nearest_id);
-                if(nearpillarID < m_result.pillarcount()) {
-                    if(!connect_to_nearpillar(head, nearpillarID)) {
-                        nearest_id = -1;    // continue searching
-                        spindex.remove(ne); // without the current pillar
-                    }
-                }
-            }
-        }
-
-        return nearest_id >= 0;
-    }
-
-    // This is a proxy function for pillar creation which will mind the gap
-    // between the pad and the model bottom in zero elevation mode.
-    void create_ground_pillar(const Vec3d &jp,
-                              const Vec3d &sourcedir,
-                              double       radius,
-                              int          head_id = -1)
-    {
-        // People were killed for this number (seriously)
-        static const double SQR2 = std::sqrt(2.0);
-        static const Vec3d  DOWN = {0.0, 0.0, -1.0};
-
-        double gndlvl       = m_result.ground_level;
-        Vec3d  endp         = {jp(X), jp(Y), gndlvl};
-        double sd           = m_cfg.pillar_base_safety_distance_mm;
-        int    pillar_id    = -1;
-        double min_dist     = sd + m_cfg.base_radius_mm + EPSILON;
-        double dist         = 0;
-        bool   can_add_base = true;
-        bool   normal_mode  = true;
-
-        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.
-
-            normal_mode     = false;
-            double mv       = min_dist - dist;
-            double azimuth  = std::atan2(sourcedir(Y), sourcedir(X));
-            double sinpolar = std::sin(PI - m_cfg.bridge_slope);
-            double cospolar = std::cos(PI - m_cfg.bridge_slope);
-            double cosazm   = std::cos(azimuth);
-            double sinazm   = std::sin(azimuth);
-
-            auto dir = Vec3d(cosazm * sinpolar, sinazm * sinpolar, cospolar)
-                           .normalized();
-
-            using namespace libnest2d::opt;
-            StopCriteria scr;
-            scr.stop_score = min_dist;
-            SubplexOptimizer solver(scr);
-
-            auto result = solver.optimize_max(
-                [this, dir, jp, gndlvl](double mv) {
-                    Vec3d endp = jp + SQR2 * mv * dir;
-                    endp(Z)    = gndlvl;
-                    return std::sqrt(m_mesh.squared_distance(endp));
-                },
-                initvals(mv), bound(0.0, 2 * min_dist));
-
-            mv           = std::get<0>(result.optimum);
-            endp         = jp + SQR2 * mv * 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_intersect(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 - SQR2 * (gndlvl - endp(Z)) * dir; // back off
-                else {
-
-                    auto hit = bridge_mesh_intersect(endp, DOWN, radius);
-                    if (!std::isinf(hit.distance())) abort_in_shame();
-
-                    Pillar &plr = m_result.add_pillar(endp, pgnd, radius);
-
-                    if (can_add_base)
-                        plr.add_base(m_cfg.base_height_mm,
-                                     m_cfg.base_radius_mm);
-
-                    pillar_id = plr.id;
-                }
-
-                m_result.add_bridge(jp, endp, radius);
-                m_result.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_result.add_pillar(unsigned(head_id), jp, radius);
-            }
-        }
-
-        if (normal_mode) {
-            Pillar &plr = head_id >= 0
-                              ? m_result.add_pillar(unsigned(head_id),
-                                                    endp,
-                                                    radius)
-                              : m_result.add_pillar(jp, endp, radius);
-
-            if (can_add_base)
-                plr.add_base(m_cfg.base_height_mm, m_cfg.base_radius_mm);
-
-            pillar_id = plr.id;
-        }
-
-        if(pillar_id >= 0) // Save the pillar endpoint in the spatial index
-            m_pillar_index.insert(endp, pillar_id);
-    }
-
-public:
-
-    Algorithm(const SupportConfig& config,
-              const EigenMesh3D& emesh,
-              const std::vector<SupportPoint>& support_pts,
-              Result& result,
-              ThrowOnCancel thr) :
-        m_cfg(config),
-        m_mesh(emesh),
-        m_support_pts(support_pts),
-        m_support_nmls(support_pts.size(), 3),
-        m_result(result),
-        m_points(support_pts.size(), 3),
-        m_thr(thr)
-    {
-        // 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));
-            m_points.row(i)(Y) = double(sp.pos(Y));
-            m_points.row(i)(Z) = double(sp.pos(Z));
-            ++i;
-        }
-    }
-
-
-    // Now let's define the individual steps of the support generation algorithm
-
-    // Filtering step: here we will discard inappropriate support points
-    // and decide the future of the appropriate ones. We will check if a
-    // pinhead is applicable and adjust its angle at each support point. We
-    // will also merge the support points that are just too close and can
-    // be considered as one.
-    void 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());
-        m_iheadless.reserve(aliases.size());
-        for(auto& a : aliases) {
-            // 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.
-
-        using libnest2d::opt::bound;
-        using libnest2d::opt::initvals;
-        using libnest2d::opt::GeneticOptimizer;
-        using libnest2d::opt::StopCriteria;
-
-        ccr::Mutex mutex;
-        auto addfn = [&mutex](PtIndices &container, unsigned val) {
-            std::lock_guard<ccr::Mutex> lk(mutex);
-            container.emplace_back(val);
-        };
-
-        ccr::enumerate(filtered_indices.begin(), filtered_indices.end(),
-                       [this, &nmls, addfn](unsigned fidx, size_t i)
-        {
-            m_thr();
-
-            auto n = nmls.row(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.
-
-            double z       = n(2);
-            double r       = 1.0; // for normalized vector
-            double polar   = std::acos(z / r);
-            double azimuth = std::atan2(n(1), n(0));
-
-            // 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 = Vec3d(std::cos(azimuth) * std::sin(polar),
-                                std::sin(azimuth) * std::sin(polar),
-                                std::cos(polar)).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 n = Vec3d(std::cos(azm) * std::sin(plr),
-                                           std::sin(azm) * std::sin(plr),
-                                           std::cos(plr)).normalized();
-
-                            double score = pinhead_mesh_intersect(
-                                hp, n, 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 = Vec3d(std::cos(azimuth) * std::sin(polar),
-                                   std::sin(azimuth) * std::sin(polar),
-                                   std::cos(polar)).normalized();
-                        t = 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_result.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);
-                }
-            }
-        });
-
-        m_thr();
-    }
-
-    // Pinhead creation: based on the filtering results, the Head objects
-    // will be constructed (together with their triangle meshes).
-    void add_pinheads()
-    {
-        for (unsigned i : m_iheads) {
-            m_thr();
-            m_result.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
-                        );
-        }
-    }
-
-    // Further classification of the support points with pinheads. If the
-    // ground is directly reachable through a vertical line parallel to the
-    // Z axis we consider a support point as pillar candidate. If touches
-    // the model geometry, it will be marked as non-ground facing and
-    // further steps will process it. Also, the pillars will be grouped
-    // into clusters that can be interconnected with bridges. Elements of
-    // these groups may or may not be interconnected. Here we only run the
-    // clustering algorithm.
-    void classify()
-    {
-        // We should first get the heads that reach the ground directly
-        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_result.head(i);
-            Vec3d n(0, 0, -1);
-            double r = head.r_back_mm;
-            Vec3d headjp = head.junction_point();
-
-            // collision check
-            auto hit = bridge_mesh_intersect(headjp, n, 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(std::make_pair(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_result.head(i).junction_point();
-        };
-
-        auto predicate = [this](const PointIndexEl &e1,
-                                const PointIndexEl &e2) {
-            double d2d = distance(to_2d(e1.first), to_2d(e2.first));
-            double d3d = distance(e1.first, e2.first);
-            return d2d < 2 * m_cfg.base_radius_mm
-                   && d3d < m_cfg.max_bridge_length_mm;
-        };
-
-        m_pillar_clusters = cluster(ground_head_indices,
-                                    pointfn,
-                                    predicate,
-                                    m_cfg.max_bridges_on_pillar);
-    }
-
-    // Step: Routing the ground connected pinheads, and interconnecting
-    // them with additional (angled) bridges. Not all of these pinheads
-    // will be a full pillar (ground connected). Some will connect to a
-    // nearby pillar using a bridge. The max number of such side-heads for
-    // a central pillar is limited to avoid bad weight distribution.
-    void routing_to_ground()
-    {
-        const double pradius = m_cfg.head_back_radius_mm;
-        // const double gndlvl = m_result.ground_level;
-
-        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
-            // for another head with a full pillar. Also when there are two
-            // 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;
-            long lcid = cluster_centroid(cl,
-                [&points](size_t idx) { return points.row(long(idx)); },
-                [thr](const Vec3d& p1, const Vec3d& p2)
-            {
-                thr();
-                return distance(Vec2d(p1(X), p1(Y)), Vec2d(p2(X), p2(Y)));
-            });
-
-            assert(lcid >= 0);
-            unsigned hid = cl[size_t(lcid)]; // Head ID
-
-            cl_centroids.emplace_back(hid);
-
-            Head& h = m_result.head(hid);
-            h.transform();
-
-            create_ground_pillar(h.junction_point(), h.dir, h.r_back_mm, h.id);
-        }
-
-        // 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_result.head_pillar(cidx).id;
-
-            for(auto c : cl) { m_thr();
-                if(c == cidx) continue;
-
-                auto& sidehead = m_result.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);
-                }
-            }
-        }
-    }
-
-    // Step: routing the pinheads that would connect to the model surface
-    // along the Z axis downwards. For now these will actually be connected with
-    // the model surface with a flipped pinhead. In the future here we could use
-    // some smart algorithms to search for a safe path to the ground or to a
-    // nearby pillar that can hold the supported weight.
-    void 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.
-
-        // First we want to index the available pillars. The best is to connect
-        // these points to the available pillars
-
-        auto routedown = [this](Head& head, const Vec3d& dir, double dist)
-        {
-            head.transform();
-            Vec3d hjp = head.junction_point();
-            Vec3d endp = hjp + dist * dir;
-            m_result.add_bridge(hjp, endp, head.r_back_mm);
-            m_result.add_junction(endp, head.r_back_mm);
-
-            this->create_ground_pillar(endp, dir, head.r_back_mm);
-        };
-
-        std::vector<unsigned> modelpillars;
-        ccr::Mutex mutex;
-
-        // TODO: connect these to the ground pillars if possible
-        ccr::enumerate(m_iheads_onmodel.begin(), m_iheads_onmodel.end(),
-                       [this, routedown, &modelpillars, &mutex]
-                       (const std::pair<unsigned, EigenMesh3D::hit_result> &el,
-                        size_t)
-        {
-            m_thr();
-            unsigned idx = el.first;
-            EigenMesh3D::hit_result hit = el.second;
-
-            auto& head = m_result.head(idx);
-            Vec3d hjp = head.junction_point();
-
-            // /////////////////////////////////////////////////////////////////
-            // Search nearby pillar
-            // /////////////////////////////////////////////////////////////////
-
-            if(search_pillar_and_connect(head)) { head.transform(); return; }
-
-            // /////////////////////////////////////////////////////////////////
-            // Try straight path
-            // /////////////////////////////////////////////////////////////////
-
-            // Cannot connect to nearby pillar. We will try to search for
-            // a route to the ground.
-
-            double t = bridge_mesh_intersect(hjp, head.dir, head.r_back_mm);
-            double d = 0, tdown = 0;
-            Vec3d dirdown(0.0, 0.0, -1.0);
-
-            t = std::min(t, m_cfg.max_bridge_length_mm);
-
-            while(d < t && !std::isinf(tdown = bridge_mesh_intersect(
-                                           hjp + d*head.dir,
-                                           dirdown, head.r_back_mm))) {
-                d += head.r_back_mm;
-            }
-
-            if(std::isinf(tdown)) { // we heave found a route to the ground
-                routedown(head, head.dir, d); return;
-            }
-
-            // /////////////////////////////////////////////////////////////////
-            // Optimize bridge direction
-            // /////////////////////////////////////////////////////////////////
-
-            // Straight path failed so we will try to search for a suitable
-            // direction out of the cavity.
-
-            // Get the spherical representation of the normal. its easier to
-            // work with.
-            double z = head.dir(Z);
-            double r = 1.0;     // for normalized vector
-            double polar = std::acos(z / r);
-            double azimuth = std::atan2(head.dir(Y), head.dir(X));
-
-            using libnest2d::opt::bound;
-            using libnest2d::opt::initvals;
-            using libnest2d::opt::GeneticOptimizer;
-            using libnest2d::opt::StopCriteria;
-
-            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);
-            solver.seed(0); // we want deterministic behavior
-
-            double r_back = head.r_back_mm;
-
-            auto oresult = solver.optimize_max(
-                        [this, hjp, r_back](double plr, double azm)
-            {
-                Vec3d n = Vec3d(std::cos(azm) * std::sin(plr),
-                               std::sin(azm) * std::sin(plr),
-                               std::cos(plr)).normalized();
-                return bridge_mesh_intersect(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
-            );
-
-            d = 0; t = oresult.score;
-
-            polar = std::get<0>(oresult.optimum);
-            azimuth = std::get<1>(oresult.optimum);
-            Vec3d bridgedir = Vec3d(std::cos(azimuth) * std::sin(polar),
-                              std::sin(azimuth) * std::sin(polar),
-                              std::cos(polar)).normalized();
-
-            t = std::min(t, m_cfg.max_bridge_length_mm);
-
-            while(d < t && !std::isinf(tdown = bridge_mesh_intersect(
-                                           hjp + d*bridgedir,
-                                           dirdown,
-                                           head.r_back_mm))) {
-                d += head.r_back_mm;
-            }
-
-            if(std::isinf(tdown)) { // we heave found a route to the ground
-                routedown(head, bridgedir, d); return;
-            }
-
-            // /////////////////////////////////////////////////////////////////
-            // Route to model body
-            // /////////////////////////////////////////////////////////////////
-
-            double zangle = std::asin(hit.direction()(Z));
-            zangle = std::max(zangle, PI/4);
-            double h = std::sin(zangle) * head.fullwidth();
-
-            // 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) {
-                Vec3d endp{hjp(X), hjp(Y), hjp(Z) - hit.distance() + h};
-                auto center_hit = m_mesh.query_ray_hit(hjp, dirdown);
-
-                double hitdiff = center_hit.distance() - hit.distance();
-                Vec3d hitp = std::abs(hitdiff) < 2*head.r_back_mm?
-                                center_hit.position() : hit.position();
-
-                head.transform();
-
-                Pillar& pill = m_result.add_pillar(unsigned(head.id),
-                                                   endp,
-                                                   head.r_back_mm);
-
-                Vec3d taildir = endp - hitp;
-                double dist = distance(endp, hitp) + m_cfg.head_penetration_mm;
-                double w = dist - 2 * head.r_pin_mm - head.r_back_mm;
-
-                Head tailhead(head.r_back_mm,
-                              head.r_pin_mm,
-                              w,
-                              m_cfg.head_penetration_mm,
-                              taildir,
-                              hitp);
-
-                tailhead.transform();
-                pill.base = tailhead.mesh;
-
-                // Experimental: add the pillar to the index for cascading
-                std::lock_guard<ccr::Mutex> lk(mutex);
-                modelpillars.emplace_back(unsigned(pill.id));
-                return;
-            }
-
-            // We have failed to route this head.
-            BOOST_LOG_TRIVIAL(warning)
-                    << "Failed to route model facing support point."
-                    << " ID: " << idx;
-            head.invalidate();
-        });
-
-        for(auto pillid : modelpillars) {
-            auto& pillar = m_result.pillar(pillid);
-            m_pillar_index.insert(pillar.endpoint(), pillid);
-        }
-    }
-
-    // Helper function for interconnect_pillars where pairs of already connected
-    // pillars should be checked for not to be processed again. This can be done
-    // in O(log) or even constant time with a set or an unordered set of hash
-    // values uniquely representing a pair of integers. The order of numbers
-    // within the pair should not matter, it has the same unique hash.
-    template<class I> static I pairhash(I a, I b)
-    {
-        using std::ceil; using std::log2; using std::max; using std::min;
-
-        static_assert(std::is_integral<I>::value,
-                      "This function works only for integral types.");
-
-        I g = min(a, b), l = max(a, b);
-
-        auto bits_g = g ? int(ceil(log2(g))) : 0;
-
-        // Assume the hash will fit into the output variable
-        assert((l ? (ceil(log2(l))) : 0) + bits_g < int(sizeof(I) * CHAR_BIT));
-
-        return (l << bits_g) + g;
-    }
-
-    void 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
-        // be connected multiple times this is ensured by the 'pairs' set which
-        // remembers the processed pillar pairs
-        auto cascadefn =
-                [this, d, &pairs, min_height_ratio, H1] (const PointIndexEl& el)
-        {
-            Vec3d qp = el.first;    // endpoint of the pillar
-
-            const Pillar& pillar = m_result.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;
-
-            // Query all remaining points within reach
-            auto qres = m_pillar_index.query([qp, d](const PointIndexEl& e){
-                return distance(e.first, qp) < 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_result.pillar(re.second);
-
-                // this neighbor is occupied, skip
-                if(neighborpillar.links >= neighbors) 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_result.increment_links(pillar);
-
-                    if(neighborpillar.height < H1 ||
-                       pillar.height / neighborpillar.height > min_height_ratio)
-                        m_result.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.
-        //
-        // 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_result.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_result.pillar(pid); };
-
-            // Decide how many additional pillars will be needed:
-
-            unsigned needpillars = 0;
-            if (pillar().bridges > m_cfg.max_bridges_on_pillar)
-                needpillars = 3;
-            else if (pillar().links < 2 && pillar().height > H2) {
-                // Not enough neighbors to support this pillar
-                needpillars = 2 - pillar().links;
-            } else if (pillar().links < 1 && pillar().height > H1) {
-                // No neighbors could be found and the pillar is too long.
-                needpillars = 1;
-            }
-
-            // 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_result.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]);
-                     n++)
-                {
-                    double a = alpha + n * PI / 3;
-                    Vec3d  s = sp;
-                    s(X) += std::cos(a) * r;
-                    s(Y) += std::sin(a) * r;
-                    spts[n] = s;
-
-                    // Check the path vertically down
-                    auto hr = bridge_mesh_intersect(s, {0, 0, -1}, 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);
-
-                if(interconnect(pillar(), p)) {
-                    Pillar& pp = m_result.add_pillar(p);
-                    m_pillar_index.insert(pp.endpoint(), unsigned(pp.id));
-
-                    m_result.add_junction(s, pillar().r);
-                    double t = bridge_mesh_intersect(pillarsp,
-                                                     dirv(pillarsp, s),
-                                                     pillar().r);
-                    if(distance(pillarsp, s) < t)
-                        m_result.add_bridge(pillarsp, s, pillar().r);
-
-                    if(pillar().endpoint()(Z) > m_result.ground_level)
-                        m_result.add_junction(pillar().endpoint(), pillar().r);
-
-                    newpills.emplace_back(pp.id);
-                    m_result.increment_links(pillar());
-                }
-            }
-
-            if(!newpills.empty()) {
-                for(auto it = newpills.begin(), nx = std::next(it);
-                    nx != newpills.end(); ++it, ++nx) {
-                    const Pillar& itpll = m_result.pillar(*it);
-                    const Pillar& nxpll = m_result.pillar(*nx);
-                    if(interconnect(itpll, nxpll)) {
-                        m_result.increment_links(itpll);
-                        m_result.increment_links(nxpll);
-                    }
-                }
-
-                m_pillar_index.foreach(cascadefn);
-            }
-        }
-    }
-
-    // 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 ()
-    {
-        // 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 = R/3;
-
-            // 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 dir = {0, 0, -1};
-            Vec3d sj = sp + R * n;              // stick start point
-
-            // This is only for checking
-            double idist = bridge_mesh_intersect(sph, dir, R, true);
-            double dist = ray_mesh_intersect(sj, dir);
-            if (std::isinf(dist))
-                dist = sph(Z) - m_mesh.ground_level()
-                       + m_mesh.ground_level_offset();
-
-            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;
-            }
-
-            Vec3d ej = sj + (dist + HWIDTH_MM)* dir;
-            m_result.add_compact_bridge(sp, ej, n, R, !std::isinf(dist));
-        }
-    }
-
-    void merge_result() { m_result.merge_and_cleanup(); }
-};
-
-bool SLASupportTree::generate(const std::vector<SupportPoint> &support_points,
-                              const EigenMesh3D& mesh,
-                              const SupportConfig &cfg,
-                              const Controller &ctl)
-{
-    if(support_points.empty()) return false;
-
-    Algorithm alg(cfg, mesh, support_points, *m_impl, ctl.cancelfn);
-
-    // Let's define the individual steps of the processing. We can experiment
-    // later with the ordering and the dependencies between them.
-    enum Steps {
-        BEGIN,
-        FILTER,
-        PINHEADS,
-        CLASSIFY,
-        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(&Algorithm::filter, &alg),
-
-        std::bind(&Algorithm::add_pinheads, &alg),
-
-        std::bind(&Algorithm::classify, &alg),
-
-        std::bind(&Algorithm::routing_to_ground, &alg),
-
-        std::bind(&Algorithm::routing_to_model, &alg),
-
-        std::bind(&Algorithm::interconnect_pillars, &alg),
-
-        std::bind(&Algorithm::routing_headless, &alg),
-
-        std::bind(&Algorithm::merge_result, &alg),
-
-        [] () {
-            // Done
-        },
-
-        [] () {
-            // Abort
-        }
-    };
-
-    Steps pc = BEGIN;
-
-    if(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 = [&ctl, &pc] () {
-        static const std::array<std::string, NUM_STEPS> stepstr {
-            "Starting",
-            "Filtering",
-            "Generate pinheads",
-            "Classification",
-            "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,
-            30,
-            50,
-            60,
-            70,
-            80,
-            85,
-            99,
-            100,
-            0
-        };
-
-        if(ctl.stopcondition()) pc = ABORT;
-
-        switch(pc) {
-        case BEGIN: pc = FILTER; break;
-        case FILTER: pc = PINHEADS; break;
-        case PINHEADS: pc = CLASSIFY; break;
-        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 MERGE_RESULT: pc = DONE; break;
-        case DONE:
-        case ABORT: break;
-        default: ;
-        }
-
-        ctl.statuscb(stepstate[pc], stepstr[pc]);
-    };
-
-    // Just here we run the computation...
-    while(pc < DONE) {
-        progress();
-        program[pc]();
-    }
-
-    return pc == ABORT;
-}
-
-SLASupportTree::SLASupportTree(double gnd_lvl): m_impl(new Impl()) {
-    m_impl->ground_level = gnd_lvl;
-}
-
-const TriangleMesh &SLASupportTree::merged_mesh() const
-{
-    return m_impl->merged_mesh();
-}
-
-void SLASupportTree::merged_mesh_with_pad(TriangleMesh &outmesh) const {
-    outmesh.merge(merged_mesh());
-    outmesh.merge(get_pad());
+void SupportTree::retrieve_full_mesh(TriangleMesh &outmesh) const {
+    outmesh.merge(retrieve_mesh(MeshType::Support));
+    outmesh.merge(retrieve_mesh(MeshType::Pad));
 }
 
-std::vector<ExPolygons> SLASupportTree::slice(
+std::vector<ExPolygons> SupportTree::slice(
     const std::vector<float> &grid, float cr) const
 {
-    const TriangleMesh &sup_mesh = m_impl->merged_mesh();
-    const TriangleMesh &pad_mesh = get_pad();
+    const TriangleMesh &sup_mesh = retrieve_mesh(MeshType::Support);
+    const TriangleMesh &pad_mesh = retrieve_mesh(MeshType::Pad);
 
     using Slices = std::vector<ExPolygons>;
     auto slices = reserve_vector<Slices>(2);
@@ -2609,7 +61,7 @@ std::vector<ExPolygons> SLASupportTree::slice(
         slices.emplace_back();
 
         TriangleMeshSlicer sup_slicer(&sup_mesh);
-        sup_slicer.slice(grid, cr, &slices.back(), m_impl->ctl().cancelfn);
+        sup_slicer.slice(grid, cr, &slices.back(), ctl().cancelfn);
     }
 
     if (!pad_mesh.empty()) {
@@ -2617,12 +69,13 @@ std::vector<ExPolygons> SLASupportTree::slice(
 
         auto bb = pad_mesh.bounding_box();
         auto maxzit = std::upper_bound(grid.begin(), grid.end(), bb.max.z());
-
-        auto padgrid = reserve_vector<float>(grid.end() - maxzit);
+        
+        auto cap = grid.end() - maxzit;
+        auto padgrid = reserve_vector<float>(size_t(cap > 0 ? cap : 0));
         std::copy(grid.begin(), maxzit, std::back_inserter(padgrid));
 
         TriangleMeshSlicer pad_slicer(&pad_mesh);
-        pad_slicer.slice(padgrid, cr, &slices.back(), m_impl->ctl().cancelfn);
+        pad_slicer.slice(padgrid, cr, &slices.back(), ctl().cancelfn);
     }
 
     size_t len = grid.size();
@@ -2644,33 +97,20 @@ std::vector<ExPolygons> SLASupportTree::slice(
     return mrg;
 }
 
-const TriangleMesh &SLASupportTree::add_pad(const ExPolygons& modelbase,
-                                            const PoolConfig& pcfg) const
-{
-    return m_impl->create_pad(merged_mesh(), modelbase, pcfg).tmesh;
-}
-
-const TriangleMesh &SLASupportTree::get_pad() const
+SupportTree::UPtr SupportTree::create(const SupportableMesh &sm,
+                                      const JobController &  ctl)
 {
-    return m_impl->pad().tmesh;
-}
-
-void SLASupportTree::remove_pad()
-{
-    m_impl->remove_pad();
-}
-
-SLASupportTree::SLASupportTree(const std::vector<SupportPoint> &points,
-                               const EigenMesh3D& emesh,
-                               const SupportConfig &cfg,
-                               const Controller &ctl):
-    m_impl(new Impl(ctl))
-{
-    m_impl->ground_level = emesh.ground_level() - cfg.object_elevation_mm;
-    generate(points, emesh, cfg, ctl);
+    auto builder = make_unique<SupportTreeBuilder>();
+    builder->m_ctl = ctl;
+    
+    if (sm.cfg.enabled) {
+        builder->build(sm);
+        builder->merge_and_cleanup();   // clean metadata, leave only the meshes.
+    } else {
+        builder->ground_level = sm.emesh.ground_level();
+    }
+    
+    return std::move(builder);
 }
 
-SLASupportTree::~SLASupportTree() {}
-
-}
-}
+}} // namespace Slic3r::sla
diff --git a/src/libslic3r/SLA/SLASupportTree.hpp b/src/libslic3r/SLA/SLASupportTree.hpp
index d7f15c17b..322b29251 100644
--- a/src/libslic3r/SLA/SLASupportTree.hpp
+++ b/src/libslic3r/SLA/SLASupportTree.hpp
@@ -2,24 +2,14 @@
 #define SLASUPPORTTREE_HPP
 
 #include <vector>
-#include <array>
-#include <cstdint>
 #include <memory>
 #include <Eigen/Geometry>
 
 #include "SLACommon.hpp"
-
+#include "SLAPad.hpp"
 
 namespace Slic3r {
 
-// Needed types from Point.hpp
-typedef int32_t coord_t;
-typedef Eigen::Matrix<double,   3, 1, Eigen::DontAlign> Vec3d;
-typedef Eigen::Matrix<float,    3, 1, Eigen::DontAlign> Vec3f;
-typedef Eigen::Matrix<coord_t,  3, 1, Eigen::DontAlign> Vec3crd;
-typedef std::vector<Vec3d>                              Pointf3s;
-typedef std::vector<Vec3crd>                            Points3;
-
 class TriangleMesh;
 class Model;
 class ModelInstance;
@@ -32,13 +22,17 @@ using ExPolygons = std::vector<ExPolygon>;
 
 namespace sla {
 
-enum class PillarConnectionMode {
+enum class PillarConnectionMode
+{
     zigzag,
     cross,
     dynamic
 };
 
-struct SupportConfig {
+struct SupportConfig
+{
+    bool   enabled = true;
+    
     // Radius in mm of the pointing side of the head.
     double head_front_radius_mm = 0.2;
 
@@ -85,6 +79,11 @@ struct SupportConfig {
     // The shortest distance between a pillar base perimeter from the model
     // body. This is only useful when elevation is set to zero.
     double pillar_base_safety_distance_mm = 0.5;
+    
+    double head_fullwidth() const {
+        return 2 * head_front_radius_mm + head_width_mm +
+               2 * head_back_radius_mm - head_penetration_mm;
+    }
 
     // /////////////////////////////////////////////////////////////////////////
     // Compile time configuration values (candidates for runtime)
@@ -104,101 +103,78 @@ struct SupportConfig {
     static const unsigned max_bridges_on_pillar;
 };
 
-struct PoolConfig;
+enum class MeshType { Support, Pad };
 
 /// A Control structure for the support calculation. Consists of the status
 /// indicator callback and the stop condition predicate.
-struct Controller {
-
+struct JobController
+{
+    using StatusFn = std::function<void(unsigned, const std::string&)>;
+    using StopCond = std::function<bool(void)>;
+    using CancelFn = std::function<void(void)>;
+    
     // This will signal the status of the calculation to the front-end
-    std::function<void(unsigned, const std::string&)> statuscb =
-            [](unsigned, const std::string&){};
-
+    StatusFn statuscb = [](unsigned, const std::string&){};
+    
     // Returns true if the calculation should be aborted.
-    std::function<bool(void)> stopcondition = [](){ return false; };
-
+    StopCond stopcondition = [](){ return false; };
+    
     // Similar to cancel callback. This should check the stop condition and
     // if true, throw an appropriate exception. (TriangleMeshSlicer needs this)
     // consider it a hard abort. stopcondition is permits the algorithm to
     // terminate itself
-    std::function<void(void)> cancelfn = [](){};
+    CancelFn cancelfn = [](){};
 };
 
-using PointSet = Eigen::MatrixXd;
-
-//EigenMesh3D to_eigenmesh(const TriangleMesh& m);
-
-// needed for find best rotation
-//EigenMesh3D to_eigenmesh(const ModelObject& model);
-
-// Simple conversion of 'vector of points' to an Eigen matrix
-//PointSet    to_point_set(const std::vector<sla::SupportPoint>&);
-
-
-/* ************************************************************************** */
+struct SupportableMesh
+{
+    EigenMesh3D   emesh;
+    SupportPoints pts;
+    SupportConfig cfg;
+
+    explicit SupportableMesh(const TriangleMesh & trmsh,
+                             const SupportPoints &sp,
+                             const SupportConfig &c)
+        : emesh{trmsh}, pts{sp}, cfg{c}
+    {}
+    
+    explicit SupportableMesh(const EigenMesh3D   &em,
+                             const SupportPoints &sp,
+                             const SupportConfig &c)
+        : emesh{em}, pts{sp}, cfg{c}
+    {}
+};
 
 /// The class containing mesh data for the generated supports.
-class SLASupportTree {
-    class Impl;     // persistent support data
-    std::unique_ptr<Impl> m_impl;
-
-    Impl& get() { return *m_impl; }
-    const Impl& get() const { return *m_impl; }
-
-    friend void add_sla_supports(Model&,
-                                 const SupportConfig&,
-                                 const Controller&);
-
-    // The generation algorithm is quite long and will be captured in a separate
-    // class with private data, helper methods, etc... This data is only needed
-    // during the calculation whereas the Impl class contains the persistent
-    // data, mostly the meshes.
-    class Algorithm;
-
-    // Generate the 3D supports for a model intended for SLA print. This
-    // will instantiate the Algorithm class and call its appropriate methods
-    // with status indication.
-    bool generate(const std::vector<SupportPoint>& pts,
-                  const EigenMesh3D& mesh,
-                  const SupportConfig& cfg = {},
-                  const Controller& ctl = {});
-
+class SupportTree
+{
+    JobController m_ctl;
 public:
-
-    SLASupportTree(double ground_level = 0.0);
-
-    SLASupportTree(const std::vector<SupportPoint>& pts,
-                   const EigenMesh3D& em,
-                   const SupportConfig& cfg = {},
-                   const Controller& ctl = {});
+    using UPtr = std::unique_ptr<SupportTree>;
     
-    SLASupportTree(const SLASupportTree&) = delete;
-    SLASupportTree& operator=(const SLASupportTree&) = delete;
-
-    ~SLASupportTree();
+    static UPtr create(const SupportableMesh &input,
+                       const JobController &ctl = {});
 
-    /// Get the whole mesh united into the output TriangleMesh
-    /// WITHOUT THE PAD
-    const TriangleMesh& merged_mesh() const;
+    virtual ~SupportTree() = default;
 
-    void merged_mesh_with_pad(TriangleMesh&) const;
+    virtual const TriangleMesh &retrieve_mesh(MeshType meshtype) const = 0;
 
-    std::vector<ExPolygons> slice(const std::vector<float> &,
-                                  float closing_radius) const;
-
-    /// Adding the "pad" (base pool) under the supports
+    /// Adding the "pad" under the supports.
     /// modelbase will be used according to the embed_object flag in PoolConfig.
-    /// If set, the plate will interpreted as the model's intrinsic pad. 
+    /// If set, the plate will be interpreted as the model's intrinsic pad. 
     /// Otherwise, the modelbase will be unified with the base plate calculated
     /// from the supports.
-    const TriangleMesh& add_pad(const ExPolygons& modelbase,
-                                const PoolConfig& pcfg) const;
-
-    /// Get the pad geometry
-    const TriangleMesh& get_pad() const;
-
-    void remove_pad();
-
+    virtual const TriangleMesh &add_pad(const ExPolygons &modelbase,
+                                        const PadConfig & pcfg) = 0;
+    
+    virtual void remove_pad() = 0;
+    
+    std::vector<ExPolygons> slice(const std::vector<float> &,
+                                  float closing_radius) const;
+    
+    void retrieve_full_mesh(TriangleMesh &outmesh) const;
+    
+    const JobController &ctl() const { return m_ctl; }
 };
 
 }
diff --git a/src/libslic3r/SLA/SLASupportTreeBuilder.cpp b/src/libslic3r/SLA/SLASupportTreeBuilder.cpp
new file mode 100644
index 000000000..2e0310ed8
--- /dev/null
+++ b/src/libslic3r/SLA/SLASupportTreeBuilder.cpp
@@ -0,0 +1,525 @@
+#include "SLASupportTreeBuilder.hpp"
+#include "SLASupportTreeBuildsteps.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.indices;
+    
+    // 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)
+            facets.emplace_back((i == 0) ?
+                                    Vec3crd(coord_t(ring.size()), 0, 1) :
+                                    Vec3crd(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(Vec3crd(id - 1, id,
+                                            id + coord_t(ring.size() - 1)));
+                facets.emplace_back(Vec3crd(id - 1, id_ringsize, id));
+            } else {
+                facets.emplace_back(Vec3crd(id_ringsize - 1, id_ringsize, id));
+                facets.emplace_back(Vec3crd(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(Vec3crd(id - 1, id_ringsize, id));
+            } else {
+                auto ci = coord_t(id_ringsize + coord_t(i));
+                facets.emplace_back(Vec3crd(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.indices;
+    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)
+    , 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.indices.emplace_back(i1s1, i2s1, i2s2);
+        mesh.indices.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.indices.emplace_back(i2s2, i2s1, i1s1);
+    mesh.indices.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.indices.swap(body.indices);
+    }
+}
+
+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.indices;
+    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,
+         const ExPolygons &  model_contours,
+         double              ground_level,
+         const PadConfig &   pcfg,
+         ThrowOnCancel       thr)
+    : cfg(pcfg)
+    , zlevel(ground_level + pcfg.full_height() - pcfg.required_elevation())
+{
+    thr();
+    
+    ExPolygons sup_contours;
+    
+    float zstart = float(zlevel);
+    float zend   = zstart + float(pcfg.full_height() + EPSILON);
+    
+    pad_blueprint(support_mesh, sup_contours, grid(zstart, zend, 0.1f), thr);
+    create_pad(sup_contours, model_contours, tmesh, pcfg);
+    
+    tmesh.translate(0, 0, float(zlevel));
+    if (!tmesh.empty()) tmesh.require_shared_vertices();
+}
+
+const TriangleMesh &SupportTreeBuilder::add_pad(const ExPolygons &modelbase,
+                                                const PadConfig & cfg)
+{
+    m_pad = Pad{merged_mesh(), modelbase, ground_level, cfg, ctl().cancelfn};
+    return m_pad.tmesh;
+}
+
+SupportTreeBuilder::SupportTreeBuilder(SupportTreeBuilder &&o)
+    : m_heads(std::move(o.m_heads))
+    , m_head_indices{std::move(o.m_head_indices)}
+    , 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}
+    , m_model_height{o.m_model_height}
+    , ground_level{o.ground_level}
+{}
+
+SupportTreeBuilder::SupportTreeBuilder(const SupportTreeBuilder &o)
+    : m_heads(o.m_heads)
+    , m_head_indices{o.m_head_indices}
+    , 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}
+    , m_model_height{o.m_model_height}
+    , ground_level{o.ground_level}
+{}
+
+SupportTreeBuilder &SupportTreeBuilder::operator=(SupportTreeBuilder &&o)
+{
+    m_heads = std::move(o.m_heads);
+    m_head_indices = std::move(o.m_head_indices);
+    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;
+    m_model_height = o.m_model_height;
+    ground_level = o.ground_level;
+    return *this;
+}
+
+SupportTreeBuilder &SupportTreeBuilder::operator=(const SupportTreeBuilder &o)
+{
+    m_heads = o.m_heads;
+    m_head_indices = o.m_head_indices;
+    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;
+    m_model_height = o.m_model_height;
+    ground_level = o.ground_level;
+    return *this;
+}
+
+const TriangleMesh &SupportTreeBuilder::merged_mesh() const
+{
+    if (m_meshcache_valid) return m_meshcache;
+    
+    Contour3D merged;
+    
+    for (auto &head : m_heads) {
+        if (ctl().stopcondition()) break;
+        if (head.is_valid()) merged.merge(head.mesh);
+    }
+    
+    for (auto &stick : m_pillars) {
+        if (ctl().stopcondition()) break;
+        merged.merge(stick.mesh);
+        merged.merge(stick.base);
+    }
+    
+    for (auto &j : m_junctions) {
+        if (ctl().stopcondition()) break;
+        merged.merge(j.mesh);
+    }
+    
+    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);
+    }
+    
+    for (auto &bs : m_crossbridges) {
+        if (ctl().stopcondition()) break;
+        merged.merge(bs.mesh);
+    }
+    
+    if (ctl().stopcondition()) {
+        // In case of failure we have to return an empty mesh
+        m_meshcache = TriangleMesh();
+        return m_meshcache;
+    }
+    
+    m_meshcache = mesh(merged);
+    
+    // The mesh will be passed by const-pointer to TriangleMeshSlicer,
+    // which will need this.
+    if (!m_meshcache.empty()) m_meshcache.require_shared_vertices();
+    
+    BoundingBoxf3 &&bb = m_meshcache.bounding_box();
+    m_model_height       = bb.max(Z) - bb.min(Z);
+    
+    m_meshcache_valid = true;
+    return m_meshcache;
+}
+
+double SupportTreeBuilder::full_height() const
+{
+    if (merged_mesh().empty() && !pad().empty())
+        return pad().cfg.full_height();
+    
+    double h = mesh_height();
+    if (!pad().empty()) h += pad().cfg.required_elevation();
+    return h;
+}
+
+const TriangleMesh &SupportTreeBuilder::merge_and_cleanup()
+{
+    // in case the mesh is not generated, it should be...
+    auto &ret = merged_mesh(); 
+    
+    // Doing clear() does not garantee to release the memory.
+    m_heads = {};
+    m_head_indices = {};
+    m_pillars = {};
+    m_junctions = {};
+    m_bridges = {};
+    m_compact_bridges = {};
+    
+    return ret;
+}
+
+const TriangleMesh &SupportTreeBuilder::retrieve_mesh(MeshType meshtype) const
+{
+    switch(meshtype) {
+    case MeshType::Support: return merged_mesh();
+    case MeshType::Pad:     return pad().tmesh;
+    }
+    
+    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);
+}
+
+}
+}
diff --git a/src/libslic3r/SLA/SLASupportTreeBuilder.hpp b/src/libslic3r/SLA/SLASupportTreeBuilder.hpp
new file mode 100644
index 000000000..c0d9f04c0
--- /dev/null
+++ b/src/libslic3r/SLA/SLASupportTreeBuilder.hpp
@@ -0,0 +1,496 @@
+#ifndef SUPPORTTREEBUILDER_HPP
+#define SUPPORTTREEBUILDER_HPP
+
+#include "SLAConcurrency.hpp"
+#include "SLABoilerPlate.hpp"
+#include "SLASupportTree.hpp"
+#include "SLAPad.hpp"
+#include <libslic3r/MTUtils.hpp>
+
+namespace Slic3r {
+namespace sla {
+
+/**
+ * Terminology:
+ *
+ * Support point:
+ * The point on the model surface that needs support.
+ *
+ * Pillar:
+ * A thick column that spans from a support point to the ground and has
+ * a thick cone shaped base where it touches the ground.
+ *
+ * Ground facing support point:
+ * A support point that can be directly connected with the ground with a pillar
+ * that does not collide or cut through the model.
+ *
+ * Non ground facing support point:
+ * A support point that cannot be directly connected with the ground (only with
+ * the model surface).
+ *
+ * Head:
+ * The pinhead that connects to the model surface with the sharp end end
+ * to a pillar or bridge stick with the dull end.
+ *
+ * Headless support point:
+ * A support point on the model surface for which there is not enough place for
+ * the head. It is either in a hole or there is some barrier that would collide
+ * with the head geometry. The headless support point can be ground facing and
+ * non ground facing as well.
+ *
+ * Bridge:
+ * A stick that connects two pillars or a head with a pillar.
+ *
+ * Junction:
+ * A small ball in the intersection of two or more sticks (pillar, bridge, ...)
+ *
+ * CompactBridge:
+ * A bridge that connects a headless support point with the model surface or a
+ * 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);
+}
+
+template<class Vec> double distance(const Vec& pp1, const Vec& pp2) {
+    auto p = pp2 - pp1;
+    return distance(p);
+}
+
+Contour3D sphere(double rho, Portion portion = make_portion(0.0, 2.0*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 ssteps, const Vec3d &sp = {0,0,0});
+
+const constexpr long ID_UNSET = -1;
+
+struct Head {
+    Contour3D mesh;
+    
+    size_t steps = 45;
+    Vec3d dir = {0, 0, -1};
+    Vec3d tr = {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;
+    
+    long bridge_id = ID_UNSET;
+    
+    inline void invalidate() { id = ID_UNSET; }
+    inline bool is_valid() const { return id >= 0; }
+    
+    Head(double r_big_mm,
+         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()
+    {
+        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;
+    }
+    
+    inline double fullwidth() const
+    {
+        return 2 * r_pin_mm + width_mm + 2*r_back_mm - penetration_mm;
+    }
+    
+    inline Vec3d junction_point() const
+    {
+        return tr + ( 2 * r_pin_mm + width_mm + r_back_mm - penetration_mm)*dir;
+    }
+    
+    inline double request_pillar_radius(double radius) const
+    {
+        const double rmax = r_back_mm;
+        return radius > 0 && radius < rmax ? radius : rmax;
+    }
+};
+
+struct Junction {
+    Contour3D mesh;
+    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;
+    }
+};
+
+struct Pillar {
+    Contour3D mesh;
+    Contour3D base;
+    double r = 1;
+    size_t steps = 0;
+    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
+    long start_junction_id = ID_UNSET;
+    
+    // How many bridges are connected to this pillar
+    unsigned bridges = 0;
+    
+    // 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
+    {
+        return {endpt(X), endpt(Y), endpt(Z) + height};
+    }
+    
+    inline const Vec3d& endpoint() const { return endpt; }
+    
+    Pillar& add_base(double baseheight = 3, double radius = 2);
+};
+
+// A Bridge between two pillars (with junction endpoints)
+struct Bridge {
+    Contour3D mesh;
+    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);
+};
+
+// 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);
+};
+
+// A wrapper struct around the pad
+struct Pad {
+    TriangleMesh tmesh;
+    PadConfig cfg;
+    double zlevel = 0;
+    
+    Pad() = default;
+    
+    Pad(const TriangleMesh &support_mesh,
+        const ExPolygons &  model_contours,
+        double              ground_level,
+        const PadConfig &   pcfg,
+        ThrowOnCancel       thr);
+    
+    bool empty() const { return tmesh.facets_count() == 0; }
+};
+
+// This class will hold the support tree meshes with some additional
+// bookkeeping as well. Various parts of the support geometry are stored
+// separately and are merged when the caller queries the merged mesh. The
+// merged result is cached for fast subsequent delivery of the merged mesh
+// which can be quite complex. The support tree creation algorithm can use an
+// instance of this class as a somewhat higher level tool for crafting the 3D
+// support mesh. Parts can be added with the appropriate methods such as
+// add_head or add_pillar which forwards the constructor arguments and fills
+// the IDs of these substructures. The IDs are basically indices into the
+// arrays of the appropriate type (heads, pillars, etc...). One can later query
+// e.g. a pillar for a specific head...
+//
+// The support pad is considered an auxiliary geometry and is not part of the
+// 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;    
+    Pad m_pad;
+    
+    using Mutex = ccr::SpinningMutex;
+    
+    mutable TriangleMesh m_meshcache;
+    mutable Mutex m_mutex;
+    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)
+    {
+        std::lock_guard<Mutex> lk(m_mutex);
+        br.emplace_back(std::forward<Args>(args)...);
+        br.back().id = long(br.size() - 1);
+        m_meshcache_valid = false;
+        return br.back();
+    }
+    
+public:
+    double ground_level = 0;
+    
+    SupportTreeBuilder() = default;
+    SupportTreeBuilder(SupportTreeBuilder &&o);
+    SupportTreeBuilder(const SupportTreeBuilder &o);
+    SupportTreeBuilder& operator=(SupportTreeBuilder &&o);
+    SupportTreeBuilder& operator=(const SupportTreeBuilder &o);
+
+    template<class...Args> Head& add_head(unsigned id, Args&&... args)
+    {
+        std::lock_guard<Mutex> lk(m_mutex);
+        m_heads.emplace_back(std::forward<Args>(args)...);
+        m_heads.back().id = id;
+        
+        if (id >= m_head_indices.size()) m_head_indices.resize(id + 1);
+        m_head_indices[id] = m_heads.size() - 1;
+        
+        m_meshcache_valid = false;
+        return m_heads.back();
+    }
+    
+    template<class...Args> long add_pillar(long headid, Args&&... args)
+    {
+        std::lock_guard<Mutex> lk(m_mutex);
+        if (m_pillars.capacity() < m_heads.size())
+            m_pillars.reserve(m_heads.size() * 10);
+        
+        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)...);
+        Pillar& pillar = m_pillars.back();
+        pillar.id = long(m_pillars.size() - 1);
+        head.pillar_id = pillar.id;
+        pillar.start_junction_id = head.id;
+        pillar.starts_from_head = true;
+        
+        m_meshcache_valid = false;
+        return pillar.id;
+    }
+    
+    void add_pillar_base(long pid, double baseheight = 3, double radius = 2)
+    {
+        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);
+    }
+    
+    void increment_bridges(const Pillar& pillar)
+    {
+        std::lock_guard<Mutex> lk(m_mutex);
+        assert(pillar.id >= 0 && size_t(pillar.id) < m_pillars.size());
+        
+        if(pillar.id >= 0 && size_t(pillar.id) < m_pillars.size())
+            m_pillars[size_t(pillar.id)].bridges++;
+    }
+    
+    void increment_links(const Pillar& pillar)
+    {
+        std::lock_guard<Mutex> lk(m_mutex);
+        assert(pillar.id >= 0 && size_t(pillar.id) < m_pillars.size());
+        
+        if(pillar.id >= 0 && size_t(pillar.id) < m_pillars.size()) 
+            m_pillars[size_t(pillar.id)].links++;
+    }
+    
+    unsigned bridgecount(const Pillar &pillar) const {
+        std::lock_guard<Mutex> lk(m_mutex);
+        assert(pillar.id >= 0 && size_t(pillar.id) < m_pillars.size());
+        return pillar.bridges;
+    }
+    
+    template<class...Args> long add_pillar(Args&&...args)
+    {
+        std::lock_guard<Mutex> lk(m_mutex);
+        if (m_pillars.capacity() < m_heads.size())
+            m_pillars.reserve(m_heads.size() * 10);
+        
+        m_pillars.emplace_back(std::forward<Args>(args)...);
+        Pillar& pillar = m_pillars.back();
+        pillar.id = long(m_pillars.size() - 1);
+        pillar.starts_from_head = false;
+        m_meshcache_valid = false;
+        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);
+        m_junctions.emplace_back(std::forward<Args>(args)...);
+        m_junctions.back().id = long(m_junctions.size() - 1);
+        m_meshcache_valid = false;
+        return m_junctions.back();
+    }
+    
+    const Bridge& add_bridge(const Vec3d &s, const Vec3d &e, double r, size_t n = 45)
+    {
+        return _add_bridge(m_bridges, s, e, r, n);
+    }
+    
+    const Bridge& add_bridge(long headid, const Vec3d &endp, size_t s = 45)
+    {
+        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.back().id = long(m_bridges.size() - 1);
+        
+        h.bridge_id = m_bridges.back().id;
+        m_meshcache_valid = false;
+        return m_bridges.back();
+    }
+    
+    template<class...Args> const Bridge& add_crossbridge(Args&&... args)
+    {
+        return _add_bridge(m_crossbridges, std::forward<Args>(args)...);
+    }
+    
+    template<class...Args> const CompactBridge& add_compact_bridge(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();
+    }
+    
+    Head &head(unsigned id)
+    {
+        std::lock_guard<Mutex> lk(m_mutex);
+        assert(id < m_head_indices.size());
+        
+        m_meshcache_valid = false;
+        return m_heads[m_head_indices[id]];
+    }
+    
+    inline size_t pillarcount() const {
+        std::lock_guard<Mutex> lk(m_mutex);
+        return m_pillars.size();
+    }
+    
+    inline const std::vector<Pillar> &pillars() const { return m_pillars; }
+    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; }
+    
+    template<class T> inline IntegerOnly<T, const Pillar&> pillar(T id) const
+    {
+        std::lock_guard<Mutex> lk(m_mutex);
+        assert(id >= 0 && size_t(id) < m_pillars.size() &&
+               size_t(id) < std::numeric_limits<size_t>::max());
+        
+        return m_pillars[size_t(id)];
+    }
+    
+    template<class T> inline IntegerOnly<T, Pillar&> pillar(T id) 
+    {
+        std::lock_guard<Mutex> lk(m_mutex);
+        assert(id >= 0 && size_t(id) < m_pillars.size() &&
+               size_t(id) < std::numeric_limits<size_t>::max());
+        
+        return m_pillars[size_t(id)];
+    }
+    
+    const Pad& pad() const { return m_pad; }
+    
+    // WITHOUT THE PAD!!!
+    const TriangleMesh &merged_mesh() const;
+    
+    // WITH THE PAD
+    double full_height() const;
+    
+    // WITHOUT THE PAD!!!
+    inline double mesh_height() const
+    {
+        if (!m_meshcache_valid) merged_mesh();
+        return m_model_height;
+    }
+    
+    // Intended to be called after the generation is fully complete
+    const TriangleMesh & merge_and_cleanup();
+    
+    // Implement SupportTree interface:
+
+    const TriangleMesh &add_pad(const ExPolygons &modelbase,
+                                const PadConfig & pcfg) override;
+    
+    void remove_pad() override { m_pad = Pad(); }
+    
+    virtual const TriangleMesh &retrieve_mesh(
+        MeshType meshtype = MeshType::Support) const override;
+
+    bool build(const SupportableMesh &supportable_mesh);
+};
+
+}} // namespace Slic3r::sla
+
+#endif // SUPPORTTREEBUILDER_HPP
diff --git a/src/libslic3r/SLA/SLASupportTreeBuildsteps.cpp b/src/libslic3r/SLA/SLASupportTreeBuildsteps.cpp
new file mode 100644
index 000000000..392a963e1
--- /dev/null
+++ b/src/libslic3r/SLA/SLASupportTreeBuildsteps.cpp
@@ -0,0 +1,1387 @@
+#include "SLASupportTreeBuildsteps.hpp"
+
+#include <libnest2d/optimizers/nlopt/genetic.hpp>
+#include <libnest2d/optimizers/nlopt/subplex.hpp>
+#include <boost/log/trivial.hpp>
+
+namespace Slic3r {
+namespace sla {
+
+SupportTreeBuildsteps::SupportTreeBuildsteps(SupportTreeBuilder &   builder,
+                                           const SupportableMesh &sm)
+    : m_cfg(sm.cfg)
+    , m_mesh(sm.emesh)
+    , m_support_pts(sm.pts)
+    , m_support_nmls(sm.pts.size(), 3)
+    , m_builder(builder)
+    , m_points(sm.pts.size(), 3)
+    , m_thr(builder.ctl().cancelfn)
+{
+    // 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));
+        m_points.row(i)(Y) = double(sp.pos(Y));
+        m_points.row(i)(Z) = double(sp.pos(Z));
+        ++i;
+    }
+}
+
+bool SupportTreeBuildsteps::execute(SupportTreeBuilder &   builder,
+                                   const SupportableMesh &sm)
+{
+    if(sm.pts.empty()) return false;
+    
+    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 {
+        BEGIN,
+        FILTER,
+        PINHEADS,
+        CLASSIFY,
+        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 {
+            "Starting",
+            "Filtering",
+            "Generate pinheads",
+            "Classification",
+            "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,
+            30,
+            50,
+            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;
+        case PINHEADS: pc = CLASSIFY; break;
+        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 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;
+}
+
+EigenMesh3D::hit_result SupportTreeBuildsteps::pinhead_mesh_intersect(
+    const Vec3d &s, const Vec3d &dir, double r_pin, double r_back, double width)
+{
+    static const size_t SAMPLES = 8;
+    
+    // method based on:
+    // https://math.stackexchange.com/questions/73237/parametric-equation-of-a-circle-in-3d-space
+    
+    // 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.
+    
+    // Move away slightly from the touching point to avoid raycasting on the
+    // inner surface of the mesh.
+    Vec3d v = dir;     // Our direction (axis)
+    Vec3d c = s + width * dir;
+    const double& sd = m_cfg.safety_distance_mm;
+    
+    // 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.
+    Vec3d a(0, 1, 0), b;
+    
+    // The portions of the circle (the head-back circle) for which we will
+    // shoot rays.
+    std::array<double, SAMPLES> phis;
+    for(size_t i = 0; i < phis.size(); ++i) phis[i] = i*2*PI/phis.size();
+    
+    auto& m = m_mesh;
+    using HitResult = EigenMesh3D::hit_result;
+    
+    // Hit results
+    std::array<HitResult, SAMPLES> hits;
+    
+    // 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'
+    auto chk1 = [] (double val) {
+        return std::abs(std::abs(val) - 1) < 1e-20;
+    };
+    
+    if(chk1(v(X)) || chk1(v(Y)) || chk1(v(Z))) {
+        a = {v(Z), v(X), v(Y)};
+        b = {v(Y), v(Z), v(X)};
+    }
+    else {
+        a(Z) = -(v(Y)*a(Y)) / v(Z); a.normalize();
+        b = a.cross(v);
+    }
+
+    // Now a and b vectors are perpendicular to v and to each other.
+    // Together they define the plane where we have to iterate with the
+    // given angles in the 'phis' vector
+    ccr::enumerate(
+        phis.begin(), phis.end(),
+        [&hits, &m, sd, r_pin, r_back, s, a, b, c](double phi, size_t i) {
+           double sinphi = std::sin(phi);
+           double cosphi = std::cos(phi);
+        
+           // Let's have a safety coefficient for the radiuses.
+           double rpscos = (sd + r_pin) * cosphi;
+           double rpssin = (sd + r_pin) * sinphi;
+           double rpbcos = (sd + r_back) * cosphi;
+           double rpbsin = (sd + r_back) * sinphi;
+        
+           // Point on the circle on the pin sphere
+           Vec3d ps(s(X) + rpscos * a(X) + rpssin * b(X),
+                    s(Y) + rpscos * a(Y) + rpssin * b(Y),
+                    s(Z) + rpscos * a(Z) + rpssin * b(Z));
+        
+           // 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).
+        
+           // This is the point on the circle on the back sphere
+           Vec3d p(c(X) + rpbcos * a(X) + rpbsin * b(X),
+                   c(Y) + rpbcos * a(Y) + rpbsin * b(Y),
+                   c(Z) + rpbcos * a(Z) + rpbsin * b(Z));
+        
+           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() > r_pin + sd) {
+                   // If we are inside the model and the hit
+                   // distance is bigger than our pin circle
+                   // diameter, it probably indicates that the
+                   // support point was already inside the
+                   // model, or there is really no space
+                   // around the point. We will assign a zero
+                   // hit distance to these cases which will
+                   // enforce the function return value to be
+                   // an invalid ray with zero hit distance.
+                   // (see min_element at the end)
+                   hits[i] = HitResult(0.0);
+               } else {
+                   // re-cast the ray from the outside of the
+                   // object. The starting point has an offset
+                   // of 2*safety_distance because the
+                   // original ray has also had an offset
+                   auto q2 = m.query_ray_hit(
+                       ps + (q.distance() + 2 * sd) * n, n);
+                   hits[i] = q2;
+               }
+           } else
+               hits[i] = q;
+        });
+
+    auto mit = std::min_element(hits.begin(), hits.end());
+    
+    return *mit;
+}
+
+EigenMesh3D::hit_result SupportTreeBuildsteps::bridge_mesh_intersect(
+    const Vec3d &s, const Vec3d &dir, double r, bool ins_check)
+{
+    static const size_t SAMPLES = 8;
+    
+    // helper vector calculations
+    Vec3d a(0, 1, 0), b;
+    const double& sd = m_cfg.safety_distance_mm;
+    
+    // INFO: for explanation of the method used here, see the previous
+    // method's comments.
+    
+    auto chk1 = [] (double val) {
+        return std::abs(std::abs(val) - 1) < 1e-20;
+    };
+    
+    if(chk1(dir(X)) || chk1(dir(Y)) || chk1(dir(Z))) {
+        a = {dir(Z), dir(X), dir(Y)};
+        b = {dir(Y), dir(Z), dir(X)};
+    }
+    else {
+        a(Z) = -(dir(Y)*a(Y)) / dir(Z); a.normalize();
+        b = a.cross(dir);
+    }
+    
+    // circle portions
+    std::array<double, SAMPLES> phis;
+    for(size_t i = 0; i < phis.size(); ++i) phis[i] = i*2*PI/phis.size();
+    
+    auto& m = m_mesh;
+    using HitResult = EigenMesh3D::hit_result;
+    
+    // Hit results
+    std::array<HitResult, SAMPLES> hits;
+    
+    ccr::enumerate(
+        phis.begin(), phis.end(),
+        [&m, a, b, sd, dir, r, s, ins_check, &hits] (double phi, size_t i) {
+            double sinphi = std::sin(phi);
+            double cosphi = std::cos(phi);
+            
+            // Let's have a safety coefficient for the radiuses.
+            double rcos = (sd + r) * cosphi;
+            double rsin = (sd + r) * sinphi;
+            
+            // Point on the circle on the pin sphere
+            Vec3d p (s(X) + rcos * a(X) + rsin * b(X),
+                    s(Y) + rcos * a(Y) + rsin * b(Y),
+                    s(Z) + rcos * a(Z) + rsin * b(Z));
+            
+            auto hr = m.query_ray_hit(p + sd*dir, dir);
+            
+            if(ins_check && hr.is_inside()) {
+                if(hr.distance() > 2 * r + sd) hits[i] = HitResult(0.0);
+                else {
+                    // re-cast the ray from the outside of the object
+                    auto hr2 =
+                        m.query_ray_hit(p + (hr.distance() + 2*sd)*dir, dir);
+                    
+                    hits[i] = hr2;
+                }
+            } else hits[i] = hr;
+        });
+    
+    auto mit = std::min_element(hits.begin(), hits.end());
+    
+    return *mit;
+}
+
+bool SupportTreeBuildsteps::interconnect(const Pillar &pillar,
+                                         const Pillar &nextpillar)
+{
+    // We need to get the starting point of the zig-zag pattern. We have to
+    // be aware that the two head junctions are at different heights. We
+    // may start from the lowest junction and call it a day but this
+    // strategy would leave unconnected a lot of pillar duos where the
+    // 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_intersect(sj, dirv(sj, ej), pillar.r) >= bridge_distance)
+        {
+            m_builder.add_crossbridge(sj, ej, pillar.r);
+            was_connected = true;
+        }
+        
+        // double bridging: (crosses)
+        if(docrosses) {
+            Vec3d sjback(ej(X), ej(Y), sj(Z));
+            Vec3d ejback(sj(X), sj(Y), ej(Z));
+            if (sjback(Z) <= slower(Z) && ejback(Z) >= eupper(Z) &&
+                bridge_mesh_intersect(sjback, dirv(sjback, ejback),
+                                      pillar.r) >= bridge_distance) {
+                // need to check collision for the cross stick
+                m_builder.add_crossbridge(sjback, ejback, pillar.r);
+                was_connected = true;
+            }
+        }
+        
+        sj.swap(ej);
+        ej(Z) = sj(Z) + zstep;
+    }
+    
+    return was_connected;
+}
+
+bool SupportTreeBuildsteps::connect_to_nearpillar(const Head &head,
+                                                  long        nearpillar_id)
+{
+    auto nearpillar = [this, nearpillar_id]() -> const Pillar& {
+        return m_builder.pillar(nearpillar_id);
+    };
+    
+    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_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_intersect(headjp, {0,0,-1}, 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;
+    if(bridgeend(Z) < minz) return false;
+    
+    double t = bridge_mesh_intersect(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_junction(bridgestart, r);
+            m_builder.add_bridge(bridgestart, bridgeend, head.r_back_mm);
+        } 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,
+                                                 const Vec3d &sourcedir,
+                                                 double       radius,
+                                                 long         head_id)
+{
+    // People were killed for this number (seriously)
+    static const double SQR2 = std::sqrt(2.0);
+    static const Vec3d  DOWN = {0.0, 0.0, -1.0};
+    
+    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 (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.
+        
+        normal_mode     = false;
+        double mind     = min_dist - dist;
+        double azimuth  = std::atan2(sourcedir(Y), sourcedir(X));
+        double sinpolar = std::sin(PI - m_cfg.bridge_slope);
+        double cospolar = std::cos(PI - m_cfg.bridge_slope);
+        double cosazm   = std::cos(azimuth);
+        double sinazm   = std::sin(azimuth);
+        
+        auto dir = Vec3d(cosazm * sinpolar, sinazm * sinpolar, cospolar)
+                       .normalized();
+        
+        using namespace libnest2d::opt;
+        StopCriteria scr;
+        scr.stop_score = min_dist;
+        SubplexOptimizer solver(scr);
+        
+        auto result = solver.optimize_max(
+            [this, dir, jp, gndlvl](double mv) {
+                Vec3d endpt = jp + SQR2 * mv * dir;
+                endpt(Z)    = gndlvl;
+                return std::sqrt(m_mesh.squared_distance(endpt));
+            },
+            initvals(mind), bound(0.0, 2 * min_dist));
+        
+        mind           = std::get<0>(result.optimum);
+        endp         = jp + SQR2 * mind * 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_intersect(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 - SQR2 * (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);
+            }
+            
+            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);
+        }
+    }
+    
+    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);
+    }
+    
+    if(pillar_id >= 0) // Save the pillar endpoint in the spatial index
+        m_pillar_index.guarded_insert(endp, unsigned(pillar_id));
+}
+
+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());
+    m_iheadless.reserve(aliases.size());
+    for(auto& a : aliases) {
+        // 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.
+    
+    using libnest2d::opt::bound;
+    using libnest2d::opt::initvals;
+    using libnest2d::opt::GeneticOptimizer;
+    using libnest2d::opt::StopCriteria;
+    
+    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) {
+        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.
+        
+        double z       = n(2);
+        double r       = 1.0; // for normalized vector
+        double polar   = std::acos(z / r);
+        double azimuth = std::atan2(n(1), n(0));
+        
+        // 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 = Vec3d(std::cos(azimuth) * std::sin(polar),
+                            std::sin(azimuth) * std::sin(polar),
+                            std::cos(polar)).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 = Vec3d(std::cos(azm) * std::sin(plr),
+                                         std::sin(azm) * std::sin(plr),
+                                         std::cos(plr)).normalized();
+                        
+                        double score = pinhead_mesh_intersect(
+                            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 = Vec3d(std::cos(azimuth) * std::sin(polar),
+                               std::sin(azimuth) * std::sin(polar),
+                               std::cos(polar)).normalized();
+                    t = 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);
+            }
+        }
+    };
+    
+    ccr::enumerate(filtered_indices.begin(), filtered_indices.end(), filterfn);
+    
+    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()
+{
+    // We should first get the heads that reach the ground directly
+    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);
+        Vec3d n(0, 0, -1);
+        double r = head.r_back_mm;
+        Vec3d headjp = head.junction_point();
+        
+        // collision check
+        auto hit = bridge_mesh_intersect(headjp, n, 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(std::make_pair(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));
+        double d3d = distance(e1.first, e2.first);
+        return d2d < 2 * m_cfg.base_radius_mm
+               && d3d < m_cfg.max_bridge_length_mm;
+    };
+
+    m_pillar_clusters = cluster(ground_head_indices, pointfn, predicate,
+                                m_cfg.max_bridges_on_pillar);
+}
+
+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
+        // for another head with a full pillar. Also when there are two
+        // 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;
+        long        lcid   = cluster_centroid(
+            cl, [&points](size_t idx) { return points.row(long(idx)); },
+            [thr](const Vec3d &p1, const Vec3d &p2) {
+                thr();
+                return distance(Vec2d(p1(X), p1(Y)), Vec2d(p2(X), p2(Y)));
+            });
+        
+        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);
+    }
+    
+    // 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);
+            }
+        }
+    }
+}
+
+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.
+    
+    // First we want to index the available pillars. The best is to connect
+    // these points to the available pillars
+    
+    auto routedown = [this](Head& head, const Vec3d& dir, double dist)
+    {
+        head.transform();
+        Vec3d endp = head.junction_point() + dist * 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);
+    };
+    
+    std::vector<unsigned> modelpillars;
+    ccr::SpinningMutex mutex;
+
+    auto onmodelfn =
+    [this, routedown, &modelpillars, &mutex]
+        (const std::pair<unsigned, EigenMesh3D::hit_result> &el, size_t)
+    {        
+        m_thr();
+        unsigned idx = el.first;
+        EigenMesh3D::hit_result hit = el.second;
+        
+        auto& head = m_builder.head(idx);
+        Vec3d hjp = head.junction_point();
+        
+        // /////////////////////////////////////////////////////////////////
+        // Search nearby pillar
+        // /////////////////////////////////////////////////////////////////
+        
+        if(search_pillar_and_connect(head)) { head.transform(); return; }
+        
+        // /////////////////////////////////////////////////////////////////
+        // Try straight path
+        // /////////////////////////////////////////////////////////////////
+        
+        // Cannot connect to nearby pillar. We will try to search for
+        // a route to the ground.
+        
+        double t = bridge_mesh_intersect(hjp, head.dir, head.r_back_mm);
+        double d = 0, tdown = 0;
+        Vec3d dirdown(0.0, 0.0, -1.0);
+        
+        t = std::min(t, m_cfg.max_bridge_length_mm);
+        
+        while(d < t && !std::isinf(tdown = bridge_mesh_intersect(
+                                        hjp + d*head.dir,
+                                        dirdown, head.r_back_mm))) {
+            d += head.r_back_mm;
+        }
+        
+        if(std::isinf(tdown)) { // we heave found a route to the ground
+            routedown(head, head.dir, d); return;
+        }
+        
+        // /////////////////////////////////////////////////////////////////
+        // Optimize bridge direction
+        // /////////////////////////////////////////////////////////////////
+        
+        // Straight path failed so we will try to search for a suitable
+        // direction out of the cavity.
+        
+        // Get the spherical representation of the normal. its easier to
+        // work with.
+        double z = head.dir(Z);
+        double r = 1.0;     // for normalized vector
+        double polar = std::acos(z / r);
+        double azimuth = std::atan2(head.dir(Y), head.dir(X));
+        
+        using libnest2d::opt::bound;
+        using libnest2d::opt::initvals;
+        using libnest2d::opt::GeneticOptimizer;
+        using libnest2d::opt::StopCriteria;
+        
+        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);
+        solver.seed(0); // we want deterministic behavior
+        
+        double r_back = head.r_back_mm;
+        
+        auto oresult = solver.optimize_max(
+            [this, hjp, r_back](double plr, double azm)
+            {
+                Vec3d n = Vec3d(std::cos(azm) * std::sin(plr),
+                                std::sin(azm) * std::sin(plr),
+                                std::cos(plr)).normalized();
+                return bridge_mesh_intersect(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
+            );
+        
+        d = 0; t = oresult.score;
+        
+        polar = std::get<0>(oresult.optimum);
+        azimuth = std::get<1>(oresult.optimum);
+        Vec3d bridgedir = Vec3d(std::cos(azimuth) * std::sin(polar),
+                                std::sin(azimuth) * std::sin(polar),
+                                std::cos(polar)).normalized();
+        
+        t = std::min(t, m_cfg.max_bridge_length_mm);
+        
+        while(d < t && !std::isinf(tdown = bridge_mesh_intersect(
+                                        hjp + d*bridgedir,
+                                        dirdown,
+                                        head.r_back_mm))) {
+            d += head.r_back_mm;
+        }
+        
+        if(std::isinf(tdown)) { // we heave found a route to the ground
+            routedown(head, bridgedir, d); return;
+        }
+        
+        // /////////////////////////////////////////////////////////////////
+        // Route to model body
+        // /////////////////////////////////////////////////////////////////
+        
+        double zangle = std::asin(hit.direction()(Z));
+        zangle = std::max(zangle, PI/4);
+        double h = std::sin(zangle) * head.fullwidth();
+        
+        // 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) {
+            Vec3d endp{hjp(X), hjp(Y), hjp(Z) - hit.distance() + h};
+            auto center_hit = m_mesh.query_ray_hit(hjp, dirdown);
+            
+            double hitdiff = center_hit.distance() - hit.distance();
+            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);
+            Pillar &pill = m_builder.pillar(pillar_id);
+
+            Vec3d taildir = endp - hitp;
+            double dist = distance(endp, hitp) + m_cfg.head_penetration_mm;
+            double w = dist - 2 * head.r_pin_mm - head.r_back_mm;
+            
+            if (w < 0.) {
+                BOOST_LOG_TRIVIAL(error) << "Pinhead width is negative!";
+                w = 0.;
+            }
+                
+            Head tailhead(head.r_back_mm,
+                          head.r_pin_mm,
+                          w,
+                          m_cfg.head_penetration_mm,
+                          taildir,
+                          hitp);
+            
+            tailhead.transform();
+            pill.base = tailhead.mesh;
+            
+            // Experimental: add the pillar to the index for cascading
+            std::lock_guard<ccr::SpinningMutex> lk(mutex);
+            modelpillars.emplace_back(unsigned(pill.id));
+            return;
+        }
+        
+        // We have failed to route this head.
+        BOOST_LOG_TRIVIAL(warning)
+            << "Failed to route model facing support point."
+            << " ID: " << idx;
+        head.invalidate();
+    };
+
+    ccr::enumerate(m_iheads_onmodel.begin(), m_iheads_onmodel.end(), onmodelfn);
+    
+    for(auto pillid : modelpillars) {
+        auto& pillar = m_builder.pillar(pillid);
+        m_pillar_index.insert(pillar.endpoint(), pillid);
+    }
+}
+
+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
+    // be connected multiple times this is ensured by the 'pairs' set which
+    // remembers the processed pillar pairs
+    auto cascadefn =
+        [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;
+        
+        // Query all remaining points within reach
+        auto qres = m_pillar_index.query([qp, d](const PointIndexEl& e){
+            return distance(e.first, qp) < 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(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.
+    //
+    // 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;
+        else if (pillar().links < 2 && pillar().height > H2) {
+            // Not enough neighbors to support this pillar
+            needpillars = 2;
+        } else if (pillar().links < 1 && pillar().height > H1) {
+            // 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]);
+                 n++)
+            {
+                double a = alpha + n * PI / 3;
+                Vec3d  s = sp;
+                s(X) += std::cos(a) * r;
+                s(Y) += std::sin(a) * r;
+                spts[n] = s;
+                
+                // Check the path vertically down
+                auto hr = bridge_mesh_intersect(s, {0, 0, -1}, 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);
+
+                if (interconnect(pillar(), p)) {
+                    Pillar &pp = m_builder.pillar(m_builder.add_pillar(p));
+                    m_pillar_index.insert(pp.endpoint(), unsigned(pp.id));
+
+                    m_builder.add_junction(s, pillar().r);
+                    double t = bridge_mesh_intersect(pillarsp,
+                                                     dirv(pillarsp, s),
+                                                     pillar().r);
+                    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);
+
+                    newpills.emplace_back(pp.id);
+                    m_builder.increment_links(pillar());
+                    m_builder.increment_links(pp);
+                }
+            }
+
+        if(!newpills.empty()) {
+            for(auto it = newpills.begin(), nx = std::next(it);
+                 nx != newpills.end(); ++it, ++nx) {
+                const Pillar& itpll = m_builder.pillar(*it);
+                const Pillar& nxpll = m_builder.pillar(*nx);
+                if(interconnect(itpll, nxpll)) {
+                    m_builder.increment_links(itpll);
+                    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 = 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 dir = {0, 0, -1};
+        Vec3d sj = sp + R * n;              // stick start point
+        
+        // This is only for checking
+        double idist = bridge_mesh_intersect(sph, dir, R, true);
+        double realdist = ray_mesh_intersect(sj, dir);
+        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) * dir;
+        m_builder.add_compact_bridge(sp, ej, n, R, use_endball);
+    }
+}
+
+}
+}
diff --git a/src/libslic3r/SLA/SLASupportTreeBuildsteps.hpp b/src/libslic3r/SLA/SLASupportTreeBuildsteps.hpp
new file mode 100644
index 000000000..b92e44dbd
--- /dev/null
+++ b/src/libslic3r/SLA/SLASupportTreeBuildsteps.hpp
@@ -0,0 +1,289 @@
+#ifndef SLASUPPORTTREEALGORITHM_H
+#define SLASUPPORTTREEALGORITHM_H
+
+#include <cstdint>
+
+#include "SLASupportTreeBuilder.hpp"
+
+namespace Slic3r {
+namespace sla {
+
+// The minimum distance for two support points to remain valid.
+const double /*constexpr*/ D_SP = 0.1;
+
+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)};
+}
+
+// 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)
+{
+    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: ;
+    }
+
+    // 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());
+}
+
+inline Vec3d dirv(const Vec3d& startp, const Vec3d& endp) {
+    return (endp - startp).normalized();
+}
+
+class PillarIndex {
+    PointIndex m_index;
+    using Mutex = ccr::BlockingMutex;
+    mutable Mutex m_mutex;
+
+public:
+
+    template<class...Args> inline void guarded_insert(Args&&...args)
+    {
+        std::lock_guard<Mutex> lck(m_mutex);
+        m_index.insert(std::forward<Args>(args)...);
+    }
+
+    template<class...Args>
+    inline std::vector<PointIndexEl> guarded_query(Args&&...args) const
+    {
+        std::lock_guard<Mutex> lck(m_mutex);
+        return m_index.query(std::forward<Args>(args)...);
+    }
+
+    template<class...Args> inline void insert(Args&&...args)
+    {
+        m_index.insert(std::forward<Args>(args)...);
+    }
+
+    template<class...Args>
+    inline std::vector<PointIndexEl> query(Args&&...args) const
+    {
+        return m_index.query(std::forward<Args>(args)...);
+    }
+
+    template<class Fn> inline void foreach(Fn fn) { m_index.foreach(fn); }
+    template<class Fn> inline void guarded_foreach(Fn fn)
+    {
+        std::lock_guard<Mutex> lck(m_mutex);
+        m_index.foreach(fn);
+    }
+
+    PointIndex guarded_clone()
+    {
+        std::lock_guard<Mutex> lck(m_mutex);
+        return m_index;
+    }
+};
+
+// Helper function for pillar interconnection where pairs of already connected
+// pillars should be checked for not to be processed again. This can be done
+// in constant time with a set of hash values uniquely representing a pair of
+// integers. The order of numbers within the pair should not matter, it has
+// the same unique hash. The hash value has to have twice as many bits as the
+// arguments need. If the same integral type is used for args and return val,
+// make sure the arguments use only the half of the type's bit depth.
+template<class I, class DoubleI = IntegerOnly<I>>
+IntegerOnly<DoubleI> pairhash(I a, I b)
+{
+    using std::ceil; using std::log2; using std::max; using std::min;
+    static const auto constexpr Ibits = int(sizeof(I) * CHAR_BIT);
+    static const auto constexpr DoubleIbits = int(sizeof(DoubleI) * CHAR_BIT);
+    static const auto constexpr shift = DoubleIbits / 2 < Ibits ? Ibits / 2 : Ibits;
+
+    I g = min(a, b), l = max(a, b);
+
+    // Assume the hash will fit into the output variable
+    assert((g ? (ceil(log2(g))) : 0) <= shift);
+    assert((l ? (ceil(log2(l))) : 0) <= shift);
+
+    return (DoubleI(g) << shift) + l;
+}
+
+class SupportTreeBuildsteps {
+    const SupportConfig& m_cfg;
+    const EigenMesh3D& m_mesh;
+    const std::vector<SupportPoint>& m_support_pts;
+
+    using PtIndices = std::vector<unsigned>;
+
+    PtIndices m_iheads;            // support points with pinhead
+    PtIndices m_iheadless;         // headless support points
+
+    // supp. pts. connecting to model: point index and the ray hit data
+    std::vector<std::pair<unsigned, EigenMesh3D::hit_result>> m_iheads_onmodel;
+
+    // normals for support points from model faces.
+    PointSet  m_support_nmls;
+
+    // Clusters of points which can reach the ground directly and can be
+    // bridged to one central pillar
+    std::vector<PtIndices> m_pillar_clusters;
+
+    // This algorithm uses the SupportTreeBuilder class to fill gradually
+    // the support elements (heads, pillars, bridges, ...)
+    SupportTreeBuilder& m_builder;
+
+    // support points in Eigen/IGL format
+    PointSet m_points;
+
+    // throw if canceled: It will be called many times so a shorthand will
+    // come in handy.
+    ThrowOnCancel m_thr;
+
+    // A spatial index to easily find strong pillars to connect to.
+    PillarIndex m_pillar_index;
+    
+    // When bridging heads to pillars... TODO: find a cleaner solution
+    ccr::BlockingMutex m_bridge_mutex;
+
+    inline double ray_mesh_intersect(const Vec3d& s,
+                                     const Vec3d& dir)
+    {
+        return m_mesh.query_ray_hit(s, dir).distance();
+    }
+
+    // This function will test if a future pinhead would not collide with the
+    // model geometry. It does not take a 'Head' object because those are
+    // created after this test. Parameters: s: The touching point on the model
+    // surface. dir: This is the direction of the head from the pin to the back
+    // r_pin, r_back: the radiuses of the pin and the back sphere width: This
+    // is the full width from the pin center to the back center m: The object
+    // mesh.
+    // The return value is the hit result from the ray casting. If the starting
+    // 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(
+        const Vec3d& s,
+        const Vec3d& dir,
+        double r_pin,
+        double r_back,
+        double width);
+
+    // Checking bridge (pillar and stick as well) intersection with the model.
+    // If the function is used for headless sticks, the ins_check parameter
+    // have to be true as the beginning of the stick might be inside the model
+    // geometry.
+    // The return value is the hit result from the ray casting. If the starting
+    // 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(
+        const Vec3d& s,
+        const Vec3d& dir,
+        double r,
+        bool ins_check = false);
+
+    // Helper function for interconnecting two pillars with zig-zag bridges.
+    bool interconnect(const Pillar& pillar, const Pillar& nextpillar);
+
+    // For connecting a head to a nearby pillar.
+    bool connect_to_nearpillar(const Head& head, long nearpillar_id);
+
+    bool search_pillar_and_connect(const Head& head);
+
+    // This is a proxy function for pillar creation which will mind the gap
+    // between the pad and the model bottom in zero elevation mode.
+    void create_ground_pillar(const Vec3d &jp,
+                              const Vec3d &sourcedir,
+                              double       radius,
+                              long         head_id = ID_UNSET);
+public:
+    SupportTreeBuildsteps(SupportTreeBuilder & builder, const SupportableMesh &sm);
+
+    // Now let's define the individual steps of the support generation algorithm
+
+    // Filtering step: here we will discard inappropriate support points
+    // and decide the future of the appropriate ones. We will check if a
+    // pinhead is applicable and adjust its angle at each support point. We
+    // will also merge the support points that are just too close and can
+    // be considered as one.
+    void filter();
+
+    // Pinhead creation: based on the filtering results, the Head objects
+    // will be constructed (together with their triangle meshes).
+    void add_pinheads();
+
+    // Further classification of the support points with pinheads. If the
+    // ground is directly reachable through a vertical line parallel to the
+    // Z axis we consider a support point as pillar candidate. If touches
+    // the model geometry, it will be marked as non-ground facing and
+    // further steps will process it. Also, the pillars will be grouped
+    // into clusters that can be interconnected with bridges. Elements of
+    // these groups may or may not be interconnected. Here we only run the
+    // clustering algorithm.
+    void classify();
+
+    // Step: Routing the ground connected pinheads, and interconnecting
+    // them with additional (angled) bridges. Not all of these pinheads
+    // will be a full pillar (ground connected). Some will connect to a
+    // nearby pillar using a bridge. The max number of such side-heads for
+    // a central pillar is limited to avoid bad weight distribution.
+    void routing_to_ground();
+
+    // Step: routing the pinheads that would connect to the model surface
+    // along the Z axis downwards. For now these will actually be connected with
+    // the model surface with a flipped pinhead. In the future here we could use
+    // some smart algorithms to search for a safe path to the ground or to a
+    // nearby pillar that can hold the supported weight.
+    void routing_to_model();
+
+    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);
+};
+
+}
+}
+
+#endif // SLASUPPORTTREEALGORITHM_H
diff --git a/src/libslic3r/SLA/SLASupportTreeIGL.cpp b/src/libslic3r/SLA/SLASupportTreeIGL.cpp
index 95d451271..05f8b1984 100644
--- a/src/libslic3r/SLA/SLASupportTreeIGL.cpp
+++ b/src/libslic3r/SLA/SLASupportTreeIGL.cpp
@@ -77,7 +77,7 @@ bool PointIndex::remove(const PointIndexEl& el)
 }
 
 std::vector<PointIndexEl>
-PointIndex::query(std::function<bool(const PointIndexEl &)> fn)
+PointIndex::query(std::function<bool(const PointIndexEl &)> fn) const
 {
     namespace bgi = boost::geometry::index;
 
@@ -86,7 +86,7 @@ PointIndex::query(std::function<bool(const PointIndexEl &)> fn)
     return ret;
 }
 
-std::vector<PointIndexEl> PointIndex::nearest(const Vec3d &el, unsigned k = 1)
+std::vector<PointIndexEl> PointIndex::nearest(const Vec3d &el, unsigned k = 1) const
 {
     namespace bgi = boost::geometry::index;
     std::vector<PointIndexEl> ret; ret.reserve(k);
@@ -104,6 +104,11 @@ 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
  * ************************************************************************** */
@@ -274,6 +279,8 @@ double EigenMesh3D::squared_distance(const Vec3d &p, int& i, Vec3d& c) const {
  * Misc functions
  * ****************************************************************************/
 
+namespace  {
+
 bool point_on_edge(const Vec3d& p, const Vec3d& e1, const Vec3d& e2,
                    double eps = 0.05)
 {
@@ -289,11 +296,13 @@ template<class Vec> double distance(const Vec& pp1, const Vec& pp2) {
     return std::sqrt(p.transpose() * p);
 }
 
+}
+
 PointSet normals(const PointSet& points,
                  const EigenMesh3D& mesh,
                  double eps,
                  std::function<void()> thr, // throw on cancel
-                 const std::vector<unsigned>& pt_indices = {})
+                 const std::vector<unsigned>& pt_indices)
 {
     if(points.rows() == 0 || mesh.V().rows() == 0 || mesh.F().rows() == 0)
         return {};
@@ -419,9 +428,17 @@ 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>(
@@ -433,25 +450,22 @@ ClusteredPoints cluster(Index3D &sindex,
     // 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);
-            auto cmp = [](const PointIndexEl& e1, const PointIndexEl& e2){
-                return e1.second < e2.second;
-            };
-
-            std::sort(tmp.begin(), tmp.end(), cmp);
+           
+            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);
+                                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);
+            std::sort(cluster.begin(), cluster.end(), cmp_ptidx_elements);
 
             if(!newpts.empty() && (!max_points || cluster.size() < max_points))
                 group(newpts, cluster);
@@ -479,7 +493,6 @@ ClusteredPoints cluster(Index3D &sindex,
     return result;
 }
 
-namespace {
 std::vector<PointIndexEl> distance_queryfn(const Index3D& sindex,
                                           const PointIndexEl& p,
                                           double dist,
@@ -496,7 +509,8 @@ std::vector<PointIndexEl> distance_queryfn(const Index3D& sindex,
 
     return tmp;
 }
-}
+
+} // namespace
 
 // Clustering a set of points by the given criteria
 ClusteredPoints cluster(
@@ -558,5 +572,5 @@ ClusteredPoints cluster(const PointSet& pts, double dist, unsigned max_points)
     });
 }
 
-}
-}
+} // namespace sla
+} // namespace Slic3r