1 问题描述：
1.1 问题1
已知三维空间中的源向量
p
p
p和目标（参考）向量
q
q
q，计算旋转矩阵
R
R
R，最终能够使得源向量
p
p
p在旋转矩阵
R
R
R的作用下，与目标向量
q
q
q对齐（平行），如下图所示。
表示成公式的形式：
q
=
R
⋅
p
q=R\cdot p
q=R⋅p （1）
式中：
p
=
[
p
x
p
y
p
z
]
p=\begin{bmatrix} p_x\\ p_y\\ p_z \end{bmatrix}
p=⎣⎡pxpypz⎦⎤,
q
=
[
q
x
q
y
q
z
]
q=\begin{bmatrix} q_x\\ q_y\\ q_z \end{bmatrix}
q=⎣⎡qxqyqz⎦⎤,
R
=
[
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
]
R=\begin{bmatrix} r_{11} &r_{12} &r_{13}\\ r_{21} &r_{22} &r_{23}\\ r_{31} &r_{32} &r_{33} \end{bmatrix}
R=⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤。
1.2 问题2
已知三维空间中的源向量
p
p
p和目标（参考）向量
q
q
q，以及一锚点
a
a
a，计算刚体变换矩阵
T
T
T，最终能够使得源向量
p
p
p在矩阵
T
T
T的作用下，与目标向量
q
q
q对齐（平行），且矩阵
T
T
T的作用不会改变锚点
a
a
a的位置，如下图所示。
表示成公式的形式：
[
q
0
]
=
T
⋅
[
p
0
]
\begin{bmatrix} q\\ 0 \end{bmatrix} =T\cdot \begin{bmatrix} p\\ 0 \end{bmatrix}
[q0]=T⋅[p0] （2）
[
a
1
]
=
T
⋅
[
a
1
]
\begin{bmatrix} a\\ 1 \end{bmatrix} =T\cdot \begin{bmatrix} a\\ 1 \end{bmatrix}
[a1]=T⋅[a1] （3）
式中：
T
=
[
R
t
0
1
]
=
[
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
]
T= \begin{bmatrix} R &t\\ 0 &1 \end{bmatrix}= \begin{bmatrix} 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{bmatrix}
T=[R0t1]=⎣⎢⎢⎡r11r21r310r12r22r320r13r23r330txtytz1⎦⎥⎥⎤。
注：齐次形式下，向量的“尾巴”是0，点的“尾巴”是1。
2 解决方案
2.1 问题1
问题可被简化成：源向量
p
p
p绕向量
n
n
n旋转
θ
\theta
θ角度，能够与目标向量
q
q
q对齐，即罗德里格斯旋转。
其中，向量
n
n
n是向量
p
p
p和
q
q
q所在平面的法向量，旋转角度
θ
\theta
θ是向量
p
p
p和
q
q
q之间的夹角，即：
n
=
p
×
q
∣
p
×
q
∣
n=\frac{p\times q}{|p\times q|}
n=∣p×q∣p×q （4）
θ
=
acos
p
⋅
q
∣
p
∣
⋅
∣
q
∣
\theta= \text{acos}\frac{p\cdot q}{|p|\cdot |q|}
θ=acos∣p∣⋅∣q∣p⋅q （5）
参考文章：罗德里格斯旋转公式
可得：
R
=
I
+
(
1
−
c
o
s
θ
)
∗
N
2
+
s
i
n
θ
∗
N
R=I+(1-cos\theta)*N^2+ sin\theta * N
R=I+(1−cosθ)∗N2+sinθ∗N （6）
式中，矩阵
N
N
N是向量
n
n
n的反对称矩阵形式，即：
N
=
n
∧
=
[
0
−
n
z
n
y
n
z
0
−
n
x
−
n
y
n
x
0
]
N=n^{\wedge}= \begin{bmatrix} 0 &-n_z &n_y\\ n_z &0 &-n_x\\ -n_y &n_x &0 \end{bmatrix}
N=n∧=⎣⎡0nz−ny−nz0nxny−nx0⎦⎤ （7）
最终的刚体变换矩阵
T
T
T如下：
T
=
[
R
0
0
1
]
T= \begin{bmatrix} R &0\\ 0 &1 \end{bmatrix}
T=[R001] （8）
2.2 问题2
问题2相对于问题1，增加了锚点，问题可被简化成：源向量
p
p
p以
a
a
a为旋转中心，并绕向量
n
n
n旋转
θ
\theta
θ角度，最终能够与目标向量
q
q
q对齐。
整个变换过程可以分为3个部分，如下：
- 构造坐标系
x
′
y
′
z
′
x’y’z’
x′y′z′
由于是以
a
a
a为旋转中心进行变换，因此，以
a
a
a为原点，构建坐标系
x
′
y
′
z
′
x’y’z’
x′y′z′，各轴方向与坐标系
x
y
z
xyz
xyz的方向一样，因此，坐标系
x
y
z
xyz
xyz相对于坐标系
x
′
y
′
z
′
x’y’z’
x′y′z′的变化矩阵如下：
[
I
−
a
0
1
]
\begin{bmatrix} I &-a\\ 0 &1 \end{bmatrix}
[I0−a1] （9）
式（9）中的变换矩阵能够将坐标系
x
y
z
xyz
xyz中描述的点，转换至坐标系
x
′
y
′
z
′
x’y’z’
x′y′z′空间。
- 在坐标系
x
′
y
′
z
′
x’y’z’
x′y′z′下绕向量n
n
n旋转θ
\theta
θ角度
该过程的变换矩阵可利用罗德里格斯旋转公式获得，即式（8），如下：
[
R
0
0
1
]
\begin{bmatrix} R &0\\ 0 &1 \end{bmatrix}
[R001] （10）
- 变换回坐标系
x
y
z
xyz
xyz
由式（10）计算得到的点坐标都描述在坐标系
x
′
y
′
z
′
x’y’z’
x′y′z′空间，需要变换回原始坐标系
x
y
z
xyz
xyz中，变换矩阵如下：
[
I
a
0
1
]
\begin{bmatrix} I &a\\ 0 &1 \end{bmatrix}
[I0a1] （11）
合并上述3个过程，即联立式（9）-（11），如下：
T
=
[
I
a
0
1
]
[
R
0
0
1
]
[
I
−
a
0
1
]
=
[
R
−
R
⋅
a
+
a
0
1
]
T=\begin{bmatrix} I &a\\ 0 &1 \end{bmatrix} \begin{bmatrix} R &0\\ 0 &1 \end{bmatrix} \begin{bmatrix} I &-a\\ 0 &1 \end{bmatrix}= \begin{bmatrix} R &-R\cdot a+a\\ 0 &1 \end{bmatrix}
T=[I0a1][R001][I0−a1]=[R0−R⋅a+a1] （12）
上式完整展开如下：
{
T
[
0
]
[
0
]
=
u
2
+
(
v
2
+
w
2
)
∗
c
o
s
θ
T
[
0
]
[
1
]
=
u
∗
v
∗
(
1
−
c
o
s
θ
)
−
w
∗
s
i
n
θ
T
[
0
]
[
2
]
=
u
∗
w
∗
(
1
−
c
o
s
θ
)
+
v
∗
s
i
n
θ
T
[
0
]
[
3
]
=
(
x
∗
(
v
2
+
w
2
)
−
u
∗
(
y
∗
v
+
z
∗
w
)
)
∗
(
1
−
c
o
s
θ
)
+
(
y
∗
w
−
z
∗
v
)
∗
s
i
n
θ
T
[
1
]
[
0
]
=
u
∗
v
∗
(
1
−
c
o
s
θ
)
+
w
∗
s
i
n
θ
T
[
1
]
[
1
]
=
v
2
+
(
u
2
+
w
2
)
∗
c
o
s
θ
T
[
1
]
[
2
]
=
v
∗
w
∗
(
1
−
c
o
s
θ
)
−
u
∗
s
i
n
θ
T
[
1
]
[
3
]
=
(
y
∗
(
u
2
+
w
2
)
−
v
∗
(
x
∗
u
+
z
∗
w
)
)
∗
(
1
−
c
o
s
θ
)
+
(
z
∗
u
−
x
∗
w
)
∗
s
i
n
θ
T
[
2
]
[
0
]
=
u
∗
w
∗
(
1
−
c
o
s
θ
)
−
v
∗
s
i
n
θ
T
[
2
]
[
1
]
=
v
∗
w
∗
(
1
−
c
o
s
θ
)
+
u
∗
s
i
n
θ
T
[
2
]
[
2
]
=
w
2
+
(
u
2
+
v
2
)
∗
c
o
s
θ
T
[
2
]
[
3
]
=
(
z
∗
(
u
2
+
v
2
)
−
w
∗
(
x
∗
u
+
y
∗
v
)
)
∗
(
1
−
c
o
s
θ
)
+
(
x
∗
v
−
y
∗
u
)
∗
s
i
n
θ
\left\{\begin{matrix} \begin{aligned} &T[0][0]=u^2+(v^2+w^2)*cos\theta \\ &T[0][1]=u*v*(1-cos\theta)-w*sin\theta \\ &T[0][2]=u*w*(1-cos\theta)+v*sin\theta \\ &T[0][3]=(x*(v^2+w^2)-u*(y*v+z*w))*(1-cos\theta)+(y*w-z*v)*sin\theta \\ &T[1][0]=u*v*(1-cos\theta)+w*sin\theta \\ &T[1][1]=v^2+(u^2+w^2)*cos\theta \\ &T[1][2]=v*w*(1-cos\theta)-u*sin\theta \\ &T[1][3]=(y*(u^2+w^2)-v*(x*u+z*w))*(1-cos\theta)+ (z*u-x*w)*sin\theta \\ &T[2][0]=u*w*(1-cos\theta)-v*sin\theta \\ &T[2][1]=v*w*(1-cos\theta)+u*sin\theta \\ &T[2][2]=w^2+(u^2+v^2)*cos\theta \\ &T[2][3]=(z*(u^2+v^2)-w*(x*u+y*v))*(1-cos\theta)+(x*v-y*u)*sin\theta \end{aligned} \end{matrix}\right.
⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧T[0][0]=u2+(v2+w2)∗cosθT[0][1]=u∗v∗(1−cosθ)−w∗sinθT[0][2]=u∗w∗(1−cosθ)+v∗sinθT[0][3]=(x∗(v2+w2)−u∗(y∗v+z∗w))∗(1−cosθ)+(y∗w−z∗v)∗sinθT[1][0]=u∗v∗(1−cosθ)+w∗sinθT[1][1]=v2+(u2+w2)∗cosθT[1][2]=v∗w∗(1−cosθ)−u∗sinθT[1][3]=(y∗(u2+w2)−v∗(x∗u+z∗w))∗(1−cosθ)+(z∗u−x∗w)∗sinθT[2][0]=u∗w∗(1−cosθ)−v∗sinθT[2][1]=v∗w∗(1−cosθ)+u∗sinθT[2][2]=w2+(u2+v2)∗cosθT[2][3]=(z∗(u2+v2)−w∗(x∗u+y∗v))∗(1−cosθ)+(x∗v−y∗u)∗sinθ （13）
式中，法向量
n
=
[
u
,
v
,
w
]
T
n=[u,v,w]^T
n=[u,v,w]T，锚点
a
=
[
x
,
y
,
z
]
T
a=[x,y,z]^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]