From 8a78428d107d63f1ce00099c1105fb76abde7956 Mon Sep 17 00:00:00 2001
From: tamasmeszaros <meszaros.q@gmail.com>
Date: Wed, 25 May 2022 15:12:22 +0200
Subject: [PATCH] Add tests to find_merge_pt and make them pass

---
 src/libslic3r/BranchingTree/PointCloud.cpp | 16 +++--
 tests/sla_print/sla_print_tests.cpp        | 74 ++++++++++++++++++++++
 2 files changed, 86 insertions(+), 4 deletions(-)

diff --git a/src/libslic3r/BranchingTree/PointCloud.cpp b/src/libslic3r/BranchingTree/PointCloud.cpp
index f84b1dfc6..29284c1d7 100644
--- a/src/libslic3r/BranchingTree/PointCloud.cpp
+++ b/src/libslic3r/BranchingTree/PointCloud.cpp
@@ -14,19 +14,27 @@ std::optional<Vec3f> find_merge_pt(const Vec3f &A,
     Vec3f Da = (B - A).normalized(), Db = -Da;
     auto [polar_da, azim_da] = Geometry::dir_to_spheric(Da);
     auto [polar_db, azim_db] = Geometry::dir_to_spheric(Db);
-    polar_da = std::max(polar_da, float(PI) - max_slope);
-    polar_db = std::max(polar_db, float(PI) - max_slope);
+    polar_da = std::max(polar_da, float(PI) / 2.f + max_slope);
+    polar_db = std::max(polar_db, float(PI) / 2.f + max_slope);
 
     Da = Geometry::spheric_to_dir<float>(polar_da, azim_da);
     Db = Geometry::spheric_to_dir<float>(polar_db, azim_db);
 
+    // This formula is based on
+    // https://stackoverflow.com/questions/27459080/given-two-points-and-two-direction-vectors-find-the-point-where-they-intersect
     double t1 =
         (A.z() * Db.x() + Db.z() * B.x() - B.z() * Db.x() - Db.z() * A.x()) /
         (Da.x() * Db.z() - Da.z() * Db.x());
 
-    double t2 = (A.x() + Da.x() * t1 - B.x()) / Db.x();
+    if (std::isnan(t1) || std::abs(t1) < EPSILON)
+        t1 = (A.z() * Db.y() + Db.z() * B.y() - B.z() * Db.y() - Db.z() * A.y()) /
+             (Da.y() * Db.z() - Da.z() * Db.y());
 
-    return t1 > 0. && t2 > 0. ? A + t1 * Da : std::optional<Vec3f>{};
+    Vec3f m1 = A + t1 * Da;
+
+    double t2 = (m1.z() - B.z()) / Db.z();
+
+    return t1 >= 0. && t2 >= 0. ? m1 : std::optional<Vec3f>{};
 }
 
 void to_eigen_mesh(const indexed_triangle_set &its,
diff --git a/tests/sla_print/sla_print_tests.cpp b/tests/sla_print/sla_print_tests.cpp
index 8ae02c808..fa64b0c10 100644
--- a/tests/sla_print/sla_print_tests.cpp
+++ b/tests/sla_print/sla_print_tests.cpp
@@ -8,6 +8,7 @@
 
 #include <libslic3r/TriangleMeshSlicer.hpp>
 #include <libslic3r/SLA/SupportTreeMesher.hpp>
+#include <libslic3r/BranchingTree/PointCloud.hpp>
 
 namespace {
 
@@ -172,6 +173,79 @@ TEST_CASE("DefaultSupports::FloorSupportsDoNotPierceModel", "[SLASupportGenerati
 //    for (auto &fname: SUPPORT_TEST_MODELS) test_supports(fname, supportcfg);
 //}
 
+TEST_CASE("BranchingSupports::MergePointFinder", "[SLASupportGeneration][Branching]") {
+    SECTION("Identical points have the same merge point") {
+        Vec3f a{0.f, 0.f, 0.f}, b = a;
+        float slope = PI / 4.f;
+
+        auto mergept = branchingtree::find_merge_pt(a, b, slope);
+
+        REQUIRE(bool(mergept));
+        REQUIRE((*mergept - b).norm() < EPSILON);
+        REQUIRE((*mergept - a).norm() < EPSILON);
+    }
+
+    // ^ Z
+    // | a *
+    // |
+    // | b * <= mergept
+    SECTION("Points at different heights have the lower point as mergepoint") {
+        Vec3f a{0.f, 0.f, 0.f}, b = {0.f, 0.f, -1.f};
+        float slope = PI / 4.f;
+
+        auto mergept = branchingtree::find_merge_pt(a, b, slope);
+
+        REQUIRE(bool(mergept));
+        REQUIRE((*mergept - b).squaredNorm() < EPSILON);
+    }
+
+    // -|---------> X
+    //  a       b
+    //  *       *
+    //      * <= mergept
+    SECTION("Points at different X have mergept in the middle at lower Z") {
+        Vec3f a{0.f, 0.f, 0.f}, b = {1.f, 0.f, 0.f};
+        float slope = PI / 4.f;
+
+        auto mergept = branchingtree::find_merge_pt(a, b, slope);
+
+        REQUIRE(bool(mergept));
+
+           // Distance of mergept should be equal from both input points
+        float D = std::abs((*mergept - b).squaredNorm() - (*mergept - a).squaredNorm());
+
+        REQUIRE(D < EPSILON);
+    }
+
+    // -|---------> Y
+    //  a       b
+    //  *       *
+    //      * <= mergept
+    SECTION("Points at different Y have mergept in the middle at lower Z") {
+        Vec3f a{0.f, 0.f, 0.f}, b = {0.f, 1.f, 0.f};
+        float slope = PI / 4.f;
+
+        auto mergept = branchingtree::find_merge_pt(a, b, slope);
+
+        REQUIRE(bool(mergept));
+
+        // Distance of mergept should be equal from both input points
+        float D = std::abs((*mergept - b).squaredNorm() - (*mergept - a).squaredNorm());
+
+        REQUIRE(D < EPSILON);
+    }
+
+    SECTION("Points separated by less than critical angle have the lower point as mergept") {
+        Vec3f a{0.f, 0.f, 0.f}, b = {-0.5f, -0.5f, -1.f};
+        float slope = PI / 4.f;
+
+        auto mergept = branchingtree::find_merge_pt(a, b, slope);
+
+        REQUIRE(bool(mergept));
+        REQUIRE((*mergept - b).norm() < EPSILON);
+    }
+}
+
 TEST_CASE("BranchingSupports::ElevatedSupportsDoNotPierceModel", "[SLASupportGeneration][Branching]") {
 
     sla::SupportTreeConfig supportcfg;