fix normal

2024-10-19 18:23:34 +08:00
parent 1f8c017a01
commit 7d25777983
11 changed files with 147 additions and 48 deletions
--- a/utils/pose.py
+++ b/utils/pose.py
@@ -149,3 +149,18 @@ class PoseUtil:
        if debug:
            print("uniform scale:", scale)
        return scale
+
+    @staticmethod
+    def rotation_matrix_from_axis_angle(axis, angle):
+        cos_angle = np.cos(angle)
+        sin_angle = np.sin(angle)
+        one_minus_cos = 1 - cos_angle
+
+        x, y, z = axis
+        rotation_matrix = np.array([
+            [cos_angle + x*x*one_minus_cos, x*y*one_minus_cos - z*sin_angle, x*z*one_minus_cos + y*sin_angle],
+            [y*x*one_minus_cos + z*sin_angle, cos_angle + y*y*one_minus_cos, y*z*one_minus_cos - x*sin_angle],
+            [z*x*one_minus_cos - y*sin_angle, z*y*one_minus_cos + x*sin_angle, cos_angle + z*z*one_minus_cos]
+        ])
+
+        return rotation_matrix