1 files changed, 55 insertions, 3 deletions

diff --git a/src/libslic3r/SLA/Clustering.hpp b/src/libslic3r/SLA/Clustering.hpp
index 1b0d47d95..269ec2882 100644
--- a/src/libslic3r/SLA/Clustering.hpp
+++ b/src/libslic3r/SLA/Clustering.hpp
@@ -2,7 +2,8 @@
 #define SLA_CLUSTERING_HPP
 
 #include <vector>
-#include <libslic3r/SLA/Common.hpp>
+
+#include <libslic3r/Point.hpp>
 #include <libslic3r/SLA/SpatIndex.hpp>
 
 namespace Slic3r { namespace sla {
@@ -16,7 +17,7 @@ ClusteredPoints cluster(const std::vector<unsigned>& indices,
                         double dist,
                         unsigned max_points);
 
-ClusteredPoints cluster(const PointSet& points,
+ClusteredPoints cluster(const Eigen::MatrixXd& points,
                         double dist,
                         unsigned max_points);
 
@@ -26,5 +27,56 @@ ClusteredPoints cluster(
     std::function<bool(const PointIndexEl&, const PointIndexEl&)> predicate,
     unsigned max_points);
 
-}}
+// This function returns the position of the centroid in the input 'clust'
+// vector of point indices.
+template<class DistFn, class PointFn>
+long cluster_centroid(const ClusterEl &clust, PointFn pointfn, DistFn df)
+{
+    switch(clust.size()) {
+    case 0: /* empty cluster */ return -1;
+    case 1: /* only one element */ return 0;
+    case 2: /* if two elements, there is no center */ return 0;
+    default: ;
+    }
+
+    // The function works by calculating for each point the average distance
+    // from all the other points in the cluster. We create a selector bitmask of
+    // the same size as the cluster. The bitmask will have two true bits and
+    // false bits for the rest of items and we will loop through all the
+    // permutations of the bitmask (combinations of two points). Get the
+    // distance for the two points and add the distance to the averages.
+    // The point with the smallest average than wins.
+
+    // The complexity should be O(n^2) but we will mostly apply this function
+    // for small clusters only (cca 3 elements)
+
+    std::vector<bool> sel(clust.size(), false);   // create full zero bitmask
+    std::fill(sel.end() - 2, sel.end(), true);    // insert the two ones
+    std::vector<double> avgs(clust.size(), 0.0);  // store the average distances
+
+    do {
+        std::array<size_t, 2> idx;
+        for(size_t i = 0, j = 0; i < clust.size(); i++)
+            if(sel[i]) idx[j++] = i;
+
+        double d = df(pointfn(clust[idx[0]]),
+                      pointfn(clust[idx[1]]));
+
+        // add the distance to the sums for both associated points
+        for(auto i : idx) avgs[i] += d;
+
+        // now continue with the next permutation of the bitmask with two 1s
+    } while(std::next_permutation(sel.begin(), sel.end()));
+
+    // Divide by point size in the cluster to get the average (may be redundant)
+    for(auto& a : avgs) a /= clust.size();
+
+    // get the lowest average distance and return the index
+    auto minit = std::min_element(avgs.begin(), avgs.end());
+    return long(minit - avgs.begin());
+}
+
+
+}} // namespace Slic3r::sla
+
 #endif // CLUSTERING_HPP