点 欧拉角 另一个点

### 使用欧拉角实现三维空间中一点到另一点的旋转变换

为了理解如何使用欧拉角来描述物体在三维空间中的旋转并完成从一个点到另一个点的变换，可以按照以下方式考虑。

#### 定义初始条件和目标位置
假设有一个位于 \( P_0(x_0, y_0, z_0) \) 的点，在应用一系列由欧拉角定义的旋转之后移动到了新的位置 \( P_1(x_1, y_1, z_1) \)[^2]。这里所说的欧拉角具体指的是 yaw (偏航), pitch (俯仰), 和 roll (翻滚)，它们分别对应于围绕 Z 轴、Y 轴以及 X 轴发生的旋转角度变化[^3]。

#### 构建旋转矩阵
基于给定的欧拉角参数，构建相应的旋转矩阵 R 来表达这些连续的转动过程：

\[R = R_z(\psi) * R_y(\theta) * R_x(\phi)\]

其中，
- \(R_x\) 表示沿X轴方向上的基本旋转；
- \(R_y\) 是沿着Y轴执行的基础旋转操作；
- \(R_z\) 则是在Z轴上实施的标准旋转形式；

而 \((\phi,\theta,\psi)\) 分别代表了上述提到过的三个不同类型的转角量值——即 roll、pitch 和 yaw [^1]。

对于每一个单独的基本旋转而言，其对应的旋转矩阵可写作如下所示的形式:

\[R_x (\alpha)=\begin{bmatrix} 1 & 0& 0 \\ 0 & cos{\alpha}& -sin{\alpha}\\ 0 & sin{\alpha}&cos{\alpha}\end{bmatrix},\quad
R_y (\beta )=\begin{bmatrix} cos{\beta }& 0& sin{\beta }\\ 0 & 1 & 0\\ -sin{\beta }& 0 & cos{\beta }\end{bmatrix},\quad
R_z (\gamma )= \begin{bmatrix} cos{\gamma }& -sin{\gamma }& 0\\ sin{\gamma }& cos{\gamma }& 0\\ 0 & 0 & 1\end{bmatrix}
\]

因此，当已知某点的位置向量 p=[x,y,z]^T 并希望计算该点经过指定欧拉角所引起的位移后的最终坐标时，则可以通过下面这个公式来进行求解：

\[p' = Rp\]

此处 p’ 即为变换后的新坐标矢量。

import numpy as np

def euler_to_rotation_matrix(yaw, pitch, roll):
    """Convert Euler angles to rotation matrix."""
    
    cy = np.cos(np.radians(yaw))
    sy = np.sin(np.radians(yaw))
    cp = np.cos(np.radians(pitch))
    sp = np.sin(np.radians(pitch))
    cr = np.cos(np.radians(roll))
    sr = np.sin(np.radians(roll))

    Rz_yaw = np.array([[cy,-sy , 0],
                       [sy, cy, 0],
                       [0 , 0 , 1]])

    Ry_pitch = np.array([[cp, 0, sp],
                         [0, 1, 0],
                         [-sp, 0,cp]])
                         
    Rx_roll = np.array([[1, 0, 0],
                        [0,cr ,-sr ],
                        [0,sr , cr ]])

    # Rotation Matrix around fixed frame
    R = Rz_yaw @ Ry_pitch @ Rx_roll
    
    return R


point_initial = np.array([1., 0., 0.])[:, None]
angles_degrees = {'yaw': 90,'pitch': 45,'roll': 30}

rotation_matrix = euler_to_rotation_matrix(**angles_degrees)

transformed_point = rotation_matrix.dot(point_initial).flatten()

print(f"The transformed point is {tuple(transformed_point)}")