三位空间坐标转换

三位空间坐标转换

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

绕Z轴旋转

旋转矩阵
[ c o s θ − s i n θ 0 s i n θ c o s θ 0 0 0 1 ] begin{bmatrix} cos theta & -sin theta & 0 \ sin theta & cos theta & 0 \ 0 & 0 & 1 end{bmatrix} ⎣⎡​cosθsinθ0​−sinθcosθ0​001​⎦⎤​

    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轴旋转

旋转矩阵
[ c o s β 0 s i n β 0 1 0 − s i n β 0 c o s β ] begin{bmatrix} cosbeta & 0 & sinbeta \ 0 & 1 & 0 \ -sinbeta & 0 & cosbeta end{bmatrix} ⎣⎡​cosβ0−sinβ​010​sinβ0cosβ​⎦⎤​

    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轴旋转

旋转矩阵
[ 1 0 0 0 c o s α − s i n α 0 s i n α c o s α ] begin{bmatrix} 1 & 0 & 0 \ 0 & cos alpha & -sin alpha \ 0 & sin alpha & cosalpha end{bmatrix} ⎣⎡​100​0cosαsinα​0−sinαcosα​⎦⎤​

    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_x nx​, n y n_y ny​, n z n_z nz​）是单位向量，因此代码中要做转化处理

旋转矩阵
[ n x 2 ( 1 − c o s θ ) + c o s θ n x n y ( 1 − c o s θ ) + n z s i n θ n x n z ( 1 − c o s θ ) − n y s i n θ n x n y ( 1 − c o s θ ) − n z s i n θ n y 2 ( 1 − c o s θ ) + c o s θ n y n z ( 1 − c o s θ ) + n x s i n θ n x n z ( 1 − c o s θ ) + n y s i n θ n y n z ( 1 − c o s θ ) − n x s i n θ n x 2 ( 1 − c o s θ ) + c o s θ ] begin{bmatrix} n^2_x(1-costheta)+costheta &n_xn_y(1-costheta)+n_zsintheta&n_xn_z(1-costheta)-n_ysintheta\ n_xn_y(1-costheta)-n_zsintheta&n_y^2(1-costheta)+costheta&n_yn_z(1-costheta)+n_xsintheta\ n_xn_z(1-costheta)+n_ysintheta&n_yn_z(1-costheta)-n_xsintheta&n_x^2(1-costheta)+costheta end{bmatrix} ⎣⎡​nx2​(1−cosθ)+cosθnx​ny​(1−cosθ)−nz​sinθnx​nz​(1−cosθ)+ny​sinθ​nx​ny​(1−cosθ)+nz​sinθny2​(1−cosθ)+cosθny​nz​(1−cosθ)−nx​sinθ​nx​nz​(1−cosθ)−ny​sinθny​nz​(1−cosθ)+nx​sinθnx2​(1−cosθ)+cosθ​⎦⎤​

    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')]