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

文章目录

  • 一、问题描述
  • 二、推导步骤
  • 三、 M A T L A B MATLAB MATLAB代码
  • 四、总结

一、问题描述

  对于三维空间任意一点 P ( p x , p y , p z ) P(p_x,p_y,p_z) P(px,py,pz),求绕任意轴线旋转角度 α \alpha α得到新的点 P ′ ( p x ′ , p y ′ , p z ′ ) P'(p'_x,p'_y,p'_z) P(px,py,pz)。轴线的单位方向向量为 n ^ ( n x 2 + n y 2 + n z 2 = 1 ) \bm{\hat{n}}(n_x^2+n_y^2+n_z^2=1) n^(nx2+ny2+nz2=1),且过点 Q ( x 0 , y 0 , z 0 ) Q(x_0,y_0,z_0) Q(x0,y0,z0)
三维空间任意一点绕任意轴线旋转_第1张图片

二、推导步骤

  轴线 n ^ \bm{\hat{n}} n^在坐标系 X Y Z XYZ XYZ下的直线方程为:
{ x = x 0 + n x t y = y 0 + n y t z = z 0 + n z t (1) \left\{ \begin{array}{c} x=x_0+n_xt \\ y=y_0+n_yt \\ \tag 1 z=z_0+n_zt\end{array}\right. x=x0+nxty=y0+nytz=z0+nzt(1)
  弧 P P ′ PP' PP所在平面在坐标系 X Y Z XYZ XYZ下的平面方程为:
n x ( x − p x ) + n y ( y − p y ) + n z ( z − p z ) = 0 (2) n_x(x-p_x)+ n_y(y-p_y)+ n_z(z-p_z)=0 \tag 2 nx(xpx)+ny(ypy)+nz(zpz)=0(2)
  根据式(1)和式(2),且 n x 2 + n y 2 + n z 2 = 1 n_x^2+n_y^2+n_z^2=1 nx2+ny2+nz2=1,可以求得:
t 0 = n x ( p x − x 0 ) + n y ( p y − y 0 ) + n z ( p z − z 0 ) (3) t_0 = n_x(p_x - x_0) + n_y(p_y - y_0) + n_z(p_z -z_0) \tag 3 t0=nx(pxx0)+ny(pyy0)+nz(pzz0)(3)
  设弧 P P ′ PP' PP的圆心在坐标系 X Y Z XYZ XYZ下的坐标为 ( x c , y c , z c ) (x_c,y_c,z_c) (xc,yc,zc),将 t 0 t_0 t0代入式 ( 1 ) (1) (1),得圆心坐标:
{ x c = x 0 + n x t 0 y c = y 0 + n y t 0 z c = z 0 + n z t 0 (4) \left\{ \begin{array}{c} x_c=x_0+n_xt_0 \\ y_c=y_0+n_yt_0 \\ \tag 4 z_c=z_0+n_zt_0\end{array}\right. xc=x0+nxt0yc=y0+nyt0zc=z0+nzt0(4)
  圆半径 r r r:
r = ( p x − x c ) 2 + ( p y − y c ) 2 + ( p z − z c ) 2 (5) r = \sqrt{(p_x-x_c)^2 + (p_y-y_c)^2 + (p_z-z_c)^2} \tag 5 r=(pxxc)2+(pyyc)2+(pzzc)2 (5)
  向量 O P \bm{OP} OP:
O P = [ p x − x c    p y − y c    p z − z c ] T / r (6) \bm{OP}=[p_x-x_c \ \ p_y-y_c \ \ p_z-z_c]^T / r \tag 6 OP=[pxxc  pyyc  pzzc]T/r(6)
  如上图,建立坐标系 x ′ y ′ z ′ x'y'z' xyz,根据右手法则:
y ′ = [ n x    n y    n z ] T × O P (7) \bm{y'}=[n_x\ \ n_y\ \ n_z]^T\times \bm{OP}\tag 7 y=[nx  ny  nz]T×OP(7)
  坐标系 x ′ y ′ z ′ x'y'z' xyz与坐标系 X Y Z XYZ XYZ的旋转变换矩阵为:
R 3 × 3 = [ O P    y ′    n ^ ] (8) R_{3\times3}=[\bm{OP}\ \ \bm{y'}\ \ \bm{\hat{n}} ]\tag 8 R3×3=[OP  y  n^](8)
  点 P ′ P' P在坐标系 x ′ y ′ z ′ x'y'z' xyz的坐标为:
{ x t e m p = r c o s ( α ) y t e m p = r s i n ( α ) z t e m p = 0 (9) \left\{ \begin{array}{c} x_{temp}=rcos(\alpha) \\ y_{temp}=rsin(\alpha) \\ \tag 9 z_{temp}=0 \\ \end{array}\right. xtemp=rcos(α)ytemp=rsin(α)ztemp=0(9)
  利用齐次变换,将点 P ′ P' P在坐标系 x ′ y ′ z ′ x'y'z' xyz的坐标变换到坐标系 X Y Z XYZ XYZ的坐标:
[ p x ′ p y ′ p z ′ 1 ] = [ 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 ] [ x t e m p y t e m p z t e m p 1 ] (10) \left[ \begin{matrix} p'_x \\ p'_y \\ p'_z \\ 1 \end{matrix} \right] = \left[ \begin{matrix} 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{matrix} \right] \left[ \begin{matrix} x_{temp} \\ y_{temp} \\ z_{temp} \\ 1 \end{matrix} \right] \tag{10} pxpypz1=R11R21R310R12R22R320R13R23R330xcyczc1xtempytempztemp1(10)

  化简式 ( 10 ) (10) (10)得:
{ p x ′ = R 11 x t e m p + R 12 y t e m p + x c p y ′ = R 21 x t e m p + R 22 y t e m p + y c p z ′ = R 31 x t e m p + R 32 y t e m p + z c (11) \left\{ \begin{array}{c} p'_x=R_{11}x_{temp}+R_{12}y_{temp}+ x_c\\ p'_y=R_{21}x_{temp}+R_{22}y_{temp}+ y_c\\ p'_z=R_{31}x_{temp}+R_{32}y_{temp}+ z_c \tag{11} \end{array}\right. px=R11xtemp+R12ytemp+xcpy=R21xtemp+R22ytemp+ycpz=R31xtemp+R32ytemp+zc(11)

  式 ( 11 ) (11) (11)可以展开,并写成:
[ p x ′ p y ′ p z ′ 1 ] = T 4 × 4 [ p x p y p z 1 ] (12) \left[ \begin{matrix} p'_x \\ p'_y \\ p'_z \\ 1 \end{matrix} \right] = T_{4\times4} \left[ \begin{matrix} p_x \\ p_y \\ p_z \\ 1 \end{matrix} \right]\tag{12} pxpypz1=T4×4pxpypz1(12)

T 4 × 4 = [ n x 2 K + c o s ( α ) n x n y K − n z s i n ( α ) n x n z K + n y s i n ( α ) ( x 0 − n x M ) K + ( n z y 0 − n y z 0 ) s i n ( α ) n x n y K + n z s i n ( α ) n y 2 K + c o s ( α ) n y n z K − n x s i n ( α ) ( y 0 − n y M ) K + ( n x z 0 − n z x 0 ) s i n ( α ) n x n z K − n y s i n ( α ) n y n z K + n x s i n ( α ) n z 2 K + c o s ( α ) ( z 0 − n z M ) K + ( n y x 0 − n x y 0 ) s i n ( α ) 0 0 0 1 ] T_{4\times4}=\left[ \begin{matrix} n_x^2 K + cos(\alpha) & n_x n_yK - n_z sin(\alpha) & n_xn_z K + n_y sin(\alpha) & (x_0 - n_xM)K + (n_zy_0 - n_yz_0)sin(\alpha) \\ n_xn_yK + n_zsin(\alpha) & n_y^2 K + cos(\alpha) & n_yn_zK - n_xsin(\alpha) & (y_0 - n_yM)K + (n_xz_0 - n_zx_0)sin(\alpha) \\ n_xn_zK - n_ysin(\alpha) & n_yn_zK + n_xsin(\alpha) & n_z^2K + cos(\alpha) & (z_0 - n_zM)K + (n_yx_0 - n_xy_0)sin(\alpha) \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right] \\ T4×4=nx2K+cos(α)nxnyK+nzsin(α)nxnzKnysin(α)0nxnyKnzsin(α)ny2K+cos(α)nynzK+nxsin(α)0nxnzK+nysin(α)nynzKnxsin(α)nz2K+cos(α)0(x0nxM)K+(nzy0nyz0)sin(α)(y0nyM)K+(nxz0nzx0)sin(α)(z0nzM)K+(nyx0nxy0)sin(α)1
  其中, K = 1 − c o s ( α ) , M = n x x 0 + n y y 0 + n z z 0 K = 1 - cos(\alpha),M = n_xx_0 + n_yy_0 + n_zz_0 K=1cos(α)M=nxx0+nyy0+nzz0

三、 M A T L A B MATLAB 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])

四、总结

  本文算法的结论具有普遍性,当轴线的单位方向向量 n ^ \bm{\hat{n}} n^和经过的点 Q ( x 0 , y 0 , z 0 ) Q(x_0,y_0,z_0) Q(x0,y0,z0)取特殊值时,可以得到许多很有用的结论。
  1.当旋转轴为 x x x轴,则 ( x 0 , y 0 , z 0 ) = ( 0 , 0 , 0 ) , ( n x , n y , n z ) = ( 1 , 0 , 0 ) (x_0,y_0,z_0)=(0,0,0),(n_x,n_y,n_z)=(1,0,0) (x0,y0,z0)=(0,0,0),(nx,ny,nz)=(1,0,0)时,
T 4 × 4 = [ 1 0 0 0 0 c o s ( α ) − s i n ( α ) 0 0 s i n ( α ) c o s ( α ) 0 0 0 0 1 ] (13) T_{4\times4}=\left[ \begin{matrix} 1 & 0 & 0 & 0 \\ 0 & cos(\alpha) & -sin(\alpha) & 0 \\ 0 & sin(\alpha) & cos(\alpha) & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right] \tag{13} T4×4=10000cos(α)sin(α)00sin(α)cos(α)00001(13)
  2.当旋转轴为 y y y轴,则 ( x 0 , y 0 , z 0 ) = ( 0 , 0 , 0 ) , ( n x , n y , n z ) = ( 0 , 1 , 0 ) (x_0,y_0,z_0)=(0,0,0),(n_x,n_y,n_z)=(0,1,0) (x0,y0,z0)=(0,0,0),(nx,ny,nz)=(0,1,0)时,
T 4 × 4 = [ c o s ( α ) 0 s i n ( α ) 0 0 1 0 0 − s i n ( α ) 0 c o s ( α ) 0 0 0 0 1 ] (14) T_{4\times4}=\left[ \begin{matrix} cos(\alpha) & 0 & sin(\alpha) & 0 \\ 0 & 1 & 0 & 0 \\ -sin(\alpha) & 0 & cos(\alpha) & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right] \tag{14} T4×4=cos(α)0sin(α)00100sin(α)0cos(α)00001(14)
  3.当旋转轴为 z z z轴,则 ( x 0 , y 0 , z 0 ) = ( 0 , 0 , 0 ) , ( n x , n y , n z ) = ( 0 , 0 , 1 ) (x_0,y_0,z_0)=(0,0,0),(n_x,n_y,n_z)=(0,0,1) (x0,y0,z0)=(0,0,0),(nx,ny,nz)=(0,0,1)时,
T 4 × 4 = [ c o s ( α ) − s i n ( α ) 0 0 s i n ( α ) c o s ( α ) 0 0 0 0 1 0 0 0 0 1 ] (15) T_{4\times4}=\left[ \begin{matrix} cos(\alpha) & -sin(\alpha) & 0 & 0 \\ sin(\alpha) & cos(\alpha) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right] \tag{15} T4×4=cos(α)sin(α)00sin(α)cos(α)0000100001(15)
  4.当旋转轴过点 ( 0 , t y , t z ) (0,t_y,t_z) (0,ty,tz)且平行于 x x x轴,则 ( x 0 , y 0 , z 0 ) = ( 0 , t y , t z ) , ( n x , n y , n z ) = ( 1 , 0 , 0 ) (x_0,y_0,z_0)=(0,t_y,t_z),(n_x,n_y,n_z)=(1,0,0) (x0,y0,z0)=(0,ty,tz),(nx,ny,nz)=(1,0,0)时,
T 4 × 4 = [ 1 0 0 0 0 c o s ( α ) − s i n ( α ) t y K + t z s i n ( α ) 0 s i n ( α ) c o s ( α ) t z K − t y s i n ( α ) 0 0 0 1 ] (16) T_{4\times4}=\left[ \begin{matrix} 1 & 0 & 0 & 0 \\ 0 & cos(\alpha) & -sin(\alpha) & t_yK+t_zsin(\alpha) \\ 0 & sin(\alpha) & cos(\alpha) & t_zK-t_ysin(\alpha) \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right] \tag{16} T4×4=10000cos(α)sin(α)00sin(α)cos(α)00tyK+tzsin(α)tzKtysin(α)1(16)
  5.当旋转轴过点 ( t x , 0 , t z ) (t_x,0,t_z) (tx,0,tz)且平行于 y y y轴,则 ( x 0 , y 0 , z 0 ) = ( t x , 0 , t z ) , ( n x , n y , n z ) = ( 0 , 1 , 0 ) (x_0,y_0,z_0)=(t_x,0,t_z),(n_x,n_y,n_z)=(0,1,0) (x0,y0,z0)=(tx,0,tz),(nx,ny,nz)=(0,1,0)时,
T 4 × 4 = [ c o s ( α ) 0 s i n ( α ) t x K − t z s i n ( α ) 0 1 0 0 − s i n ( α ) 0 c o s ( α ) t z K + t x s i n ( α ) 0 0 0 1 ] (17) T_{4\times4}=\left[ \begin{matrix} cos(\alpha) & 0 & sin(\alpha) & t_xK- t_zsin(\alpha) \\ 0 & 1 & 0 & 0 \\ -sin(\alpha) & 0 & cos(\alpha) & t_zK + t_xsin(\alpha)\\ 0 & 0 & 0 & 1 \\ \end{matrix} \right] \tag{17} T4×4=cos(α)0sin(α)00100sin(α)0cos(α)0txKtzsin(α)0tzK+txsin(α)1(17)
  6.当旋转轴过点 ( t x , t y , 0 ) (t_x,t_y,0) (tx,ty,0)且平行于 z z z轴,则 ( x 0 , y 0 , z 0 ) = ( t x , t y , 0 ) , ( n x , n y , n z ) = ( 0 , 0 , 1 ) (x_0,y_0,z_0)=(t_x,t_y,0),(n_x,n_y,n_z)=(0,0,1) (x0,y0,z0)=(tx,ty,0),(nx,ny,nz)=(0,0,1)时,
T 4 × 4 = [ c o s ( α ) − s i n ( α ) 0 t x K + t y s i n ( α ) s i n ( α ) c o s ( α ) 0 t y K − t x s i n ( α ) 0 0 1 0 0 0 0 1 ] (18) T_{4\times4}=\left[ \begin{matrix} cos(\alpha) & -sin(\alpha) & 0 & t_xK+t_ysin(\alpha)\\ sin(\alpha) & cos(\alpha) & 0 & t_yK- t_xsin(\alpha)\\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right] \tag{18} T4×4=cos(α)sin(α)00sin(α)cos(α)000010txK+tysin(α)tyKtxsin(α)01(18)
  7.当旋转轴过原点 ( 0 , 0 , 0 ) (0,0,0) (0,0,0),则 ( x 0 , y 0 , z 0 ) = ( 0 , 0 , 0 ) (x_0,y_0,z_0)=(0,0,0) (x0,y0,z0)=(0,0,0)时,
T 4 × 4 = [ n x 2 K + c o s ( α ) n x n y K − n z s i n ( α ) n x n z K + n y s i n ( α ) 0 n x n y K + n z s i n ( α ) n y 2 K + c o s ( α ) n y n z K − n x s i n ( α ) 0 n x n z K − n y s i n ( α ) n y n z K + n x s i n ( α ) n z 2 K + c o s ( α ) 0 0 0 0 1 ] (19) T_{4\times4}=\left[ \begin{matrix} n_x^2 K + cos(\alpha) & n_x n_yK - n_z sin(\alpha) & n_xn_z K + n_y sin(\alpha) & 0 \\ n_xn_yK + n_zsin(\alpha) & n_y^2 K + cos(\alpha) & n_yn_zK - n_xsin(\alpha) & 0 \\ n_xn_zK - n_ysin(\alpha) & n_yn_zK + n_xsin(\alpha) & n_z^2K + cos(\alpha) & 0 \\ 0 & 0 & 0 & 1 \\ \end{matrix} \right] \tag{19} T4×4=nx2K+cos(α)nxnyK+nzsin(α)nxnzKnysin(α)0nxnyKnzsin(α)ny2K+cos(α)nynzK+nxsin(α)0nxnzK+nysin(α)nynzKnxsin(α)nz2K+cos(α)00001(19)
   T 4 × 4 T_{4\times4} T4×4的前 3 3 3 3 3 3列就是将三维旋转的轴-角表示转化为旋转矩阵表示:
R 3 × 3 = [ n x 2 K + c o s ( α ) n x n y K − n z s i n ( α ) n x n z K + n y s i n ( α ) n x n y K + n z s i n ( α ) n y 2 K + c o s ( α ) n y n z K − n x s i n ( α ) n x n z K − n y s i n ( α ) n y n z K + n x s i n ( α ) n z 2 K + c o s ( α ) ] (20) R_{3\times3}=\left[ \begin{matrix} n_x^2 K + cos(\alpha) & n_x n_yK - n_z sin(\alpha) & n_xn_z K + n_y sin(\alpha)\\ n_xn_yK + n_zsin(\alpha) & n_y^2 K + cos(\alpha) & n_yn_zK - n_xsin(\alpha) \\ n_xn_zK - n_ysin(\alpha) & n_yn_zK + n_xsin(\alpha) & n_z^2K + cos(\alpha) \\ \end{matrix} \right] \tag{20} R3×3=nx2K+cos(α)nxnyK+nzsin(α)nxnzKnysin(α)nxnyKnzsin(α)ny2K+cos(α)nynzK+nxsin(α)nxnzK+nysin(α)nynzKnxsin(α)nz2K+cos(α)(20)
  其中, K = 1 − c o s ( α ) K = 1 - cos(\alpha) K=1cos(α)

你可能感兴趣的:(计算几何,三维空间任意一点绕任意轴线旋转,旋转矩阵推导,点绕轴旋转,旋转轴)