轴系旋转矩阵及代码实现-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θ0001⎦⎤
def z_rotation(x, y, z, thetaz):x1, y1 = x, yrz = math.radians(thetaz)outx = math.cos(rz) * x1 - math.sin(rz) * y1outy = math.sin(rz) * x1 + math.cos(rz) * y1outz = zreturn [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} cos\beta & 0 & sin\beta \\ 0 & 1 & 0 \\ -sin\beta & 0 & cos\beta \end{bmatrix} ⎣⎡cosβ0−sinβ010sinβ0cosβ⎦⎤
def y_rotation(x, y, z, thetay):x1, z1 = x, zry = math.radians(thetay)outx = math.cos(ry) * x1 + math.sin(ry) * z1outy = youtz = math.cos(ry) * z1 - math.sin(ry) * x1return [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 & cos\alpha \end{bmatrix} ⎣⎡1000cosαsinα0−sinαcosα⎦⎤
def x_rotation(x, y, z, thetax):y1, z1 = y, zrx = math.radians(thetax)outx = xouty = math.cos(rx) * y1 - math.sin(rx) * z1outz = math.cos(rx) * z1 + math.sin(rx) * y1return [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-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} ⎣⎡nx2(1−cosθ)+cosθnxny(1−cosθ)−nzsinθnxnz(1−cosθ)+nysinθnxny(1−cosθ)+nzsinθny2(1−cosθ)+cosθnynz(1−cosθ)−nxsinθnxnz(1−cosθ)−nysinθnynz(1−cosθ)+nxsinθ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) * zouty = (y1 * x1 * (1 - c) + z1 * s) * x + (y1 * y1 * (1 - c) + c) * y + (y1 * z1 * (1 - c) - x1 * s) * zoutz = (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')]
