Merge pull request #4 from HiraiKyo/feature/get_rotation_from_vectors

Add feature: Get rotation matrix from vector to vector

.gitignore

@@ -1,4 +1,3 @@
-matrix/
 points/
 pcd/
 

ply_processor_basics/matrix/__init__.py

@@ -0,0 +1 @@
+from .get_rotation_from_vectors import get_rotation_from_vectors as get_rotation_from_vectors

ply_processor_basics/matrix/get_rotation_from_vectors.py

@@ -0,0 +1,29 @@
+import numpy as np
+from numpy.typing import NDArray
+from scipy.spatial.transform import Rotation
+
+from ply_processor_basics.vector import normalize
+
+
+def get_rotation_from_vectors(vec1: NDArray[np.float32], vec2: NDArray[np.float32]):
+    """_summary_
+
+    Args:
+        vec1: The vector from.
+        vec2: The vector to.
+
+    Returns:
+        The rotation matrix from vec1 to vec2.
+    """
+    # vec1 -> vec2 の回転ベクトルを導出
+    b = normalize(vec1[:3])
+    a = normalize(vec2[:3])
+    cross = np.cross(a, b)
+    dot = np.dot(a, b)
+    angle = np.arccos(dot)
+    rotvec = normalize(cross) * angle
+
+    # 回転行列を導出
+    rotation_matrix = Rotation.from_rotvec(rotvec).as_matrix()
+
+    return rotation_matrix

tests/martix/test_get_rotation_from_vectors.py

@@ -0,0 +1,11 @@
+import numpy as np
+
+from ply_processor_basics.matrix import get_rotation_from_vectors
+
+
+def test_simple_rotation():
+    vec1 = np.array([1, 0, 0])
+    vec2 = np.array([0, 1, 0])
+
+    rotation_matrix = get_rotation_from_vectors(vec1, vec2)
+    assert np.allclose(np.dot(vec1, rotation_matrix), vec2)