From ea838d3cc1694cbfbafc71d34d3a10c53553e909 Mon Sep 17 00:00:00 2001
From: Arthur <arthur.tang@bambulab.com>
Date: Wed, 10 Apr 2024 12:20:38 +0800
Subject: [PATCH] FIX: auto-orient unstable due to numerical accuracy

jira: STUDIO-6464
Change-Id: I644bba2eea8cba6c73d2997c9399718b12eab9f2
(cherry picked from commit cccf57c75c3c348f918ededfbabb1eede5ac2ebd)
---
 src/libslic3r/Geometry.cpp | 36 ++++++------------------------------
 1 file changed, 6 insertions(+), 30 deletions(-)

diff --git a/src/libslic3r/Geometry.cpp b/src/libslic3r/Geometry.cpp
index 47915817a..84b967e0f 100644
--- a/src/libslic3r/Geometry.cpp
+++ b/src/libslic3r/Geometry.cpp
@@ -343,36 +343,12 @@ Vec3d extract_euler_angles(const Transform3d& transform)
 
 void rotation_from_two_vectors(Vec3d from, Vec3d to, Vec3d& rotation_axis, double& phi, Matrix3d* rotation_matrix)
 {
-    double epsilon = 1e-5;
-    // note: a.isMuchSmallerThan(b,prec) compares a.abs().sum()<b*prec, so previously we set b=0 && prec=dummpy_prec() is wrong
-    if ((from + to).isMuchSmallerThan(1, epsilon))
-    {
-        rotation_axis << 1, 0, 0;
-        phi = M_PI;
-        if (rotation_matrix)
-            *rotation_matrix = -Matrix3d::Identity();
-    }
-    else if ((from - to).isMuchSmallerThan(1, epsilon)) {
-        rotation_axis << 1, 0, 0;
-        phi = 0;
-        if (rotation_matrix)
-            *rotation_matrix = Matrix3d::Identity();
-    }
-    else {
-        rotation_axis = from.cross(to);
-        double s = rotation_axis.norm(); // sin(phi)
-        double c = from.dot(to); // cos(phi)
-        auto& v = rotation_axis;
-        Matrix3d kmat;
-        kmat << 0, -v[2], v[1],
-            v[2], 0, -v[0],
-            -v[1], v[0], 0;
-
-        rotation_axis.normalize();
-        phi = acos(std::min(from.dot(to), 1.0));
-        if (rotation_matrix)
-            *rotation_matrix = Matrix3d::Identity() + kmat + kmat * kmat * ((1 - c) / (s * s));
-    }
+    const Matrix3d m = Eigen::Quaterniond().setFromTwoVectors(from, to).toRotationMatrix();
+    const Eigen::AngleAxisd aa(m);
+    rotation_axis = aa.axis();
+    phi           = aa.angle();
+    if (rotation_matrix)
+        *rotation_matrix = m;
 }
 
 Transform3d translation_transform(const Vec3d &translation)