能使用等效旋转矢量到方位余弦的转换关系来编写代码么

当给出一个等效旋转矢量$\boldsymbol{q}$时，可以使用以下转换关系将其转换为方位余弦阵$\boldsymbol{C}$：

$$
\begin{aligned}
C_{11} &= q_0^2 + q_1^2 - q_2^2 - q_3^2 \\
C_{12} &= 2(q_1 q_2 + q_0 q_3) \\
C_{13} &= 2(q_1 q_3 - q_0 q_2) \\
C_{21} &= 2(q_1 q_2 - q_0 q_3) \\
C_{22} &= q_0^2 - q_1^2 + q_2^2 - q_3^2 \\
C_{23} &= 2(q_2 q_3 + q_0 q_1) \\
C_{31} &= 2(q_1 q_3 + q_0 q_2) \\
C_{32} &= 2(q_2 q_3 - q_0 q_1) \\
C_{33} &= q_0^2 - q_1^2 - q_2^2 + q_3^2
\end{aligned}
$$

其中，$q_0$表示旋转四元数的实部，$q_1, q_2, q_3$表示旋转四元数的虚部。

以下是使用转换关系将等效旋转矢量转换为方位余弦阵的Python代码：

import numpy as np

def rotvec2dcm(rotvec):
    q0 = np.cos(np.linalg.norm(rotvec) / 2)
    q1 = rotvec[0] * np.sin(np.linalg.norm(rotvec) / 2)
    q2 = rotvec[1] * np.sin(np.linalg.norm(rotvec) / 2)
    q3 = rotvec[2] * np.sin(np.linalg.norm(rotvec) / 2)
    
    C11 = q0 ** 2 + q1 ** 2 - q2 ** 2 - q3 ** 2
    C12 = 2 * (q1 * q2 + q0 * q3)
    C13 = 2 * (q1 * q3 - q0 * q2)
    C21 = 2 * (q1 * q2 - q0 * q3)
    C22 = q0 ** 2 - q1 ** 2 + q2 ** 2 - q3 ** 2
    C23 = 2 * (q2 * q3 + q0 * q1)
    C31 = 2 * (q1 * q3 + q0 * q2)
    C32 = 2 * (q2 * q3 - q0 * q1)
    C33 = q0 ** 2 - q1 ** 2 - q2 ** 2 + q3 ** 2
    
    C = np.array([[C11, C12, C13], [C21, C22, C23], [C31, C32, C33]])
    
    return C

其中，`rotvec`为等效旋转矢量，返回值`C`为方位余弦阵。