74 lines
2.2 KiB
Python
74 lines
2.2 KiB
Python
import numpy as np
|
|
from math import cos, sin
|
|
from typing import List, Union, Tuple
|
|
|
|
|
|
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_matrix(
|
|
angel: float, axis_start: np.ndarray, axis_end: np.ndarray
|
|
) -> np.ndarray:
|
|
# 依据《计算机图形学》第4版 9.28公式
|
|
axis = axis_end - axis_start
|
|
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_start)
|
|
t_i = np.linalg.inv(t)
|
|
rx_i = np.linalg.inv(rx)
|
|
ry_i = np.linalg.inv(ry)
|
|
r_transform = t_i @ rx_i @ ry_i @ rz @ ry @ rx @ t
|
|
return r_transform
|
|
|
|
|
|
def rotation(
|
|
angel: float, axis_start: np.ndarray, axis_end: np.ndarray, points: np.ndarray
|
|
) -> np.ndarray:
|
|
_rotation = Rotation(angel, axis_start.tolist(), axis_end.tolist())
|
|
return _rotation.rotate(points.tolist())
|
|
|
|
|
|
class Rotation:
|
|
def __init__(self, angel: float, axis_start: List[float], axis_end: List[float]):
|
|
self._r_transform = rotation_matrix(
|
|
angel, np.array(axis_start), np.array(axis_end)
|
|
)
|
|
|
|
def rotate(self, points: List[Union[List[float], Tuple[float]]])->List:
|
|
np_points = np.array(points)
|
|
r_transform = self._r_transform
|
|
expand_point = np.concatenate(
|
|
(np_points, np.ones((np_points.shape[0], 1))), axis=1
|
|
)
|
|
transformed_points = r_transform @ expand_point.T
|
|
return transformed_points[:-1, :].T.tolist()
|