初始xyz坐标系与机体坐标系重合机体坐标系中一正方形绕中心轴旋转后平移求正方形中心在xyz坐标系中的点坐标旋转矩阵实现

假设正方形的边长为2a，中心坐标为(x0, y0, z0)。初始时，正方形的四个顶点在机体坐标系中的坐标分别为：

A = (x0 - a, y0 - a, z0) B = (x0 + a, y0 - a, z0) C = (x0 + a, y0 + a, z0) D = (x0 - a, y0 + a, z0)

接下来，我们将正方形绕机体坐标系的中心轴旋转，假设旋转角度为θ。旋转后，正方形的四个顶点在机体坐标系中的坐标分别为：

A' = (x0 - acosθ, y0 - asinθ, z0) B' = (x0 + acosθ, y0 - asinθ, z0) C' = (x0 + acosθ, y0 + asinθ, z0) D' = (x0 - acosθ, y0 + asinθ, z0)

然后，我们将旋转后的正方形平移到xyz坐标系中，假设平移的向量为(xt, yt, zt)。平移后，正方形的四个顶点在xyz坐标系中的坐标分别为：

A'' = (x0 - acosθ + xt, y0 - asinθ + yt, z0 + zt) B'' = (x0 + acosθ + xt, y0 - asinθ + yt, z0 + zt) C'' = (x0 + acosθ + xt, y0 + asinθ + yt, z0 + zt) D'' = (x0 - acosθ + xt, y0 + asinθ + yt, z0 + zt)

最后，正方形中心在xyz坐标系中的点坐标为：

(xc, yc, zc) = ((x0 + xt)/2, (y0 + yt)/2, (z0 + zt)/2)

旋转矩阵的实现如下：

import numpy as np

def rotation_matrix(angle, axis):
    """
    生成旋转矩阵
    :param angle: 旋转角度（弧度）
    :param axis: 旋转轴（单位向量）
    :return: 旋转矩阵
    """
    c = np.cos(angle)
    s = np.sin(angle)
    x, y, z = axis
    return np.array([[x*x*(1-c)+c, x*y*(1-c)-z*s, x*z*(1-c)+y*s],
                     [x*y*(1-c)+z*s, y*y*(1-c)+c, y*z*(1-c)-x*s],
                     [x*z*(1-c)-y*s, y*z*(1-c)+x*s, z*z*(1-c)+c]])

使用该旋转矩阵可以将初始时正方形的四个顶点坐标变换到旋转后的坐标系中