From 29fe0ff94c99d641ad501b0a4a1fe567d9bb3acd Mon Sep 17 00:00:00 2001
From: facat <facat@facat.com>
Date: Thu, 3 Jun 2021 21:16:29 +0800
Subject: [PATCH] =?UTF-8?q?=E5=AE=8C=E6=88=90=E6=97=8B=E8=BD=AC=E7=9F=A9?=
 =?UTF-8?q?=E9=98=B5=E3=80=82?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: facat <facat@facat.com>
---
 .gitignore        |  2 ++
 main.py           |  9 +++++++++
 transformation.py | 47 +++++++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 58 insertions(+)
 create mode 100644 .gitignore
 create mode 100644 main.py
 create mode 100644 transformation.py

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..a47fb1d
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,2 @@
+.idea
+__pycache__
\ No newline at end of file
diff --git a/main.py b/main.py
new file mode 100644
index 0000000..58b477b
--- /dev/null
+++ b/main.py
@@ -0,0 +1,9 @@
+import math
+from transformation import rotation
+import numpy as np
+
+if __name__ == "__main__":
+    t = rotation(
+        45 / 180 * math.pi, np.array([0, 0, 1]), np.array([[1, 1, 0], [2, 3, 4]])
+    )
+    print(t)
diff --git a/transformation.py b/transformation.py
new file mode 100644
index 0000000..df2030c
--- /dev/null
+++ b/transformation.py
@@ -0,0 +1,47 @@
+import numpy as np
+from math import cos, sin
+
+
+def create_rx(unit_axis: np.ndarray) -> np.ndarray:
+    (a, b, c) = unit_axis.tolist()
+    d = (b ** 2 + c ** 2) ** 0.5
+    return np.array(
+        [[1, 0, 0, 0], [0, c / d, -b / d, 0], [0, b / d, c / d, 0], [0, 0, 0, 1]]
+    )
+
+
+def create_ry(unit_axis: np.ndarray) -> np.ndarray:
+    (a, b, c) = unit_axis.tolist()
+    d = (b ** 2 + c ** 2) ** 0.5
+    return np.array([[d, 0, -a, 0], [0, 1, 0, 0], [a, 0, d, 0], [0, 0, 0, 1]])
+
+
+def create_rz(angel: float) -> np.ndarray:
+    return np.array(
+        [
+            [cos(angel), -sin(angel), 0, 0],
+            [sin(angel), cos(angel), 0, 0],
+            [0, 0, 1, 0],
+            [0, 0, 0, 1],
+        ]
+    )
+
+
+def create_t(axis: np.ndarray) -> np.ndarray:
+    (x1, y1, z1) = axis.tolist()
+    return np.array([[1, 0, 0, -x1], [0, 1, 0, -y1], [0, 0, 1, -z1], [0, 0, 0, 1]])
+
+
+def rotation(angel: float, axis: np.ndarray, points: np.ndarray):
+    # 依据《计算机图形学》第4版 9.28公式
+    unit_axis = axis / np.linalg.norm(axis)
+    rz = create_rz(angel)
+    rx = create_rx(unit_axis)
+    ry = create_ry(unit_axis)
+    t = create_t(axis)
+    t_i = np.linalg.inv(t)
+    rx_i = np.linalg.inv(rx)
+    ry_i = np.linalg.inv(ry)
+    r_transform = t_i.dot(rx_i.dot(ry_i.dot(rz.dot(ry.dot(rx.dot(t))))))
+    expand_point = np.concatenate((points, np.ones((points.shape[0], 1))), axis=1)
+    return r_transform.dot(expand_point.T)