四元数(Quaternion)食用指南
“这简直就是黑魔法!”
开发时,每次遇到旋转问题时总会心头一震,在欧拉角和四元数这两种处理方式的选择上犹豫不决,不知不觉就陷入了四元数的淤泥中…接下来,我决定发起总攻,好好理一理四元数。
阅读注意:
- 本文的图例以左手坐标系为主
- 空间中的向量采用列项量表示
- 四元数为Hamilton形式
1 左右手坐标系对旋转的影响在哪
我们处于三维世界中,一个旋转可以描述为由一个旋转轴 v \textbf{v} v 和旋转角 θ θ θ 组成。这里其实隐含了一个问题,即 θ \theta θ 的旋转究竟以逆时针为正方向,还是以顺时针为正方向。
左手坐标系下我们可以用左手绕轴,拇指指向正方向,手指旋向为顺时针,即顺时针为正方向。同理,右手坐标系下用右手绕轴,拇指指向正方向,手指旋向为逆时针,即逆时针为正方向。
这里也是关键所在,对于一个旋转角度 θ \theta θ ,在左手坐标系下解释为顺时针旋转 θ \theta θ ,而在右手坐标系下解释为逆时针旋转 θ \theta θ。
也许你在之前还看过其他文章,不同文章在解决旋转矩阵推导、或四元数转旋转矩阵等问题时,结论公式内的正负符号会不一致,甚至还会分出左手坐标系下的旋转矩阵,或右手坐标系下的旋转矩阵的说法。其原因就是隐含的旋转正方向不一样,旋转角出现符号差异。例如:你处理左手坐标系下的旋转,却取逆时针方向为正方向,最后公式自然就有符号差异。
如果正方向随着左右手坐标系改变,最后应该是同一个公式,只是对角度旋向的解释不一样。所以,在以下的叙述中,只有在出现具体某个坐标系下时,我才会强调旋转的方向。
2 令人头疼的旋转
2.1 旋转矩阵
我们首先来看一下旋转矩阵,设有一个三维旋转矩阵
M
M
M,如下所示:
M
=
(
m
00
m
01
m
02
m
10
m
11
m
12
m
20
m
21
m
22
)
M = \begin{pmatrix} m_{00} & m_{01} & m_{02}\\ m_{10} & m_{11} & m_{12}\\ m_{20} & m_{21} & m_{22}\\ \end{pmatrix}
M=⎝⎛m00m10m20m01m11m21m02m12m22⎠⎞
旋转矩阵
M
M
M中各元素与旋转轴和旋转角的对应关系,我们不能一眼直观得出。但这里简化一下,将维度降低到二维。
我们绕原点旋转一个点
p
(
a
,
b
)
p(a,b)
p(a,b) 到
p
′
(
c
,
d
)
p^{'}(c,d)
p′(c,d) ,我们设点
p
p
p 到原点的距离为
l
l
l。那么就有:
a
=
l
cos
α
b
=
l
sin
α
c
=
l
cos
(
α
+
θ
)
=
l
cos
α
cos
θ
−
l
sin
α
sin
θ
=
a
cos
θ
−
b
sin
θ
d
=
l
sin
(
α
+
θ
)
=
l
sin
α
cos
θ
+
l
cos
α
sin
θ
=
a
sin
θ
+
b
cos
θ
a = l\cos\alpha \\ b = l\sin\alpha \\ c = l\cos(\alpha + \theta) = l\cos\alpha\cos\theta - l\sin\alpha\sin\theta = a\cos\theta - b\sin\theta \\ d = l\sin(\alpha + \theta) = l\sin\alpha\cos\theta + l\cos\alpha\sin\theta = a\sin\theta + b\cos\theta
a=lcosαb=lsinαc=lcos(α+θ)=lcosαcosθ−lsinαsinθ=acosθ−bsinθd=lsin(α+θ)=lsinαcosθ+lcosαsinθ=asinθ+bcosθ
我们可以很快地写出一个二维的旋转矩阵
M
2
M_2
M2,他能让点绕原点逆时针旋转θ角度(注意:这里采用矩阵乘列向量的方式),如下所示:
M
2
=
(
cos
θ
−
sin
θ
sin
θ
cos
θ
)
M_2 = \begin{pmatrix} \cos\theta & -\sin\theta\\ \sin\theta & \cos\theta\\ \end{pmatrix}
M2=(cosθsinθ−sinθcosθ)
事实上,三维旋转矩阵也能以类似的几何方式计算出矩阵内各个元素的值,我们将会在四元数部分一起进行推导。我们将上面的二维旋转矩阵扩展到三维,可以得出绕
x
x
x 轴、
y
y
y 轴、
z
z
z 轴的旋转矩阵
R
x
R_x
Rx、
R
y
R_y
Ry 和
R
z
R_z
Rz。
R
x
=
(
1
0
0
0
cos
θ
sin
θ
0
−
sin
θ
cos
θ
)
R
y
=
(
cos
θ
0
−
sin
θ
0
1
0
sin
θ
0
cos
θ
)
R
z
=
(
cos
θ
sin
θ
0
−
sin
θ
cos
θ
0
0
0
1
)
R_x=\begin{pmatrix} 1 & 0 & 0 \\ 0 & \cos\theta & \sin\theta \\ 0 & -\sin\theta & \cos\theta \\ \end{pmatrix}\quad R_y=\begin{pmatrix} \cos\theta & 0 & -\sin\theta\\ 0 & 1 & 0 \\ \sin\theta & 0 & \cos\theta\\ \end{pmatrix} \quad R_z=\begin{pmatrix} \cos\theta & \sin\theta & 0\\ -\sin\theta & \cos\theta & 0\\ 0 & 0 & 1\\ \end{pmatrix} \quad
Rx=⎝⎛1000cosθ−sinθ0sinθcosθ⎠⎞Ry=⎝⎛cosθ0sinθ010−sinθ0cosθ⎠⎞Rz=⎝⎛cosθ−sinθ0sinθcosθ0001⎠⎞
上述推导在左手坐标系中进行,在扩展到三维的时候,要注意哪个轴是竖轴、哪个轴是横轴,按顺序来就好。
在实际应用中,旋转矩阵 R R R大部分情况下会与缩放矩阵 S S S 和平移矩阵 T T T 组成一个变换矩阵 C = S R T C=SRT C=SRT 。如果单纯用矩阵来旋转计算成本是有点高的。
2.2 欧拉角
欧拉角简单地说就是由三个独立的旋转角度构成,每个分量记录绕各自轴旋转的角度,我们简单地写为:
E
u
l
e
r
A
n
g
l
e
=
(
α
,
β
,
γ
)
EulerAngle = (\alpha ,\beta ,\gamma)
EulerAngle=(α,β,γ)
这其实是比较符合日常生活习惯的一种表达方式,例如,我们描述一个旋转时,会说它绕
x
x
x轴转了30度,再绕
y
y
y轴转了40度,最后绕
z
z
z轴转了50度。这其中就有了一个先后旋转顺序X-Y-Z
。 我们按照这个顺序,用旋转矩阵的方式写出来。
这个旋转矩阵有个致命的缺陷,这也是欧拉角的不足之处——万向锁问题。
当
β
=
9
0
°
\beta = 90^°
β=90° 时,旋转矩阵会变成以下形式:
看上去有点类似绕xyz
轴的旋转矩阵,如果我们把这个矩阵乘以一个列向量
P
(
x
,
y
,
z
)
P(x,y,z)
P(x,y,z)
P
′
=
R
z
R
y
90
R
x
P
=
[
y
sin
(
γ
+
α
)
−
z
cos
(
γ
+
α
)
y
cos
(
γ
+
α
)
+
z
sin
(
γ
+
α
)
x
]
P^{'}=R_zR_{y90}R_xP = \begin{bmatrix} y\sin(\gamma + \alpha)-z\cos(\gamma + \alpha)\\ y\cos(\gamma + \alpha)+z\sin(\gamma + \alpha)\\ x\\ \end{bmatrix}
P′=RzRy90RxP=⎣⎡ysin(γ+α)−zcos(γ+α)ycos(γ+α)+zsin(γ+α)x⎦⎤
这里可以看到无论怎么调整
γ
\gamma
γ 和
α
\alpha
α ,旋转后的点只会在XY
平面转动,第一次旋转和最后一次旋转等价。
2.3 四元数
在进行四元数这部分,我准备从结论反向推导一波。
我们定义一个四元数
q
(
x
,
y
,
z
,
w
)
q(x,y,z,w)
q(x,y,z,w) ,
w
w
w 是实数部分,
i
2
=
j
2
=
k
2
=
i
j
k
=
−
1
i^2=j^2=k^2=ijk=-1
i2=j2=k2=ijk=−1 ,
i
i
i、
j
j
j 和
k
k
k 为虚数,它们关系如下:
q
=
w
+
x
i
+
y
j
+
z
k
q = w + xi + yj + zk
q=w+xi+yj+zk
四元数也可以用有序对记为
q
=
[
w
,
v
]
q = [w,\textbf{v}]
q=[w,v],其中
v
\textbf{v}
v 为向量
(
x
,
y
,
z
)
(x,y,z)
(x,y,z),代表
v
=
x
i
+
y
j
+
z
k
\textbf{v} = xi + yj + zk
v=xi+yj+zk。
那么虚数就有对应的写法:
i
=
[
0
,
i
]
j
=
[
0
,
j
]
k
=
[
0
,
k
]
i = [0,\textbf{i}] \quad j=[0,\textbf{j}] \quad k=[0,\textbf{k}]
i=[0,i]j=[0,j]k=[0,k]
上面的向量
i
\textbf{i}
i 相当于
(
1
,
0
,
0
)
(1,0,0)
(1,0,0) ,向量
j
\textbf{j}
j 相当于
(
0
,
1
,
0
)
(0,1,0)
(0,1,0) ,向量
k
\textbf{k}
k 相当于
(
0
,
0
,
1
)
(0,0,1)
(0,0,1) 。
那么有几个特殊的四元数,我们需要注意一下:
- 纯四元数(Pure Quaternion) 定义为 q ( x , y , z , 0 ) q(x,y,z,0) q(x,y,z,0) ,用有序对记为 [ 0 , v ] [0,\textbf{v}] [0,v]。
- 单位四元数(Unit quaternion) 定义为 q ( x , y , z , w ) q(x,y,z,w) q(x,y,z,w),其中 x 2 + y 2 + z 2 + w 2 = 1 x^2+y^2+z^2+w^2 = 1 x2+y2+z2+w2=1,用有序对可以记为 [ cos θ , sin θ n ] [\cos\theta,\sin\theta\textbf{n}] [cosθ,sinθn] ,其中 n \textbf{n} n 为单位向量。
2.3.1 四元数相乘会发生什么
那么我们定义两个四元数
q
a
(
x
a
,
y
a
,
z
a
,
w
a
)
q_a(x_a,y_a,z_a,w_a)
qa(xa,ya,za,wa) 和
q
b
(
x
b
,
y
b
,
z
b
,
w
b
)
q_b(x_b,y_b,z_b,w_b)
qb(xb,yb,zb,wb) ,可分别记为
[
w
a
,
a
]
[w_a,\textbf{a}]
[wa,a] 和
[
w
b
,
b
]
[w_b,\textbf{b}]
[wb,b],其中:
a
=
x
a
i
+
y
a
j
+
z
a
k
b
=
x
b
i
+
y
b
j
+
z
b
k
\textbf{a} = x_ai + y_aj + z_ak\\ \textbf{b} = x_bi + y_bj + z_bk
a=xai+yaj+zakb=xbi+ybj+zbk
接下来,看看两个四元数相乘会发生什么。
q
a
q
b
=
[
w
a
,
a
]
[
w
b
,
b
]
=
(
w
a
+
x
a
i
+
y
a
j
+
z
a
k
)
(
w
b
+
x
b
i
+
y
b
j
+
z
b
k
)
=
(
w
a
w
b
−
x
a
x
b
−
y
a
y
b
−
z
a
z
b
)
+
(
w
a
x
b
+
w
b
x
a
+
y
a
z
b
−
y
b
z
a
)
[
0
,
i
]
+
(
w
a
y
b
+
w
b
y
a
+
z
a
x
b
−
z
b
x
a
)
[
0
,
j
]
+
(
w
a
z
b
+
w
b
z
a
+
x
a
y
b
−
x
b
y
a
)
[
0
,
k
]
\begin{aligned} q_aq_b =& [w_a,\textbf{a}][w_b,\textbf{b}]\\ =& (w_a + x_ai + y_aj + z_ak)(w_b + x_bi + y_bj + z_bk)\\ =& (w_aw_b - x_ax_b - y_ay_b - z_az_b) \\ &+ (w_ax_b + w_bx_a + y_az_b-y_bz_a )[0,\textbf{i}] \\ &+ (w_ay_b + w_by_a + z_ax_b-z_bx_a )[0,\textbf{j}] \\ &+ (w_az_b + w_bz_a + x_ay_b-x_by_a )[0,\textbf{k}] \\ \end{aligned}
qaqb===[wa,a][wb,b](wa+xai+yaj+zak)(wb+xbi+ybj+zbk)(wawb−xaxb−yayb−zazb)+(waxb+wbxa+yazb−ybza)[0,i]+(wayb+wbya+zaxb−zbxa)[0,j]+(wazb+wbza+xayb−xbya)[0,k]
换算过程中注意通过
i
j
k
=
−
1
ijk=-1
ijk=−1 可以推出
i
j
=
k
ij = k
ij=k 、
j
i
=
−
k
ji = -k
ji=−k 等等式。注意,我们正在尝试通过
q
=
[
w
,
a
]
q = [w,\textbf{a}]
q=[w,a]这种写法在计算过程消除虚根的影响。
q
a
q
b
=
[
w
a
,
a
]
[
w
b
,
b
]
=
(
w
a
w
b
−
x
a
x
b
−
y
a
y
b
−
z
a
z
b
)
[
1
,
0
]
+
(
w
a
x
b
+
w
b
x
a
+
y
a
z
b
−
y
b
z
a
)
[
0
,
i
]
+
(
w
a
y
b
+
w
b
y
a
+
z
a
x
b
−
z
b
x
a
)
[
0
,
j
]
+
(
w
a
z
b
+
w
b
z
a
+
x
a
y
b
−
x
b
y
a
)
[
0
,
k
]
=
[
w
a
w
b
−
x
a
x
b
−
y
a
y
b
−
z
a
z
b
,
0
]
+
[
0
,
(
w
a
x
b
+
w
b
x
a
+
y
a
z
b
−
y
b
z
a
)
i
]
+
[
0
,
(
w
a
y
b
+
w
b
y
a
+
z
a
x
b
−
z
b
x
a
)
j
]
+
[
0
,
(
w
a
z
b
+
w
b
z
a
+
x
a
y
b
−
x
b
y
a
)
k
]
=
[
w
a
w
b
−
x
a
x
b
−
y
a
y
b
−
z
a
z
b
,
w
a
(
x
a
i
+
y
a
j
+
z
a
k
)
+
w
b
(
x
b
i
+
y
b
j
+
z
b
k
)
+
(
y
a
z
b
−
y
b
z
a
)
i
+
(
z
a
x
b
−
z
b
x
a
)
j
+
(
x
a
y
b
−
x
b
y
a
)
k
]
=
[
w
a
w
b
−
a
⋅
b
,
w
a
b
+
w
b
a
+
a
×
b
]
\begin{aligned} q_aq_b =& [w_a,\textbf{a}][w_b,\textbf{b}]\\ =& (w_aw_b - x_ax_b - y_ay_b - z_az_b)[1,\textbf{0}] \\ &+ (w_ax_b + w_bx_a + y_az_b-y_bz_a )[0,\textbf{i}] \\ &+ (w_ay_b + w_by_a + z_ax_b-z_bx_a )[0,\textbf{j}] \\ &+ (w_az_b + w_bz_a + x_ay_b-x_by_a )[0,\textbf{k}] \\ =&[w_aw_b - x_ax_b - y_ay_b - z_az_b,\textbf{0}] \\ &+[0,(w_ax_b + w_bx_a + y_az_b-y_bz_a)\textbf{i}]\\ &+[0,(w_ay_b + w_by_a + z_ax_b-z_bx_a )\textbf{j}] \\ &+[0,(w_az_b + w_bz_a + x_ay_b-x_by_a )\textbf{k}] \\ =&[w_aw_b - x_ax_b - y_ay_b - z_az_b,\\ &w_a(x_a\textbf{i} + y_a\textbf{j} + z_a\textbf{k}) + w_b(x_b\textbf{i} + y_b\textbf{j} + z_b\textbf{k})\\ &+ (y_az_b-y_bz_a)\textbf{i} + (z_ax_b - z_bx_a)\textbf{j} + (x_ay_b - x_by_a)\textbf{k}]\\ =&[w_aw_b-\textbf{a}\cdot\textbf{b},w_a\textbf{b}+w_b\textbf{a} + \textbf{a}\times\textbf{b}] \end{aligned}
qaqb=====[wa,a][wb,b](wawb−xaxb−yayb−zazb)[1,0]+(waxb+wbxa+yazb−ybza)[0,i]+(wayb+wbya+zaxb−zbxa)[0,j]+(wazb+wbza+xayb−xbya)[0,k][wawb−xaxb−yayb−zazb,0]+[0,(waxb+wbxa+yazb−ybza)i]+[0,(wayb+wbya+zaxb−zbxa)j]+[0,(wazb+wbza+xayb−xbya)k][wawb−xaxb−yayb−zazb,wa(xai+yaj+zak)+wb(xbi+ybj+zbk)+(yazb−ybza)i+(zaxb−zbxa)j+(xayb−xbya)k][wawb−a⋅b,wab+wba+a×b]
计算过程看上去很多,其实十分简单,我们有了最后化简的结果,四元数的魔法就变得清晰了起来。
q
a
q
b
=
[
w
a
,
a
]
[
w
b
,
b
]
=
[
w
a
w
b
−
a
⋅
b
,
w
a
b
+
w
b
a
+
a
×
b
]
\begin{aligned} q_aq_b =& [w_a,\textbf{a}][w_b,\textbf{b}] = [w_aw_b-\textbf{a}\cdot\textbf{b},w_a\textbf{b}+w_b\textbf{a} + \textbf{a}\times\textbf{b}] \end{aligned}
qaqb=[wa,a][wb,b]=[wawb−a⋅b,wab+wba+a×b]
2.3.2 单位四元数和向量相乘会旋转吗
我们设有一个单位四元数
q
u
n
i
t
(
x
u
,
y
u
,
z
u
,
w
u
)
q_{unit}(x_u,y_u,z_u,w_u)
qunit(xu,yu,zu,wu) 可记为
[
cos
θ
,
sin
θ
n
]
[\cos\theta,\sin\theta\textbf{n}]
[cosθ,sinθn]。设有一个向量
p
(
x
a
,
y
a
,
z
a
)
p(x_a,y_a,z_a)
p(xa,ya,za),我们将其看作一个纯四元数
q
p
(
x
a
,
y
a
,
z
a
,
0
)
q_p(x_a,y_a,z_a,0)
qp(xa,ya,za,0) ,那么四元数和向量相乘就变成了两个四元数相乘。
p
′
=
q
u
n
i
t
p
=
[
cos
θ
,
sin
θ
n
]
[
0
,
p
]
=
[
−
sin
θ
n
⋅
p
,
cos
θ
p
+
sin
θ
n
×
p
]
\begin{aligned} p^{'}= q_{unit}p =&[\cos\theta,\sin\theta\textbf{n}][0,\textbf{p}]\\ =&[-\sin\theta\textbf{n}\cdot\textbf{p},\cos\theta\textbf{p} + \sin\theta\textbf{n}\times\textbf{p}] \end{aligned}
p′=qunitp==[cosθ,sinθn][0,p][−sinθn⋅p,cosθp+sinθn×p]
emmmm,这tm是啥?好像直接品不出啥几何关系。
仔细想一想,我们的预期是通过四元数乘向量实现某种变换,得到一个新的向量 p ′ ( x b , y b , z b ) p^{'}(x_b,y_b,z_b) p′(xb,yb,zb) ,或者说一个新的纯四元数 ( x b , y b , z b , 0 ) (x_b,y_b,z_b,0) (xb,yb,zb,0)。那么,就需要变换的结果实部为0,即 − sin θ n ⋅ p = 0 -\sin\theta\textbf{n}\cdot\textbf{p} = 0 −sinθn⋅p=0 。
假设
sin
θ
=
0
\sin\theta= 0
sinθ=0 没有推导下去的意义,好像只有当向量
n
\textbf{n}
n 和向量
p
\textbf{p}
p 垂直时,这个结果有点意思:
[
−
sin
θ
n
⋅
p
,
cos
θ
p
+
sin
θ
n
×
p
]
=
[
0
,
cos
θ
p
+
sin
θ
m
]
[-\sin\theta\textbf{n}\cdot\textbf{p},\cos\theta\textbf{p} + \sin\theta\textbf{n}\times\textbf{p}] = [0,\cos\theta\textbf{p} + \sin\theta\textbf{m}]
[−sinθn⋅p,cosθp+sinθn×p]=[0,cosθp+sinθm]
其中
m
=
n
×
p
\textbf{m}=\textbf{n}\times\textbf{p}
m=n×p , 由于
n
\textbf{n}
n是单位向量,向量
m
\textbf{m}
m的模长和向量
p
\textbf{p}
p的模长一致,且他们三个向量互相垂直。
这个结果可以说是十分清晰了:
注意,我们在左手坐标系下谈论此问题,不要弄错 n × p \textbf{n}\times\textbf{p} n×p的方向。
当向量 n \textbf{n} n 和向量 p \textbf{p} p 垂直时,单位四元数 [ cos θ , sin θ n ] [\cos\theta,\sin\theta\textbf{n}] [cosθ,sinθn]乘向量 p \textbf{p} p时,会使 p \textbf{p} p在左手坐标系下以向量 n \textbf{n} n 为旋转轴,绕轴顺时针旋转 θ \theta θ 个角度!!!(左手坐标系下绕轴顺时针是正方向,上右图是顺着n方向看才会这样,可以自己用左手大拇指顺着n方向握住,看看手指的旋向。如果是右手坐标系,此处应解释为逆时针旋转 θ \theta θ )
等等,如果我们不假设向量 n \textbf{n} n 和向量 p \textbf{p} p 垂直,忽略结果的实部的影响,直接看虚部 cos θ p + sin θ n × p \cos\theta\textbf{p} + \sin\theta\textbf{n}\times\textbf{p} cosθp+sinθn×p 实际上也是在绕某个轴旋转 θ \theta θ角度,只不过旋转后的模长不一定和原本保持一致。
唉,这个旋转不完美,如果有某种方式,能够自己干掉 − sin θ n ⋅ p -\sin\theta\textbf{n}\cdot\textbf{p} −sinθn⋅p 就好了。
2.3.3 四元数的模、共轭与逆
在继续前进之前,我们插播一下三个概念~
我们定义一个四元数 q ( x , y , z , w ) q(x,y,z,w) q(x,y,z,w),记为 [ w , v ] [w,\textbf{v}] [w,v]。
-
模长:我们称 q q q 的模长为 ∣ q ∣ = w 2 + ∣ v ∣ 2 |q|=\sqrt{w^2+|\textbf{v}|^2} ∣q∣=w2+∣v∣2 。
-
共轭:我们称 q ∗ ( − x , − y , − z , w ) q^*(-x,-y,-z,w) q∗(−x,−y,−z,w) 为 q q q 的共轭,记为 [ w , − v ] [w,-\textbf{v}] [w,−v]。那么就有:
q ∗ q = [ w , − v ] [ w , v ] = [ w 2 − v ⋅ ( − v ) , w v − w v − v × v ] = [ w 2 + v 2 , 0 ] = ∣ q ∣ 2 \begin{aligned} q^*q=&[w,-\textbf{v}][w,\textbf{v}]\\ =&[w^2-\textbf{v}\cdot(-\textbf{v}),w\textbf{v}-w\textbf{v}-\textbf{v}\times\textbf{v}]\\ =&[w^2+\textbf{v}^2,\textbf{0}]\\ =&|q|^2 \end{aligned} q∗q====[w,−v][w,v][w2−v⋅(−v),wv−wv−v×v][w2+v2,0]∣q∣2 -
逆:当 q − 1 q = 1 q^{-1}q = 1 q−1q=1 时,称四元数 q − 1 q^{-1} q−1 为 q q q 的逆。那么就有:
q ∗ q q − 1 = q ∗ ∣ q ∣ 2 q − 1 = q ∗ q − 1 = q ∗ ∣ q ∣ 2 \begin{aligned} q^*qq^{-1}=&q^*\\ |q|^2q^{-1}=&q^*\\ q^{-1}=&\frac{q^*}{|q|^2}\end{aligned} q∗qq−1=∣q∣2q−1=q−1=q∗q∗∣q∣2q∗
如果 q q q 是一个单位四元数,那么就有:
q u − 1 = q u ∗ q_{u}^{-1}=q_{u}^* qu−1=qu∗
2.3.4 真·四元数旋转
我们设有一个单位四元数 q u ( x u , y u , z u , w u ) q_{u}(x_u,y_u,z_u,w_u) qu(xu,yu,zu,wu) 可记为 [ cos θ , sin θ n ] [\cos\theta,\sin\theta\textbf{n}] [cosθ,sinθn]。那么 q u − 1 q_u^{-1} qu−1 等于 q u ∗ q_u^{*} qu∗可记为 [ cos θ , − sin θ n ] [\cos\theta,-\sin\theta\textbf{n}] [cosθ,−sinθn]。设有一个向量 p ( x a , y a , z a ) p(x_a,y_a,z_a) p(xa,ya,za),我们将其看作一个纯四元数 p ( x a , y a , z a , 0 ) p(x_a,y_a,z_a,0) p(xa,ya,za,0) 。
我们让
q
u
p
q_up
qup 乘上
q
u
−
1
q_u^{-1}
qu−1 。
q
u
p
q
u
−
1
=
[
cos
θ
,
sin
θ
n
]
[
0
,
p
]
[
cos
θ
,
−
sin
θ
n
]
=
[
−
sin
θ
n
⋅
p
,
cos
θ
p
+
sin
θ
n
×
p
]
[
cos
θ
,
−
sin
θ
n
]
=
[
−
sin
θ
cos
θ
n
⋅
p
+
sin
θ
cos
θ
n
⋅
p
+
sin
θ
sin
θ
(
n
×
p
)
⋅
n
,
sin
θ
sin
θ
(
n
⋅
p
)
n
+
cos
θ
cos
θ
p
+
sin
θ
cos
θ
n
×
p
−
sin
θ
cos
θ
p
×
n
−
sin
θ
sin
θ
(
n
×
p
)
×
n
]
\begin{aligned} q_upq_u^{-1} =& [\cos\theta,\sin\theta\textbf{n}][0,\textbf{p}][\cos\theta,-\sin\theta\textbf{n}]\\ =& [-\sin\theta\textbf{n}\cdot\textbf{p},\cos\theta\textbf{p} + \sin\theta\textbf{n}\times\textbf{p}][\cos\theta,-\sin\theta\textbf{n}]\\ =& [-\sin\theta\cos\theta\textbf{n}\cdot\textbf{p}+\sin\theta\cos\theta\textbf{n}\cdot\textbf{p}+\sin\theta\sin\theta(\textbf{n}\times\textbf{p})\cdot\textbf{n},\\ & \sin\theta\sin\theta(\textbf{n}\cdot\textbf{p})\textbf{n}+\cos\theta\cos\theta\textbf{p}+\sin\theta\cos\theta\textbf{n}\times\textbf{p}\\ &-\sin\theta\cos\theta\textbf{p}\times\textbf{n} - \sin\theta\sin\theta(\textbf{n}\times\textbf{p})\times\textbf{n}] \end{aligned}
qupqu−1===[cosθ,sinθn][0,p][cosθ,−sinθn][−sinθn⋅p,cosθp+sinθn×p][cosθ,−sinθn][−sinθcosθn⋅p+sinθcosθn⋅p+sinθsinθ(n×p)⋅n,sinθsinθ(n⋅p)n+cosθcosθp+sinθcosθn×p−sinθcosθp×n−sinθsinθ(n×p)×n]
这里注意:
(
n
×
p
)
⋅
n
=
0
(\textbf{n}\times\textbf{p})\cdot\textbf{n} = 0
(n×p)⋅n=0
上面的结果还涉及到向量三重积 :
(
n
×
p
)
×
n
=
n
×
(
p
×
n
)
−
p
×
(
n
×
n
)
=
n
×
(
p
×
n
)
n
×
(
p
×
n
)
=
p
(
n
⋅
n
)
−
n
(
n
⋅
p
)
(\textbf{n}\times\textbf{p})\times\textbf{n} = \textbf{n}\times(\textbf{p}\times\textbf{n})-\textbf{p}\times(\textbf{n}\times\textbf{n}) =\textbf{n}\times(\textbf{p}\times\textbf{n})\\ \\ \textbf{n}\times(\textbf{p}\times\textbf{n})=\textbf{p}(\textbf{n}\cdot\textbf{n})-\textbf{n}(\textbf{n}\cdot\textbf{p})
(n×p)×n=n×(p×n)−p×(n×n)=n×(p×n)n×(p×n)=p(n⋅n)−n(n⋅p)
所以,继续化简:
q
u
p
q
u
−
1
=
[
0
,
2
sin
θ
sin
θ
(
n
⋅
p
)
n
+
cos
θ
cos
θ
p
+
2
sin
θ
cos
θ
n
×
p
−
sin
θ
sin
θ
p
(
n
⋅
n
)
]
=
[
0
,
2
sin
2
θ
(
n
⋅
p
)
n
+
cos
2
θ
p
−
sin
2
θ
p
+
2
sin
θ
cos
θ
n
×
p
]
=
[
0
,
(
1
−
cos
2
θ
)
(
n
⋅
p
)
n
+
cos
2
θ
p
+
sin
2
θ
n
×
p
]
=
[
0
,
(
n
⋅
p
)
n
+
cos
2
θ
(
p
−
(
n
⋅
p
)
n
)
+
sin
2
θ
n
×
p
]
\begin{aligned} q_upq_u^{-1} =& [0, 2\sin\theta\sin\theta(\textbf{n}\cdot\textbf{p})\textbf{n}+\cos\theta\cos\theta\textbf{p}+2\sin\theta\cos\theta\textbf{n}\times\textbf{p}\\ &- \sin\theta\sin\theta\textbf{p}(\textbf{n}\cdot\textbf{n})]\\ =& [0, 2\sin^2\theta(\textbf{n}\cdot\textbf{p})\textbf{n}\\ &+\cos^2\theta\textbf{p}- \sin^2\theta\textbf{p}\\ &+2\sin\theta\cos\theta\textbf{n}\times\textbf{p}]\\ =&[0,(1-\cos2\theta)(\textbf{n}\cdot\textbf{p})\textbf{n} + \cos2\theta\textbf{p}+\sin2\theta\textbf{n}\times\textbf{p}]\\ =&[0,(\textbf{n}\cdot\textbf{p})\textbf{n} +\cos2\theta(\textbf{p}-(\textbf{n}\cdot\textbf{p})\textbf{n})+\sin2\theta\textbf{n}\times\textbf{p}] \end{aligned}
qupqu−1====[0,2sinθsinθ(n⋅p)n+cosθcosθp+2sinθcosθn×p−sinθsinθp(n⋅n)][0,2sin2θ(n⋅p)n+cos2θp−sin2θp+2sinθcosθn×p][0,(1−cos2θ)(n⋅p)n+cos2θp+sin2θn×p][0,(n⋅p)n+cos2θ(p−(n⋅p)n)+sin2θn×p]
我们令
α
=
2
θ
\alpha = 2\theta
α=2θ。
q
u
p
q
u
−
1
=
[
0
,
(
n
⋅
p
)
n
+
cos
α
(
p
−
(
n
⋅
p
)
n
)
+
sin
α
n
×
p
]
q_upq_u^{-1}=[0,(\textbf{n}\cdot\textbf{p})\textbf{n} +\cos\alpha(\textbf{p}-(\textbf{n}\cdot\textbf{p})\textbf{n})+\sin\alpha\textbf{n}\times\textbf{p}]
qupqu−1=[0,(n⋅p)n+cosα(p−(n⋅p)n)+sinαn×p]
好家伙,这tm又是啥啊。没办法我们硬着头皮画图一波。
图中:
A
B
=
(
n
⋅
p
)
n
B
D
=
n
×
p
B
C
=
p
−
(
n
⋅
p
)
n
B
E
=
cos
α
(
p
−
(
n
⋅
p
)
n
)
+
sin
α
n
×
p
A
E
=
A
B
+
B
E
=
(
n
⋅
p
)
n
+
cos
α
(
p
−
(
n
⋅
p
)
n
)
+
sin
α
n
×
p
\begin{aligned} AB=&(\textbf{n}\cdot\textbf{p})\textbf{n}\\ BD=&\textbf{n}\times\textbf{p}\\ BC=&\textbf{p}-(\textbf{n}\cdot\textbf{p})\textbf{n}\\ BE =&\cos\alpha(\textbf{p}-(\textbf{n}\cdot\textbf{p})\textbf{n})+\sin\alpha\textbf{n}\times\textbf{p}\\ AE=&AB+BE\\ =&(\textbf{n}\cdot\textbf{p})\textbf{n} +\cos\alpha(\textbf{p}-(\textbf{n}\cdot\textbf{p})\textbf{n})+\sin\alpha\textbf{n}\times\textbf{p} \end{aligned}
AB=BD=BC=BE=AE==(n⋅p)nn×pp−(n⋅p)ncosα(p−(n⋅p)n)+sinαn×pAB+BE(n⋅p)n+cosα(p−(n⋅p)n)+sinαn×p
芜湖!单位四元数
q
u
=
[
cos
θ
,
sin
θ
n
]
q_u=[\cos\theta,\sin\theta\textbf{n}]
qu=[cosθ,sinθn] 通过运算
q
u
p
q
u
−
1
q_upq_u^{-1}
qupqu−1 让 向量
p
p
p ,在左手坐标系下绕
n
\textbf{n}
n轴顺时针旋转了
2
θ
2\theta
2θ !!!(如果是右手坐标系,应解释为逆时针旋转
2
θ
2\theta
2θ )
为了让旋转角刚好为 θ \theta θ ,我们的单位四元数应该是 q u = [ cos θ 2 , sin θ 2 n ] q_u=[\cos\frac{\theta}{2},\sin\frac{\theta}{2}\textbf{n}] qu=[cos2θ,sin2θn] 。
到此为止我们弄清了四元数让向量旋转的运算原理。
2.3.5 我们跳过的东西
上述内容,基本处于从结论反推的状态。我们忽略了虚数在旋转中发挥的作用等更加底层的原理 。如果你想知道这些原理,你可以在《Quaternions for Computer Graphics》一书中找到比本文更加详细的内容(凭大家强大的能力一定能找到它的pdf)。
此外,我们这里对以上的内容进行一个补充:
我们定义一个单位四元数 q u ( x u , y u , z u , w u ) q_{u}(x_u,y_u,z_u,w_u) qu(xu,yu,zu,wu) 可记为 [ cos θ 2 , sin θ 2 n ] [\cos\frac{\theta}{2},\sin\frac{\theta}{2}\textbf{n}] [cos2θ,sin2θn]。设有一个向量 p ( x a , y a , z a ) p(x_a,y_a,z_a) p(xa,ya,za),我们将其看作一个纯四元数 p ( x a , y a , z a , 0 ) p(x_a,y_a,z_a,0) p(xa,ya,za,0) 。那么:
- q u − 1 p q u q_u^{-1}pq_u qu−1pqu :绕 n \textbf{n} n轴旋转 − θ -\theta −θ。
当 n ⊥ p \textbf{n}\perp \textbf{p} n⊥p 时,则有:
- p q u pq_u pqu :绕 n \textbf{n} n轴旋转 − θ 2 -\frac{\theta}{2} −2θ。
- q u − 1 p q_u^{-1}p qu−1p :绕 n \textbf{n} n轴旋转 − θ 2 -\frac{\theta}{2} −2θ。
- p q u − 1 pq_u^{-1} pqu−1:绕 n \textbf{n} n轴旋转 θ 2 \frac{\theta}{2} 2θ。
所以,从上面来看 q u p q u − 1 q_upq_u^{-1} qupqu−1 可拆为两个部分 q u p q_up qup 和 p q u − 1 pq_u^{-1} pqu−1 ,分别承当旋转 θ 2 \frac{\theta}{2} 2θ的任务,合起来一共旋转 θ \theta θ。
除上面之外,我们还应该关注一下
−
q
u
-q_u
−qu 对旋转影响:
−
q
u
=
[
−
cos
θ
2
,
−
sin
θ
2
n
]
=
[
cos
(
θ
2
+
π
)
,
sin
(
θ
2
+
π
)
n
]
=
[
cos
(
θ
+
2
π
2
)
,
sin
(
θ
+
2
π
2
)
n
]
\begin{aligned} -q_u =& [-\cos\frac{\theta}{2},-\sin\frac{\theta}{2}\textbf{n}]\\ =&[\cos(\frac{\theta}{2}+\pi),\sin(\frac{\theta}{2}+\pi)\textbf{n}]\\ =&[\cos(\frac{\theta+2\pi}{2}),\sin(\frac{\theta+2\pi}{2})\textbf{n}]\\ \end{aligned}
−qu===[−cos2θ,−sin2θn][cos(2θ+π),sin(2θ+π)n][cos(2θ+2π),sin(2θ+2π)n]
可以看出
−
q
u
-q_u
−qu 绕轴旋转了
θ
+
2
π
\theta+2\pi
θ+2π ,仅仅从结果上看,
−
q
u
-q_u
−qu 和
q
u
q_u
qu 的旋转效果应该是一样的。
3 啊转转转
3.1 旋转矩阵的具体写法
在1.3.4节中我们对四元数运算的原理进行了推导,在推导过程其实也是旋转矩阵的推导内容的一部分。我们回顾一下条件:我们设有一个向量为 p ( x p , y p , z p ) p(x_p,y_p,z_p) p(xp,yp,zp),并有一个单位四元数 q u ( x u , y u , z u , w u ) q_{u}(x_u,y_u,z_u,w_u) qu(xu,yu,zu,wu) 可记为 [ cos θ 2 , sin θ 2 n ] [\cos\frac{\theta}{2},\sin\frac{\theta}{2}\textbf{n}] [cos2θ,sin2θn] ,对其进行旋转。它的旋转效果等效为旋转矩阵 M M M。
然后,我们直接从以下这个等式开始:
M
p
=
q
u
p
q
u
−
1
=
[
0
,
(
n
⋅
p
)
n
+
cos
θ
(
p
−
(
n
⋅
p
)
n
)
+
sin
θ
n
×
p
]
Mp=q_upq_u^{-1}=[0,(\textbf{n}\cdot\textbf{p})\textbf{n} +\cos\theta(\textbf{p}-(\textbf{n}\cdot\textbf{p})\textbf{n})+\sin\theta\textbf{n}\times\textbf{p}]
Mp=qupqu−1=[0,(n⋅p)n+cosθ(p−(n⋅p)n)+sinθn×p]
我们设旋转轴
n
\textbf{n}
n 为
(
x
n
,
y
n
,
z
n
)
(x_n,y_n,z_n)
(xn,yn,zn) ,对上式各个部分进行矩阵化。
首先第一部分:
(
n
⋅
p
)
n
=
(
x
n
x
p
+
y
n
y
p
+
x
n
x
p
)
n
=
[
x
n
2
x
n
y
n
x
n
z
n
x
n
y
n
y
n
2
y
n
z
n
x
n
z
n
y
n
z
n
z
n
2
]
[
x
p
y
p
z
p
]
\begin{aligned} (\textbf{n}\cdot\textbf{p})\textbf{n} =&(x_nx_p+y_ny_p+x_nx_p)\textbf{n}\\ =&\begin{bmatrix} x_n^2 & x_ny_n & x_nz_n \\ x_ny_n & y_n^2 & y_nz_n \\ x_nz_n & y_nz_n & z_n^2\end{bmatrix} \begin{bmatrix}x_p \\ y_p \\z_p\end{bmatrix} \end{aligned}
(n⋅p)n==(xnxp+ynyp+xnxp)n⎣⎡xn2xnynxnznxnynyn2ynznxnznynznzn2⎦⎤⎣⎡xpypzp⎦⎤
第二部分:
cos
θ
(
p
−
(
n
⋅
p
)
n
)
=
cos
θ
[
1
−
x
n
2
−
x
n
y
n
−
x
n
z
n
−
x
n
y
n
1
−
y
n
2
−
y
n
z
n
−
x
n
z
n
−
y
n
z
n
1
−
z
n
2
]
[
x
p
y
p
z
p
]
\begin{aligned} \cos\theta(\textbf{p}-(\textbf{n}\cdot\textbf{p})\textbf{n}) =& \cos\theta \begin{bmatrix} 1-x_n^2 & -x_ny_n & -x_nz_n \\ -x_ny_n & 1-y_n^2 & -y_nz_n \\ -x_nz_n & -y_nz_n & 1-z_n^2\end{bmatrix} \begin{bmatrix} x_p \\y_p \\ z_p\end{bmatrix} \end{aligned}
cosθ(p−(n⋅p)n)=cosθ⎣⎡1−xn2−xnyn−xnzn−xnyn1−yn2−ynzn−xnzn−ynzn1−zn2⎦⎤⎣⎡xpypzp⎦⎤
最后一部分:
sin
θ
n
×
p
=
sin
θ
(
y
n
z
p
−
z
n
y
p
,
z
n
x
p
−
x
n
z
p
,
x
n
y
p
−
y
n
x
p
)
=
sin
θ
[
0
−
z
n
y
n
z
n
0
−
x
n
−
y
n
x
n
0
]
[
x
p
y
p
z
p
]
\begin{aligned} \sin\theta\textbf{n}\times\textbf{p} =& \sin\theta(y_nz_p-z_ny_p,z_nx_p-x_nz_p,x_ny_p-y_nx_p)\\ =&\sin\theta\begin{bmatrix} 0 & -z_n & y_n \\ z_n & 0 & -x_n \\ -y_n & x_n & 0\end{bmatrix} \begin{bmatrix}x_p \\ y_p \\z_p\end{bmatrix} \end{aligned}
sinθn×p==sinθ(ynzp−znyp,znxp−xnzp,xnyp−ynxp)sinθ⎣⎡0zn−yn−zn0xnyn−xn0⎦⎤⎣⎡xpypzp⎦⎤
把它们合起来:
M
p
=
q
u
p
q
u
−
1
=
[
cos
θ
+
(
1
−
cos
θ
)
x
n
2
(
1
−
cos
θ
)
x
n
y
n
−
z
n
sin
θ
(
1
−
cos
θ
)
x
n
z
n
+
y
n
sin
θ
(
1
−
cos
θ
)
x
n
y
n
+
z
n
sin
θ
cos
θ
+
(
1
−
cos
θ
)
y
n
2
(
1
−
cos
θ
)
y
n
z
n
−
x
n
sin
θ
(
1
−
cos
θ
)
x
n
z
n
−
y
n
sin
θ
(
1
−
cos
θ
)
y
n
z
n
+
x
n
sin
θ
cos
θ
+
(
1
−
cos
θ
)
z
n
2
]
[
x
p
y
p
z
p
]
\begin{aligned} M p =& q_upq_u^{-1}\\=&\begin{bmatrix} \cos\theta+(1-\cos\theta)x_n^2 & (1-\cos\theta)x_ny_n-z_n\sin\theta & (1-\cos\theta)x_nz_n+y_n\sin\theta \\ (1-\cos\theta)x_ny_n+z_n\sin\theta & \cos\theta+(1-\cos\theta)y_n^2 & (1-\cos\theta)y_nz_n-x_n\sin\theta \\ (1-\cos\theta)x_nz_n-y_n\sin\theta & (1-\cos\theta)y_nz_n + x_n\sin\theta& \cos\theta+(1-\cos\theta)z_n^2\end{bmatrix} \begin{bmatrix}x_p \\ y_p \\z_p\end{bmatrix} \end{aligned}
Mp==qupqu−1⎣⎡cosθ+(1−cosθ)xn2(1−cosθ)xnyn+znsinθ(1−cosθ)xnzn−ynsinθ(1−cosθ)xnyn−znsinθcosθ+(1−cosθ)yn2(1−cosθ)ynzn+xnsinθ(1−cosθ)xnzn+ynsinθ(1−cosθ)ynzn−xnsinθcosθ+(1−cosθ)zn2⎦⎤⎣⎡xpypzp⎦⎤
我们获得了旋转矩阵的具体写法,我们只要知道旋转轴和旋转角度就可以构建出对应的旋转矩阵。上面的
θ
\theta
θ 在左手坐标系下顺时针为正方向,在右手坐标系下逆时针为正方向。
如果你想在左手坐标系下,仍然取逆时针为正方向,那么公式中 sin θ \sin\theta sinθ 应该替换为 − sin θ -\sin\theta −sinθ ;如果你想在右手坐标系下,取顺时针为正方向,同理公式中的 sin θ \sin\theta sinθ 应该替换为 − sin θ -\sin\theta −sinθ 。
3.2 四元数转旋转矩阵
我们接着上一节,继续推导。一个单位四元数
q
u
(
x
u
,
y
u
,
z
u
,
w
u
)
q_{u}(x_u,y_u,z_u,w_u)
qu(xu,yu,zu,wu) 可记为
[
cos
θ
2
,
sin
θ
2
n
]
[\cos\frac{\theta}{2},\sin\frac{\theta}{2}\textbf{n}]
[cos2θ,sin2θn] ,且旋转轴
n
\textbf{n}
n 为
(
x
n
,
y
n
,
z
n
)
(x_n,y_n,z_n)
(xn,yn,zn)。那么它们的关系为:
x
u
=
x
n
sin
θ
2
y
u
=
y
n
sin
θ
2
z
u
=
z
n
sin
θ
2
w
u
=
cos
θ
2
x_u = x_n\sin\frac{\theta}{2}\quad\quad y_u = y_n\sin\frac{\theta}{2}\\ z_u = z_n\sin\frac{\theta}{2}\quad\quad w_u = \cos\frac{\theta}{2}
xu=xnsin2θyu=ynsin2θzu=znsin2θwu=cos2θ
我们利用和角公式就有:
cos
θ
=
2
w
u
2
−
1
sin
θ
=
2
w
u
sin
θ
2
1
−
cos
θ
=
2
sin
2
θ
2
\cos\theta = 2w_u^2 - 1\\ \sin\theta = 2w_u\sin\frac{\theta}{2}\\ 1-\cos\theta=2\sin^2\frac{\theta}{2}
cosθ=2wu2−1sinθ=2wusin2θ1−cosθ=2sin22θ
然后我们对旋转矩阵的结果进行一个替换:
M
=
[
2
w
u
2
−
1
+
2
x
n
2
sin
2
θ
2
2
x
n
y
n
sin
2
θ
2
−
2
z
n
w
u
sin
θ
2
2
x
n
z
n
sin
2
θ
2
+
2
y
n
w
u
sin
θ
2
2
x
n
y
n
sin
2
θ
2
+
2
z
n
w
u
sin
θ
2
2
w
u
2
−
1
+
2
y
n
2
sin
2
θ
2
2
y
n
z
n
sin
2
θ
2
−
2
x
n
w
u
sin
θ
2
2
x
n
z
n
sin
2
θ
2
−
2
y
n
w
u
sin
θ
2
2
y
n
z
n
sin
2
θ
2
+
2
x
n
w
u
sin
θ
2
2
w
u
2
−
1
+
2
z
n
2
sin
2
θ
2
]
=
[
2
(
w
u
2
+
x
u
2
)
−
1
2
(
x
u
y
u
−
z
u
w
u
)
2
(
x
u
z
u
+
y
u
w
u
)
2
(
x
u
y
u
+
z
u
w
u
)
2
(
w
u
2
+
y
u
2
)
−
1
2
(
y
u
z
u
−
x
u
w
u
)
2
(
x
u
z
u
−
y
u
w
u
)
2
(
y
u
z
u
+
x
u
w
u
)
2
(
w
u
2
+
z
u
2
)
−
1
]
\begin{aligned} M =&\begin{bmatrix} 2w_u^2 - 1+2x_n^2\sin^2\frac{\theta}{2} & 2x_ny_n\sin^2\frac{\theta}{2}-2z_nw_u\sin\frac{\theta}{2} & 2x_nz_n\sin^2\frac{\theta}{2}+2y_nw_u\sin\frac{\theta}{2} \\ 2x_ny_n\sin^2\frac{\theta}{2}+2z_nw_u\sin\frac{\theta}{2} & 2w_u^2 - 1+2y_n^2\sin^2\frac{\theta}{2} & 2y_nz_n\sin^2\frac{\theta}{2}-2x_nw_u\sin\frac{\theta}{2} \\ 2x_nz_n\sin^2\frac{\theta}{2}-2y_nw_u\sin\frac{\theta}{2} & 2y_nz_n\sin^2\frac{\theta}{2} + 2x_nw_u\sin\frac{\theta}{2}& 2w_u^2 - 1+2z_n^2\sin^2\frac{\theta}{2}\end{bmatrix} \\ \\ =&\begin{bmatrix} 2(w_u^2 + x_u^2 )- 1 & 2(x_uy_u-z_uw_u) & 2(x_uz_u+y_uw_u) \\ 2(x_uy_u+z_uw_u) & 2(w_u^2+y_u^2) - 1 & 2(y_uz_u-x_uw_u) \\ 2(x_uz_u-y_uw_u) & 2(y_uz_u + x_uw_u) & 2(w_u^2+z_u^2) - 1 \end{bmatrix} \end{aligned}
M==⎣⎡2wu2−1+2xn2sin22θ2xnynsin22θ+2znwusin2θ2xnznsin22θ−2ynwusin2θ2xnynsin22θ−2znwusin2θ2wu2−1+2yn2sin22θ2ynznsin22θ+2xnwusin2θ2xnznsin22θ+2ynwusin2θ2ynznsin22θ−2xnwusin2θ2wu2−1+2zn2sin22θ⎦⎤⎣⎡2(wu2+xu2)−12(xuyu+zuwu)2(xuzu−yuwu)2(xuyu−zuwu)2(wu2+yu2)−12(yuzu+xuwu)2(xuzu+yuwu)2(yuzu−xuwu)2(wu2+zu2)−1⎦⎤
由于是单位四元数,旋转矩阵也可以写为:
M
=
[
1
−
2
(
y
u
2
+
z
u
2
)
2
(
x
u
y
u
−
z
u
w
u
)
2
(
x
u
z
u
+
y
u
w
u
)
2
(
x
u
y
u
+
z
u
w
u
)
1
−
2
(
x
u
2
+
z
u
2
)
2
(
y
u
z
u
−
x
u
w
u
)
2
(
x
u
z
u
−
y
u
w
u
)
2
(
y
u
z
u
+
x
u
w
u
)
1
−
2
(
x
u
2
+
y
u
2
)
]
\begin{aligned} M =&\begin{bmatrix} 1-2(y_u^2 + z_u^2 ) & 2(x_uy_u-z_uw_u) & 2(x_uz_u+y_uw_u) \\ 2(x_uy_u+z_uw_u) & 1-2(x_u^2+z_u^2) & 2(y_uz_u-x_uw_u) \\ 2(x_uz_u-y_uw_u) & 2(y_uz_u + x_uw_u) & 1-2(x_u^2+y_u^2) \end{bmatrix} \end{aligned}
M=⎣⎡1−2(yu2+zu2)2(xuyu+zuwu)2(xuzu−yuwu)2(xuyu−zuwu)1−2(xu2+zu2)2(yuzu+xuwu)2(xuzu+yuwu)2(yuzu−xuwu)1−2(xu2+yu2)⎦⎤
通过一些替换,现在旋转矩阵和四元数的关系已经对应了起来。
在左手坐标系下,如果你需要以逆时针为正方向,那么公式中的 x u x_u xu、 y u y_u yu、 z u z_u zu 应该替换为 − x u -x_u −xu、 − y u -y_u −yu、 − z u -z_u −zu 。
在右手坐标系下,如果你需要以顺时针为正方向,那么公式中的 x u x_u xu、 y u y_u yu、 z u z_u zu 也应该替换为 − x u -x_u −xu、 − y u -y_u −yu、 − z u -z_u −zu 。
同时注意:以上的推导在四元数和旋转矩阵均位于同一个左手(或右手)坐标系的情况下进行。
3.2 旋转矩阵转四元数
我看到有大佬写了优化这个算法的文章 :Converting a Rotation Matrix to a Quaternion
我觉很棒,强烈建议看一遍!不过需要注意的文章中的向量是行向量,涉及到的矩阵存在转置。
以下我将依据链接文章,大致翻译一下并修改为列向量的版本:
根据我们的旋转矩阵(第二个版本),我们可以得出以下四种关系式:
1
+
m
00
−
m
11
−
m
22
=
4
x
2
m
10
+
m
01
=
4
x
y
m
20
+
m
02
=
4
x
z
m
21
−
m
12
=
4
x
w
1+m_{00}-m_{11}-m_{22}=4x^2\\ m_{10}+m_{01}=4xy\\ m_{20}+m_{02}=4xz\\ m_{21}-m_{12}=4xw
1+m00−m11−m22=4x2m10+m01=4xym20+m02=4xzm21−m12=4xw
由此关系我们可以解出
x
x
x,就可以根据其他等式解出其他值。
这样做就有一个问题,如果x
是0咋办?如果是0,上面四个等式的值都会是0,造成一个没有意义的结果。即使这些表达式的值不是0,但是很接近0,那么它们可能会受到灾难性抵消(catastrophic cancellation
),从而产生一个不稳定的结果。(catastrophic cancellation
是指当减去两个大小大致相同的测量值时发生的错误尖峰。这种效应通常被称为重要性的损失或减法抵消,不管测量多么精确,这种效应很容易使实验结果变得毫无价值。)
我们可以通过以下观察来解决这个问题。关于单位四元数,我们可以肯定的一件事是,总会存在至少一个不接近0的分量,而且一定会存在一个大小至少为1/2的分量。这些上述用到的表达式都被4x
缩放过了,但是用其他类似的处理方式(指利用矩阵M以类似的方式产生表达式,例如:1 + m00+ m11+ m22= 4w2 ),也会产生缩放因子4y
、4z
和4w
。如果我们准备以这四种缩放因子的其中一种入手,我们总是能够选出一个数据比较稳定的方向。
以前的代码
在网上你可以找到一段相当标准的代码,也许有一些细微的不同,可以将写成如下(已改为列向量):
trace = m00 + m11 + m22;
if (trace > 0.0f)
{
k = 0.5f / sqrt(1.0f + trace);
q = quat( k * (m21 - m12), k * (m02 - m20), k * (m10 - m01), 0.25f / k );
}
else if ((m00 > m11) && (m00 > m22))
{
k = 0.5f / sqrt(1.0f + m00 - m11 - m22);
q = quat( 0.25f / k, k * (m10 + m01), k * (m20 + m02), k * (m21 - m12) );
}
else if (m11 > m22)
{
k = 0.5f / sqrt(1.0f + m11 - m00 - m22);
q = quat( k * (m10 + m01), 0.25f / k, k * (m21 + m12), k * (m02 - m20) );
}
else
{
k = 0.5f / sqrt(1.0f + m22 - m00 - m11);
q = quat( k * (m20 + m02), k * (m21 + m12), 0.25f / k, k * (m10 - m01) );
}
这里有不太理想的几点:
- 它最开始计算了m00+m11+m22的和,但在四种情况中的三种里面都没有用到。
- 对于四个分量有着不对称的处理方式:第一种方式是在 ∣ w ∣ > 1 / 2 |w|>1/2 ∣w∣>1/2的 情况下进行,但如果不是,它将选取 ∣ x ∣ |x| ∣x∣, ∣ y ∣ |y| ∣y∣, ∣ z ∣ |z| ∣z∣中最大值的那条分支。
- 总是对一个分量执行除法。
新的处理方式
我们来看看如何处理这些不如意的地方。首先,我们要问:什么时候每个对角元素都是负的?
因为q
是一个单位四元数,有
x
2
+
y
2
+
z
2
+
w
2
=
1
x^2+y^2+z^2+w^2=1
x2+y2+z2+w2=1的性质。所以,举个栗子,当x
和y
一起贡献了超过四元数一半的模长时,m22就是一个负数,这说明x
或y
足够大,可以避免数值上的不稳定。我们只需要测试哪个可以用(也有可能两个都可以,但我们需要一个保证)。
现在我们考虑对角元素的两两之差:
以及两两之和:
因此,对于任意两个分量的选择,我们可以将一个对角元素与另外一个对角元素(或另外一个的负数)进行比较,来选择分量更大的那一个。例如:
修改框架
总之,通过以上这些观察结果,这里建议使用分而治之的策略来选择四个代码路径中的一条。此外,我们可以只通过比较对角元素来进行选择。这里是其中一种实现方式:
if (m22 < 0) // is |(x,y)| bigger than |(z,w)|?
{
if (m00 > m11) // is |x| bigger than |y|?
// use x-form
else
// use y-form
}
else
{
if (m00 < -m11) // is |z| bigger than |w|?
// use z-form
else
// use w-form
}
这个框架解决了代码中缺少对称性的问题。而且,由于我们不再需要预先计算矩阵的迹,所以我们也不会丢弃已经计算过的值——迹只能被四条分支的其中一个中得到计算(w-form
分支)。在其他三个分支中,将计算它们自己需要的表达式。
最后,关于除法问题可按如下方式解决。原本的代码会在每条分支中进行计算:
k = 0.5f / sqrt(t);
t
是一些项的和,然后让四元数的一个分量等于
0.25
/
k
0.25/k
0.25/k。(这是我们关注的第二个除法;第一个是平方根的倒数运算,通常不使用除法直接执行。)但是请注意:
因此,除法可被代替为乘法。编译器可能会执行这种优化,但我们必须自己确保它执行。
船新版本(已改为列向量)
if (m22 < 0)
{
if (m00 > m11)
{
t = 1 + m00 - m11 - m22;
q = quat( t, m01+m10, m20+m02, m21-m12 );
}
else
{
t = 1 - m00 + m11 - m22;
q = quat( m01+m10, t, m12+m21, m02-m20 );
}
}
else
{
if (m00 < -m11)
{
t = 1 - m00 - m11 + m22;
q = quat( m20+m02, m12+m21, t, m10-m01 );
}
else
{
t = 1 + m00 + m11 + m22;
q = quat( m21-m12, m02-m20, m10-m01, t );
}
}
q *= 0.5 / Sqrt(t);
3.3 欧拉角转四元数
由于欧拉角带有旋转先后顺序,我们这里随便来个旋转顺序YXZ
,并仍在在左手坐标系下推导。
我们设有一个欧拉角为 ( α , β , γ ) (\alpha,\beta,\gamma) (α,β,γ) ,那么我们就可以构造三个分别绕轴旋转的单位四元数 q x = [ cos α 2 , sin α 2 i ] q_x=[\cos\frac{\alpha}{2},\sin\frac{\alpha}{2}\textbf{i}] qx=[cos2α,sin2αi]、 q y = [ cos β 2 , sin β 2 j ] q_y=[\cos\frac{\beta}{2},\sin\frac{\beta}{2}\textbf{j}] qy=[cos2β,sin2βj] 和 q z = [ cos γ 2 , sin γ 2 k ] q_z=[\cos\frac{\gamma}{2},\sin\frac{\gamma}{2}\textbf{k}] qz=[cos2γ,sin2γk] 。
那么对应的四元数为:
q
y
x
z
=
q
z
q
x
q
y
=
[
cos
γ
2
,
sin
γ
2
k
]
[
cos
α
2
,
sin
α
2
i
]
[
cos
β
2
,
sin
β
2
j
]
=
[
cos
γ
2
cos
α
2
,
cos
γ
2
sin
α
2
i
+
cos
α
2
sin
γ
2
k
+
sin
γ
2
sin
α
2
j
]
[
cos
β
2
,
sin
β
2
j
]
=
[
cos
γ
2
cos
α
2
cos
β
2
−
sin
γ
2
sin
α
2
sin
β
2
,
(
cos
γ
2
sin
α
2
cos
β
2
−
sin
γ
2
cos
α
2
sin
β
2
)
i
(
cos
γ
2
cos
α
2
sin
β
2
+
sin
γ
2
sin
α
2
cos
β
2
)
j
(
sin
γ
2
cos
α
2
cos
β
2
+
cos
γ
2
sin
α
2
sin
β
2
)
k
]
\begin{aligned} q_{yxz}=&q_zq_xq_y\\ =&[\cos\frac{\gamma}{2},\sin\frac{\gamma}{2}\textbf{k}][\cos\frac{\alpha}{2},\sin\frac{\alpha}{2}\textbf{i}][\cos\frac{\beta}{2},\sin\frac{\beta}{2}\textbf{j}]\\ =&[\cos\frac{\gamma}{2}\cos\frac{\alpha}{2},\cos\frac{\gamma}{2}\sin\frac{\alpha}{2}\textbf{i} + \cos\frac{\alpha}{2}\sin\frac{\gamma}{2}\textbf{k}+\sin\frac{\gamma}{2}\sin\frac{\alpha}{2}\textbf{j}][\cos\frac{\beta}{2},\sin\frac{\beta}{2}\textbf{j}]\\ =&[\cos\frac{\gamma}{2}\cos\frac{\alpha}{2}\cos\frac{\beta}{2} -\sin\frac{\gamma}{2}\sin\frac{\alpha}{2}\sin\frac{\beta}{2},\\ &(\cos\frac{\gamma}{2}\sin\frac{\alpha}{2}\cos\frac{\beta}{2}-\sin\frac{\gamma}{2}\cos\frac{\alpha}{2}\sin\frac{\beta}{2})\textbf{i}\\ &(\cos\frac{\gamma}{2}\cos\frac{\alpha}{2}\sin\frac{\beta}{2}+\sin\frac{\gamma}{2}\sin\frac{\alpha}{2}\cos\frac{\beta}{2})\textbf{j}\\ &(\sin\frac{\gamma}{2}\cos\frac{\alpha}{2}\cos\frac{\beta}{2}+\cos\frac{\gamma}{2}\sin\frac{\alpha}{2}\sin\frac{\beta}{2})\textbf{k}]\\ \end{aligned}
qyxz====qzqxqy[cos2γ,sin2γk][cos2α,sin2αi][cos2β,sin2βj][cos2γcos2α,cos2γsin2αi+cos2αsin2γk+sin2γsin2αj][cos2β,sin2βj][cos2γcos2αcos2β−sin2γsin2αsin2β,(cos2γsin2αcos2β−sin2γcos2αsin2β)i(cos2γcos2αsin2β+sin2γsin2αcos2β)j(sin2γcos2αcos2β+cos2γsin2αsin2β)k]
换种写法:
q
y
x
z
=
[
cos
γ
2
sin
α
2
cos
β
2
−
sin
γ
2
cos
α
2
sin
β
2
cos
γ
2
cos
α
2
sin
β
2
+
sin
γ
2
sin
α
2
cos
β
2
sin
γ
2
cos
α
2
cos
β
2
+
cos
γ
2
sin
α
2
sin
β
2
cos
γ
2
cos
α
2
cos
β
2
−
sin
γ
2
sin
α
2
sin
β
2
]
q_{yxz}=\begin{bmatrix} \cos\frac{\gamma}{2}\sin\frac{\alpha}{2}\cos\frac{\beta}{2}-\sin\frac{\gamma}{2}\cos\frac{\alpha}{2}\sin\frac{\beta}{2}\\ \cos\frac{\gamma}{2}\cos\frac{\alpha}{2}\sin\frac{\beta}{2}+\sin\frac{\gamma}{2}\sin\frac{\alpha}{2}\cos\frac{\beta}{2}\\ \sin\frac{\gamma}{2}\cos\frac{\alpha}{2}\cos\frac{\beta}{2}+\cos\frac{\gamma}{2}\sin\frac{\alpha}{2}\sin\frac{\beta}{2}\\ \cos\frac{\gamma}{2}\cos\frac{\alpha}{2}\cos\frac{\beta}{2} -\sin\frac{\gamma}{2}\sin\frac{\alpha}{2}\sin\frac{\beta}{2} \end{bmatrix}
qyxz=⎣⎢⎢⎡cos2γsin2αcos2β−sin2γcos2αsin2βcos2γcos2αsin2β+sin2γsin2αcos2βsin2γcos2αcos2β+cos2γsin2αsin2βcos2γcos2αcos2β−sin2γsin2αsin2β⎦⎥⎥⎤
3.4 四元数转欧拉角
累了累了,这玩意换算算着有点难写啊。
我们直接来看看别人咋弄的吧~ 下面的文章里面各种旋转顺序的欧拉角都有的哦~
不过需要注意人家是在右手坐标系下推导,且实部是放在四元数的第一位的哦~
Euler angles, quaternions, and transformation matrices for space shuttle analysis
(水平有限,有缘再补充吧~)
参考文档:
[1] Quaternions for Computer Graphics
[2] Converting a Rotation Matrix to a Quaternion
[3] Euler angles, quaternions, and transformation matrices for space shuttle analysis