From a968b61d02f59c9355ed85a474138f819b7bd367 Mon Sep 17 00:00:00 2001
From: Andrej730 <azhilenkov@gmail.com>
Date: Mon, 13 Jan 2025 13:22:04 +0500
Subject: [PATCH] np_matrix_to_euler, np_matrix_normalized

---
 .../ifcopenshell/util/shape_builder.py        | 25 +++++++++++++++++++
 .../test/util/test_shape_builder.py           | 16 ++++++++++++
 2 files changed, 41 insertions(+)

diff --git a/src/ifcopenshell-python/ifcopenshell/util/shape_builder.py b/src/ifcopenshell-python/ifcopenshell/util/shape_builder.py
index c7dd729dd3..6cbd7293f5 100644
--- a/src/ifcopenshell-python/ifcopenshell/util/shape_builder.py
+++ b/src/ifcopenshell-python/ifcopenshell/util/shape_builder.py
@@ -90,6 +90,14 @@ def np_normalized(v: VectorType) -> np.ndarray:
     return np.divide(v, np.linalg.norm(v))
 
 
+def np_matrix_normalized(matrix: np.ndarray) -> np.ndarray:
+    # Ensure translation is not affected.
+    scale_factors = np.linalg.norm(matrix[:3, :3], axis=0)
+    rotation_matrix = matrix.copy()
+    rotation_matrix[:3, :3] /= scale_factors
+    return rotation_matrix
+
+
 def np_lerp(a: VectorType, b: VectorType, t: float) -> np.ndarray:
     return a + np.subtract(b, a) * t
 
@@ -178,6 +186,23 @@ def np_rotation_matrix(
     return matrix
 
 
+def np_matrix_to_euler(matrix: np.ndarray) -> tuple[float, float, float]:
+    """Convert a rotation matrix to Euler angles.
+
+    Designed to work similar to `mathutils.Matrix.to_euler`.
+    Currently only XYZ rotation is supported.
+    """
+    if matrix.shape not in ((3, 3), (4, 4)):
+        raise ValueError(f"Matrix must be 3x3 or 4x4, got {matrix.shape}.")
+
+    matrix = np_matrix_normalized(matrix)
+    y = -np.arcsin(matrix[2, 0])
+    cos_y = np.cos(y)
+    x = np.arctan2(matrix[2, 1] / cos_y, matrix[2, 2] / cos_y)
+    z = np.arctan2(matrix[1, 0] / cos_y, matrix[0, 0] / cos_y)
+    return (x, y, z)
+
+
 def np_normal(vectors: SequenceOfVectors) -> np.ndarray:
     """Normal of 3D Polygon.
 
diff --git a/src/ifcopenshell-python/test/util/test_shape_builder.py b/src/ifcopenshell-python/test/util/test_shape_builder.py
index 019fc50d3a..fa7fb96fd6 100644
--- a/src/ifcopenshell-python/test/util/test_shape_builder.py
+++ b/src/ifcopenshell-python/test/util/test_shape_builder.py
@@ -30,6 +30,7 @@ from ifcopenshell.util.shape_builder import (
     np_angle_signed,
     np_normal,
     np_intersect_line_line,
+    np_matrix_to_euler,
 )
 from math import degrees, radians
 from typing import Any, Union
@@ -57,6 +58,21 @@ class TestMathutilsCompatibleMethods(test.bootstrap.IFC4):
         rotation_vector_args = radians(45), 4, Vector((1, 1, 1)).normalized()
         assert np.allclose(Matrix.Rotation(*rotation_vector_args), np_rotation_matrix(*rotation_vector_args))
 
+    def test_np_matrix_to_euler(self):
+        from mathutils import Euler
+
+        # Test 3x3.
+        rot = Euler((0.5, 0.5, 0.5)).to_matrix()
+        assert np.allclose(rot.to_euler(), np_matrix_to_euler(V(rot)))
+
+        rot = rot.to_4x4()
+        assert np.allclose(rot.to_euler(), np_matrix_to_euler(V(rot)))
+
+        # Ensure support scaled matrices.
+        rot = Euler((0.5, 0.5, 0.5)).to_matrix()
+        rot.col[0] *= 2
+        assert np.allclose(rot.to_euler(), np_matrix_to_euler(V(rot)))
+
     def test_np_angle(self):
         from mathutils import Vector