vscode用python编写四元数wxyz转换为旋转平移矩阵

在Visual Studio Code (VSCode) 中，你可以使用Python编写一个函数来将四元数(w, x, y, z)转换为旋转和平移矩阵。四元数通常用于表示三维空间中的旋转，而旋转和平移可以组合成一个4x4变换矩阵。这里是一个简单的示例：

import numpy as np

def quat_to_matrix(quat):
    w, x, y, z = quat
    # 创建单位四元数(1,0,0,0)，用于累积旋转
    ident = np.eye(4)
    q_ident = np.array([[1, 0, 0, 0],
                        [0, 1, 0, 0],
                        [0, 0, 1, 0],
                        [0, 0, 0, 1]])

    # 将四元数转换为旋转部分的矩阵
    rotation_part = np.array([[1 - 2 * (y**2 + z**2), 2 * (x*y - z*w),   2 * (x*z + y*w)],
                              [2 * (x*y + z*w),   1 - 2 * (x**2 + z**2), 2 * (y*z - x*w)],
                              [2 * (x*z - y*w),   2 * (y*z + x*w),   1 - 2 * (x**2 + y**2)]])
    
    # 转换为总变换矩阵，包含平移信息
    translation = np.eye(4)[..., :3]  # 初始化为单位矩阵，然后仅保留前三个分量
    translation[0, 3], translation[1, 3], translation[2, 3] = w, x, y  # 填充平移向量

    # 结合旋转和平移
    transform_matrix = np.dot(rotation_part, q_ident) + np.dot(q_ident, rotation_part.T) + np.outer(translation, translation)

    return transform_matrix

# 示例四元数
quat_example = [0.5, 0.5, 0.5, 0.5]  # 这里是一个正交的四元数，代表无旋转向量
matrix_example = quat_to_matrix(quat_example)

print("Transform matrix:")
print(matrix_example)

在这个例子中，`quat_to_matrix` 函数接受一个四元素列表表示的四元数，并返回一个4x4的变换矩阵。如果你想添加或修改平移部分，只需要更改 `translation` 部分即可。