坐标旋转问题

      给定任意单位轴q(q1,q2,q3)(向量),求向量p(x,y,z)(或点p)饶q旋转theta角度的变换后的新向量p'(或点p’)。
      使用旋转矩阵工具：构造旋转矩阵变换Trot,则变换后的新向量p’= p*Trot，其中，p’（x’,y’,z’,1）,p(x,y,z,1)为向量p’，p的4D齐次坐标表示。

          |t*q1*q1 + c,          t*q1*q2 + s*q3,      t*q1*q3 – s*q2,       0|
Trot =  |t*xq1*q2 – s*q3,    t*q2*q2 + c,           t*q2*q3 + s*q1,      0|
          |t*q1*q3 + s*q2,     t*q2*q3 – s*q1,      t*q3*q3 + c,           0|
          |0,                         0,                         0,                          1|

    其中c=cos(theta), s=sin(theta),t=1-c.