三维空间任意一点绕任意轴线旋转

一、问题描述

  对于三维空间任意一点$P(p_x,p_y,p_z)$,求绕任意轴线旋转角度$\alpha$得到新的点$P^{\prime }(p_x^{\prime },p_y^{\prime },p_z^{\prime })$。轴线的单位方向向量为$\hat{\mathbf{n}}(n_x^2+n_y^2+n_z^2=1)$，且过点$Q(x_0,y_0,z_0)$。

二、推导步骤

  轴线$\hat{\mathbf{n}}$在坐标系$XYZ$下的直线方程为：
$$\{\begin{array}{c}x=x_0+n_{x}t\\ y=y_0+n_{y}t\\ z=z_0+n_{z}t\end{array}\text{\hspace{1em}(1)}$$
  弧$P{P}^{\prime }$所在平面在坐标系$XYZ$下的平面方程为：
$$n_x(x-p_x)+n_y(y-p_y)+n_z(z-p_z)=0\text{\hspace{1em}(2)}$$
  根据式(1)和式(2)，且$n_x^2+n_y^2+n_z^2=1$，可以求得：
$$t_0=n_x(p_x-x_0)+n_y(p_y-y_0)+n_z(p_z-z_0)\text{\hspace{1em}(3)}$$
  设弧$P{P}^{\prime }$的圆心在坐标系$XYZ$下的坐标为$(x_c,y_c,z_c)$,将$t_0$代入式$(1)$，得圆心坐标：
$$\{\begin{array}{c}{x}_c=x_0+n_x{t}_0\\ y_c=y_0+n_y{t}_0\\ z_c=z_0+n_z{t}_0\end{array}\text{\hspace{1em}(4)}$$
  圆半径$r$:
$$r=\sqrt{(p_x-x_c{)}^2+(p_y-y_c{)}^2+(p_z-z_c{)}^2}\text{\hspace{1em}(5)}$$
  向量$\mathbf{O}\mathbf{P}$:
$$\mathbf{O}\mathbf{P}=[p_x-x_c{p}_y-y_c{p}_z-z_c{]}^T/r\text{\hspace{1em}(6)}$$
  如上图，建立坐标系$x^{\prime }{y}^{\prime }{z}^{\prime }$，根据右手法则：
$${\mathbf{y}}^{\prime }=[n_x{n}_y{n}_z{]}^T\times \mathbf{O}\mathbf{P}\text{\hspace{1em}(7)}$$
  坐标系$x^{\prime }{y}^{\prime }{z}^{\prime }$与坐标系$XYZ$的旋转变换矩阵为：
$$R_{3\times 3}=[\mathbf{O}\mathbf{P}{\mathbf{y}}^{\prime }\hat{\mathbf{n}}]\text{\hspace{1em}(8)}$$
  点$P^{\prime }$在坐标系$x^{\prime }{y}^{\prime }{z}^{\prime }$的坐标为：
$$\{\begin{array}{c}{x}_{temp}=rcos(\alpha )\\ y_{temp}=rsin(\alpha )\\ z_{temp}=0\end{array}\text{\hspace{1em}(9)}$$
  利用齐次变换，将点$P^{\prime }$在坐标系$x^{\prime }{y}^{\prime }{z}^{\prime }$的坐标变换到坐标系$XYZ$的坐标：
$$\left[\begin{array}{c}{p}_x^{\prime }\\ p_y^{\prime }\\ p_z^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{cccc}{R}_{11}& R_{12}& R_{13}& x_c\\ R_{21}& R_{22}& R_{23}& y_c\\ R_{31}& R_{32}& R_{33}& z_c\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{temp}\\ y_{temp}\\ z_{temp}\\ 1\end{array}\right]\text{\hspace{1em}(10)}$$

  化简式$(10)$得：
$$\{\begin{array}{c}{p}_x^{\prime }=R_{11}{x}_{temp}+R_{12}{y}_{temp}+x_c\\ p_y^{\prime }=R_{21}{x}_{temp}+R_{22}{y}_{temp}+y_c\\ p_z^{\prime }=R_{31}{x}_{temp}+R_{32}{y}_{temp}+z_c\end{array}\text{\hspace{1em}(11)}$$

  式$(11)$可以展开，并写成：
$$\left[\begin{array}{c}{p}_x^{\prime }\\ p_y^{\prime }\\ p_z^{\prime }\\ 1\end{array}\right]=T_{4\times 4}\left[\begin{array}{c}{p}_x\\ p_y\\ p_z\\ 1\end{array}\right]\text{\hspace{1em}(12)}$$

$$T_{4\times 4}=\left[\begin{array}{cccc}{n}_x^{2}K+cos(\alpha )& n_x{n}_{y}K-n_{z}sin(\alpha )& n_x{n}_{z}K+n_{y}sin(\alpha )& (x_0-n_{x}M)K+(n_z{y}_0-n_y{z}_0)sin(\alpha )\\ n_x{n}_{y}K+n_{z}sin(\alpha )& n_y^{2}K+cos(\alpha )& n_y{n}_{z}K-n_{x}sin(\alpha )& (y_0-n_{y}M)K+(n_x{z}_0-n_z{x}_0)sin(\alpha )\\ n_x{n}_{z}K-n_{y}sin(\alpha )& n_y{n}_{z}K+n_{x}sin(\alpha )& n_z^{2}K+cos(\alpha )& (z_0-n_{z}M)K+(n_y{x}_0-n_x{y}_0)sin(\alpha )\\ 0& 0& 0& 1\end{array}\right]$$
  其中，$K=1-cos(\alpha )，M=n_x{x}_0+n_y{y}_0+n_z{z}_0$

三、$MATLAB$代码

clc;
clear;

syms t nx ny nz px py pz alpha x0 y0 z0 real
syms tx ty tz axisFlag real

% axisFlag = 1 : x轴
% axisFlag = 2 : y轴
% axisFlag = 3 : z轴
% axisFlag = 4 : 过点(0,ty,tz)且平行x轴
% axisFlag = 5 : 过点(tx,0,tz)且平行y轴
% axisFlag = 6 : 过点(tx,ty,0)且平行z轴
% axisFlag = 7 : 过点(0,0,0)且单位方向向量为[nx ny nz]
% axisFlag = 8 : 过点(tx,ty,tz)且单位方向向量为[nx ny nz]

axisFlag = 8;
switch axisFlag
    case 1
        x0 = 0;
        y0 = 0;
        z0 = 0;
        nx = 1;
        ny = 0;
        nz = 0;
    case 2
        x0 = 0;
        y0 = 0;
        z0 = 0;
        nx = 0;
        ny = 1;
        nz = 0;
    case 3
        x0 = 0;
        y0 = 0;
        z0 = 0;
        nx = 0;
        ny = 0;
        nz = 1;
    case 4
        x0 = 0;
        y0 = ty;
        z0 = tz;
        nx = 1;
        ny = 0;
        nz = 0;
    case 5
        x0 = tx;
        y0 = 0;
        z0 = tz;
        nx = 0;
        ny = 1;
        nz = 0;
    case 6
        x0 = tx;
        y0 = ty;
        z0 = 0;
        nx = 0;
        ny = 0;
        nz = 1;
    case 7
        x0 = 0;
        y0 = 0;
        z0 = 0;
    case 8
        x0 = tx;
        y0 = ty;
        z0 = tz;
    otherwise
        return;
end

%{
x = nx * t + x0;
y = ny * t + y0;
z = nz * t + z0;
t0 = solve(nx * (x - px) + ny * (y - py) + nz * (z - pz) == 0, t)
%}

t0 = nx * (px - x0) + ny * (py - y0) + nz * (pz - z0);
xc = x0 + nx * t0;
yc = y0 + ny * t0;
zc = z0 + nz * t0;
r = sqrt((px - xc)^2 + (py - yc)^2 + (pz - zc)^2);

OP = [px - xc; py - yc; pz - zc] / r;
yVector = cross([nx; ny; nz], OP);
R = [OP, yVector, [nx; ny; nz]];

xtemp = r * cos(alpha);
ytemp = r * sin(alpha);
p = [R(1,1) * xtemp + R(1,2) * ytemp + xc
    R(2,1) * xtemp + R(2,2) * ytemp + yc
    R(3,1) * xtemp + R(3,2) * ytemp + zc];
p = [simplify(p(1)); simplify(p(2)); simplify(p(3))]

%% 写成矩阵形式，并验证结果正确性
K = 1 - cos(alpha);
M = nx * x0 + ny * y0 + nz * z0;
T = [nx^2 * K + cos(alpha),  nx * ny * K - nz * sin(alpha),  nx * nz * K + ny * sin(alpha),  (x0 - nx * M) * K + (nz * y0 - ny * z0) * sin(alpha)
    nx * ny * K + nz * sin(alpha),  ny^2 * K + cos(alpha),  ny * nz * K - nx * sin(alpha),  (y0 - ny * M) * K + (nx * z0 - nz * x0) * sin(alpha)
    nx * nz * K - ny * sin(alpha),  ny * nz * K + nx * sin(alpha),  nz^2 * K + cos(alpha),  (z0 - nz * M) * K + (ny * x0 - nx * y0) * sin(alpha)
    0, 0, 0, 1]

res = simplify([nx^2 * K + cos(alpha),  nx * ny * K - nz * sin(alpha),  nx * nz * K + ny * sin(alpha),  (x0 - nx * M) * K + (nz * y0 - ny * z0) * sin(alpha)
    nx * ny * K + nz * sin(alpha),  ny^2 * K + cos(alpha),  ny * nz * K - nx * sin(alpha),  (y0 - ny * M) * K + (nx * z0 - nz * x0) * sin(alpha)
    nx * nz * K - ny * sin(alpha),  ny * nz * K + nx * sin(alpha),  nz^2 * K + cos(alpha),  (z0 - nz * M) * K + (ny * x0 - nx * y0) * sin(alpha)
    0, 0, 0, 1] * [px; py; pz; 1] - [p(1); p(2); p(3); 1])

四、总结

  本文算法的结论具有普遍性，当轴线的单位方向向量$\hat{\mathbf{n}}$和经过的点$Q(x_0,y_0,z_0)$取特殊值时，可以得到许多很有用的结论。
  1.当旋转轴为$x$轴，则$(x_0,y_0,z_0)=(0,0,0),(n_x,n_y,n_z)=(1,0,0)$时，
$$T_{4\times 4}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& cos(\alpha )& -sin(\alpha )& 0\\ 0& sin(\alpha )& cos(\alpha )& 0\\ 0& 0& 0& 1\end{array}\right]\text{\hspace{1em}(13)}$$
  2.当旋转轴为$y$轴，则$(x_0,y_0,z_0)=(0,0,0),(n_x,n_y,n_z)=(0,1,0)$时，
$$T_{4\times 4}=\left[\begin{array}{cccc}cos(\alpha )& 0& sin(\alpha )& 0\\ 0& 1& 0& 0\\ -sin(\alpha )& 0& cos(\alpha )& 0\\ 0& 0& 0& 1\end{array}\right]\text{\hspace{1em}(14)}$$
  3.当旋转轴为$z$轴，则$(x_0,y_0,z_0)=(0,0,0),(n_x,n_y,n_z)=(0,0,1)$时，
$$T_{4\times 4}=\left[\begin{array}{cccc}cos(\alpha )& -sin(\alpha )& 0& 0\\ sin(\alpha )& cos(\alpha )& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\text{\hspace{1em}(15)}$$
  4.当旋转轴过点$(0,t_y,t_z)$且平行于$x$轴，则$(x_0,y_0,z_0)=(0,t_y,t_z),(n_x,n_y,n_z)=(1,0,0)$时，
$$T_{4\times 4}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& cos(\alpha )& -sin(\alpha )& t_{y}K+t_{z}sin(\alpha )\\ 0& sin(\alpha )& cos(\alpha )& t_{z}K-t_{y}sin(\alpha )\\ 0& 0& 0& 1\end{array}\right]\text{\hspace{1em}(16)}$$
  5.当旋转轴过点$(t_x,0,t_z)$且平行于$y$轴，则$(x_0,y_0,z_0)=(t_x,0,t_z),(n_x,n_y,n_z)=(0,1,0)$时，
$$T_{4\times 4}=\left[\begin{array}{cccc}cos(\alpha )& 0& sin(\alpha )& t_{x}K-t_{z}sin(\alpha )\\ 0& 1& 0& 0\\ -sin(\alpha )& 0& cos(\alpha )& t_{z}K+t_{x}sin(\alpha )\\ 0& 0& 0& 1\end{array}\right]\text{\hspace{1em}(17)}$$
  6.当旋转轴过点$(t_x,t_y,0)$且平行于$z$轴，则$(x_0,y_0,z_0)=(t_x,t_y,0),(n_x,n_y,n_z)=(0,0,1)$时，
$$T_{4\times 4}=\left[\begin{array}{cccc}cos(\alpha )& -sin(\alpha )& 0& t_{x}K+t_{y}sin(\alpha )\\ sin(\alpha )& cos(\alpha )& 0& t_{y}K-t_{x}sin(\alpha )\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\text{\hspace{1em}(18)}$$
  7.当旋转轴过原点$(0,0,0)$，则$(x_0,y_0,z_0)=(0,0,0)$时，
$$T_{4\times 4}=\left[\begin{array}{cccc}{n}_x^{2}K+cos(\alpha )& n_x{n}_{y}K-n_{z}sin(\alpha )& n_x{n}_{z}K+n_{y}sin(\alpha )& 0\\ n_x{n}_{y}K+n_{z}sin(\alpha )& n_y^{2}K+cos(\alpha )& n_y{n}_{z}K-n_{x}sin(\alpha )& 0\\ n_x{n}_{z}K-n_{y}sin(\alpha )& n_y{n}_{z}K+n_{x}sin(\alpha )& n_z^{2}K+cos(\alpha )& 0\\ 0& 0& 0& 1\end{array}\right]\text{\hspace{1em}(19)}$$
  $T_{4\times 4}$的前$3$行$3$列就是将三维旋转的轴-角表示转化为旋转矩阵表示：
$$R_{3\times 3}=\left[\begin{array}{ccc}{n}_x^{2}K+cos(\alpha )& n_x{n}_{y}K-n_{z}sin(\alpha )& n_x{n}_{z}K+n_{y}sin(\alpha )\\ n_x{n}_{y}K+n_{z}sin(\alpha )& n_y^{2}K+cos(\alpha )& n_y{n}_{z}K-n_{x}sin(\alpha )\\ n_x{n}_{z}K-n_{y}sin(\alpha )& n_y{n}_{z}K+n_{x}sin(\alpha )& n_z^{2}K+cos(\alpha )\end{array}\right]\text{\hspace{1em}(20)}$$
  其中，$K=1-cos(\alpha )$