三位空间坐标转换

最新推荐文章于 2024-09-13 20:22:23 发布

Counting Sun

最新推荐文章于 2024-09-13 20:22:23 发布

阅读量1.5k

点赞数 3

分类专栏：学习之路文章标签：矩阵线性代数几何学 python

本文链接：https://blog.csdn.net/qq_41958183/article/details/121889682

版权

学习之路专栏收录该内容

2 篇文章 0 订阅

订阅专栏

轴系旋转矩阵及代码实现-python

绕Z轴旋转

旋转矩阵
$\begin{bmatrix} cos \theta & -sin \theta & 0 \\ sin \theta & cos \theta & 0 \\ 0 & 0 & 1 \end{bmatrix}$

    def z_rotation(x, y, z, thetaz):
        x1, y1 = x, y
        rz = math.radians(thetaz)
        outx = math.cos(rz) * x1 - math.sin(rz) * y1
        outy = math.sin(rz) * x1 + math.cos(rz) * y1
        outz = z
        return [format(outx, '.2f'), format(outy, '.2f'), format(outz, '.2f')]

绕Y轴旋转

旋转矩阵
$\begin{bmatrix} cos\beta & 0 & sin\beta \\ 0 & 1 & 0 \\ -sin\beta & 0 & cos\beta \end{bmatrix}$

    def y_rotation(x, y, z, thetay):
        x1, z1 = x, z
        ry = math.radians(thetay)
        outx = math.cos(ry) * x1 + math.sin(ry) * z1
        outy = y
        outz = math.cos(ry) * z1 - math.sin(ry) * x1
        return [format(outx, '.2f'), format(outy, '.2f'), format(outz, '.2f')]

绕X轴旋转

旋转矩阵
$\begin{bmatrix} 1 & 0 & 0 \\ 0 & cos \alpha & -sin \alpha \\ 0 & sin \alpha & cos\alpha \end{bmatrix}$

    def x_rotation(x, y, z, thetax):
        y1, z1 = y, z
        rx = math.radians(thetax)
        outx = x
        outy = math.cos(rx) * y1 - math.sin(rx) * z1
        outz = math.cos(rx) * z1 + math.sin(rx) * y1
        return [format(outx, '.2f'), format(outy, '.2f'), format(outz, '.2f')]

绕任意轴旋转

具体推导过程这里不做详述，要注意公式中的( $n_x$ , $n_y$ , $n_z$ ）是单位向量，因此代码中要做转化处理

旋转矩阵
$\begin{bmatrix} n^2_x(1-cos\theta)+cos\theta &n_xn_y(1-cos\theta)+n_zsin\theta&n_xn_z(1-cos\theta)-n_ysin\theta\\ n_xn_y(1-cos\theta)-n_zsin\theta&n_y^2(1-cos\theta)+cos\theta&n_yn_z(1-cos\theta)+n_xsin\theta\\ n_xn_z(1-cos\theta)+n_ysin\theta&n_yn_z(1-cos\theta)-n_xsin\theta&n_x^2(1-cos\theta)+cos\theta \end{bmatrix}$

    def any_axis(x, y, z, vx, vy, vz, theta):
        # (vx,vy,vz)转化为单位向量
        x1 = vx / ((vx * vx + vy * vy + vz * vz) ** 0.5)
        y1 = vy / ((vx * vx + vy * vy + vz * vz) ** 0.5)
        z1 = vz / ((vx * vx + vy * vy + vz * vz) ** 0.5)
        radian = math.radians(theta)
        c = math.cos(radian)
        s = math.sin(radian)
        outx = (x1 * x1 * (1 - c) + c) * x + (x1 * y1 * (1 - c) - z1 * s) * y + (x1 * z1 * (1 - c) + y1 * s) * z
        outy = (y1 * x1 * (1 - c) + z1 * s) * x + (y1 * y1 * (1 - c) + c) * y + (y1 * z1 * (1 - c) - x1 * s) * z
        outz = (x1 * z1 * (1 - c) - y1 * s) * x + (y1 * z1 * (1 - c) + x1 * s) * y + (z1 * z1 * (1 - c) + c) * z
        # print(c,s)
        return [format(outx, '.2f'), format(outy, '.2f'), format(outz, '.2f')]