已知两向量，计算能够使两向量对齐的刚体变换矩阵

1 问题描述：

1.1 问题1

已知三维空间中的源向量$p$和目标（参考）向量$q$，计算旋转矩阵$R$，最终能够使得源向量$p$在旋转矩阵$R$的作用下，与目标向量$q$对齐（平行），如下图所示。

表示成公式的形式：

$q=R\cdot p$ （1）
式中：
$p=\left[\begin{array}{c}{p}_x\\ p_y\\ p_z\end{array}\right]$, $q=\left[\begin{array}{c}{q}_x\\ q_y\\ q_z\end{array}\right]$, $R=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$。

1.2 问题2

已知三维空间中的源向量$p$和目标（参考）向量$q$，以及一锚点$a$，计算刚体变换矩阵$T$，最终能够使得源向量$p$在矩阵$T$的作用下，与目标向量$q$对齐（平行），且矩阵$T$的作用不会改变锚点$a$的位置，如下图所示。

表示成公式的形式：

$\left[\begin{array}{c}q\\ 0\end{array}\right]=T\cdot \left[\begin{array}{c}p\\ 0\end{array}\right]$ （2）

$\left[\begin{array}{c}a\\ 1\end{array}\right]=T\cdot \left[\begin{array}{c}a\\ 1\end{array}\right]$ （3）
式中：
$T=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]=\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& t_x\\ r_{21}& r_{22}& r_{23}& t_y\\ r_{31}& r_{32}& r_{33}& t_z\\ 0& 0& 0& 1\end{array}\right]$。

注：齐次形式下，向量的“尾巴”是0，点的“尾巴”是1。

2 解决方案

2.1 问题1

问题可被简化成：源向量$p$绕向量$n$旋转$\theta$角度，能够与目标向量$q$对齐，即罗德里格斯旋转。
其中，向量$n$是向量$p$和$q$所在平面的法向量，旋转角度$\theta$是向量$p$和$q$之间的夹角，即：

$n=\frac{p\times q}{|p\times q|}$ （4）

$\theta =\text{acos}\frac{p\cdot q}{|p|\cdot |q|}$ （5）
参考文章：罗德里格斯旋转公式
可得：
$R=I+(1-cos\theta )\ast N^2+sin\theta \ast N$ （6）
式中，矩阵 $N$是向量 $n$的反对称矩阵形式，即：
$N=n^{\wedge }=\left[\begin{array}{ccc}0& -n_z& n_y\\ n_z& 0& -n_x\\ -n_y& n_x& 0\end{array}\right]$ （7）
最终的刚体变换矩阵 $T$如下：
$T=\left[\begin{array}{cc}R& 0\\ 0& 1\end{array}\right]$ （8）

2.2 问题2

问题2相对于问题1，增加了锚点，问题可被简化成：源向量$p$以$a$为旋转中心，并绕向量$n$旋转$\theta$角度，最终能够与目标向量$q$对齐。
整个变换过程可以分为3个部分，如下：

1. 构造坐标系$x^{\prime }{y}^{\prime }{z}^{\prime }$

由于是以$a$为旋转中心进行变换，因此，以$a$为原点，构建坐标系$x^{\prime }{y}^{\prime }{z}^{\prime }$，各轴方向与坐标系$xyz$的方向一样，因此，坐标系$xyz$相对于坐标系$x^{\prime }{y}^{\prime }{z}^{\prime }$的变化矩阵如下：

$\left[\begin{array}{cc}I& -a\\ 0& 1\end{array}\right]$ （9）
式（9）中的变换矩阵能够将坐标系 $xyz$中描述的点，转换至坐标系 $x^{\prime }{y}^{\prime }{z}^{\prime }$空间。

2. 在坐标系$x^{\prime }{y}^{\prime }{z}^{\prime }$下绕向量$n$旋转$\theta$角度

该过程的变换矩阵可利用罗德里格斯旋转公式获得，即式（8），如下：

$\left[\begin{array}{cc}R& 0\\ 0& 1\end{array}\right]$ （10）

3. 变换回坐标系$xyz$

由式（10）计算得到的点坐标都描述在坐标系$x^{\prime }{y}^{\prime }{z}^{\prime }$空间，需要变换回原始坐标系$xyz$中，变换矩阵如下：

$\left[\begin{array}{cc}I& a\\ 0& 1\end{array}\right]$ （11）
合并上述3个过程，即联立式（9）-（11），如下：
$T=\left[\begin{array}{cc}I& a\\ 0& 1\end{array}\right]\left[\begin{array}{cc}R& 0\\ 0& 1\end{array}\right]\left[\begin{array}{cc}I& -a\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}R& -R\cdot a+a\\ 0& 1\end{array}\right]$ （12）
上式完整展开如下：
$\{\begin{array}{c}\begin{array}{rl}& T[0][0]=u^2+(v^2+w^2)\ast cos\theta \\ & T[0][1]=u\ast v\ast (1-cos\theta )-w\ast sin\theta \\ & T[0][2]=u\ast w\ast (1-cos\theta )+v\ast sin\theta \\ & T[0][3]=(x\ast (v^2+w^2)-u\ast (y\ast v+z\ast w))\ast (1-cos\theta )+(y\ast w-z\ast v)\ast sin\theta \\ & T[1][0]=u\ast v\ast (1-cos\theta )+w\ast sin\theta \\ & T[1][1]=v^2+(u^2+w^2)\ast cos\theta \\ & T[1][2]=v\ast w\ast (1-cos\theta )-u\ast sin\theta \\ & T[1][3]=(y\ast (u^2+w^2)-v\ast (x\ast u+z\ast w))\ast (1-cos\theta )+(z\ast u-x\ast w)\ast sin\theta \\ & T[2][0]=u\ast w\ast (1-cos\theta )-v\ast sin\theta \\ & T[2][1]=v\ast w\ast (1-cos\theta )+u\ast sin\theta \\ & T[2][2]=w^2+(u^2+v^2)\ast cos\theta \\ & T[2][3]=(z\ast (u^2+v^2)-w\ast (x\ast u+y\ast v))\ast (1-cos\theta )+(x\ast v-y\ast u)\ast sin\theta \end{array}\end{array}$ （13）
式中，法向量 $n=[u,v,w{]}^T$，锚点 $a=[x,y,z{]}^T$。

3 代码实现

point_anchor = [10;20;30];			%  锚点
vec_src = [sqrt(3)/3;sqrt(3)/3;sqrt(3)/3];		% 源向量
vec_dst = [0;0;1];					% 目标向量
axis_rotation = cross(vec_src,vec_dst);				% 叉积
axis_rotation = axis_rotation/norm(axis_rotation);			% 单位法向量
angle = acos(dot(vec_src,vec_dst));				% 计算旋转角
axis_rotation_x = axis_rotation(1);
axis_rotation_y = axis_rotation(2);
axis_rotation_z = axis_rotation(3);
N = [0,-axis_rotation_z,axis_rotation_y;		% 单位法向量对应的反对称矩阵形式
    axis_rotation_z,0,-axis_rotation_x;
    -axis_rotation_y,axis_rotation_x,0];
R=eye(3)+sin(angle)*N+(1-cos(angle))*N*N;		% 根据罗德里格斯旋转公式求解得到的旋转矩阵

T = [R,-R*point_anchor+point_anchor;			% 最终需要的刚体变换矩阵
    0,0,0,1]