求饶不经过原点的旋转轴的旋转矩阵

看《机器人导论》中关于齐次变换的内容中，发现饶轴旋转的时候，分为两种情况。一、饶经过原点的轴进行旋转；二、饶不经过原点的旋转轴进行旋转。其实这两种情况下，采用旋转向量 和 旋转矩阵的相互转换的几种方法 这里面的方法都是可以求到旋转矩阵的，但是这两种旋转方式中的变换矩阵中的位置矩阵是不一样的。饶经过原点的轴进行旋转的变换矩阵，由于原点的位置没有发生改变，所以变换矩阵中的位置矩阵是零，而第二种方法中的由于旋转轴不在原点，由此变换矩阵中的位置矩阵不为零。

可以参考这里的描述[绕任意轴旋转]，这里求出的是4*4的变换矩阵(https://www.cnblogs.com/graphics/archive/2012/08/10/2627458.html)

[绕任意轴旋转]，这里求出的是3*3的变换矩阵(https://blog.csdn.net/chao56789/article/details/48138519)

function testtest()

%% 起始位姿矩阵
T1= [  -0.0008 ,  -0.0005 ,   0.0004 ,   0.2113,
    0.0004,   -0.0009 ,  -0.0002  ,  0.0202,
    0.0004 ,  -0.0000 ,   0.0009 ,   1.2244,
         0  ,       0  ,       0  ,  0.0010];
         
T= 1.0e+03 *T1;

pi=3.1415926;
qq_1= - pi/6;

p1=[0.8;0.6;0.5];
p2=[1;2;3];

PP=p2-p1;

PP=PP/sqrt( PP(1)^2 + PP(2)^2 + PP(3)^2 );
w_1=PP;          %每个关节转轴在全局坐标系中的方向
r_1=p2;         %旋量中一点在全局坐标系中的位置
c_1=[cross(r_1,w_1);w_1];
R_1 = C_poe(c_1,qq_1);

R_3=zeros(3,3);
R_3(1,1)=PP(1)^2 *(1 - cos(qq_1)) + cos(qq_1);
R_3(1,2)=PP(1)*PP(2) *(1 - cos(qq_1)) - PP(3)*sin(qq_1);
R_3(1,3)=PP(1)*PP(3) *(1 - cos(qq_1)) +  PP(2)*sin(qq_1);
R_3(2,1)=PP(1)*PP(2) *(1 - cos(qq_1)) + PP(3)*sin(qq_1);
R_3(2,2)=PP(2)^2 *(1 - cos(qq_1)) + cos(qq_1);
R_3(2,3)=PP(2)*PP(3) *(1 - cos(qq_1)) - PP(1)*sin(qq_1);
R_3(3,1)=PP(1)*PP(3) *(1 - cos(qq_1)) -  PP(2)*sin(qq_1);
R_3(3,2)=PP(2)*PP(3) *(1 - cos(qq_1)) + PP(1)*sin(qq_1);
R_3(3,3)=PP(3)^2 *(1 - cos(qq_1)) + cos(qq_1);

T_2_1=[eye(3),p2; 0 0 0 1];                      
T_2=[R_3,zeros(3,1);0 0 0 1];
T_2_2=[eye(3),-p2; 0 0 0 1];
T_2=T_2_1*T_2*T_2_2;
%% 基于旋量求得的变换矩阵
disp(R_1)
%% 基于《机器人导论》中介绍的方法求得的变换矩阵
disp(T_2)

disp(R_1*T)
disp(T_2*T)

%% 从下面结果可以看出两者是一致的
   0.8667    0.4397   -0.2356   -0.0394
   -0.4306    0.8979    0.0917    0.3600
    0.2518    0.0220    0.9675   -0.1984
         0         0         0    1.0000

    0.8667    0.4397   -0.2356   -0.0394
   -0.4306    0.8979    0.0917    0.3600
    0.2518    0.0220    0.9675   -0.1984
         0         0         0    1.0000

   1.0e+03 *

   -0.0006   -0.0008    0.0000   -0.0965
    0.0007   -0.0006   -0.0003    0.0397
    0.0002   -0.0001    0.0010    1.2381
         0         0         0    0.0010

   1.0e+03 *

   -0.0006   -0.0008    0.0000   -0.0965
    0.0007   -0.0006   -0.0003    0.0397
    0.0002   -0.0001    0.0010    1.2381
         0         0         0    0.0010

补充：如果不乘T_2_1 和 T_2_2 那么理论上得到的结果是旋转轴并不是由 p1 p2确定的旋转轴，而是由 PP 和 坐标系原点确定的坐标轴。饶轴旋转的半径是 起点到旋转轴的直线距离。