引言
线性变换:保持向量加法和缩放操作:
f
(
x
)
+
f
(
y
)
=
f
(
x
+
y
)
,
k
f
(
x
)
=
f
(
k
x
)
\mathbf f(\mathbf x)+\mathbf f(\mathbf y)=\mathbf{f}(\mathbf x+\mathbf y),\\ k\mathbf f(\mathbf x)=\mathbf f(k\mathbf x)
f(x)+f(y)=f(x+y),kf(x)=f(kx)
旋转和缩放变换都是对三维向量的线性变换,可以表示为 3 × 3 3\times3 3×3矩阵。
平移变换是对一个向量执行加上另一个向量的操作; 3 × 3 3\times3 3×3矩阵无法满足移动变换的表示。
仿射变换(affine transform)通过 4 × 4 4\times4 4×4矩阵将平移变换与旋转、缩放变换结合起来。使用齐次(homogeneous)标记来表示思维向量:
- 方向向量: v = ( v x v y v z 0 ) T \mathbf v=(v_x\quad v_y\quad v_z\quad 0)^T v=(vxvyvz0)T;
- 点: v = ( v x v y v z 1 ) T \mathbf v=(v_x\quad v_y\quad v_z\quad 1)^T v=(vxvyvz1)T;
平移、旋转、缩放、反射、切变矩阵都是放射矩阵。仿射矩阵点特点就是保持线的平行性,但不保持长度和角度。仿射变换也可以表示为任意单独的仿射变换的串联。
一、基础变换
下表是大部分基础变换的汇总:
标记 名称 特点 T ( t ) 平 移 矩 阵 移 动 一 个 点 , 仿 射 R x ( ρ ) 旋 转 矩 阵 围 绕 x 轴 旋 转 ρ 弧 度 。 正 交 、 仿 射 S ( s ) 缩 放 矩 阵 根 据 s 沿 着 个 坐 标 轴 缩 放 。 仿 射 H i j ( s ) 切 变 矩 阵 将 i 分 量 进 行 为 s 倍 数 的 依 据 j 分 量 的 切 变 操 作 。 仿 射 E ( h , p , r ) 欧 拉 变 换 以 欧 拉 角 形 式 表 示 的 指 向 矩 阵 。 正 交 、 仿 射 P o ( s ) 正 交 投 影 向 平 面 或 空 间 内 进 行 平 行 投 影 。 仿 射 P p ( s ) 透 视 投 影 向 平 面 或 空 间 内 进 行 符 合 透 视 规 则 的 投 影 s l e r p ( q ^ , r ^ , t ) 球 面 线 性 插 值 根 据 四 元 数 q ^ 和 r ^ 以 及 参 数 t 来 创 建 插 值 的 四 元 数 \begin{array}{c|c|c} \textbf{标记} & \textbf{名称} & \textbf{特点} \\ \hline \mathbf T(\mathbf t) & 平移矩阵 & 移动一个点,仿射 \\ \mathbf R_x(\rho) & 旋转矩阵 & 围绕x轴旋转\rho弧度。正交、仿射 \\ \mathbf S(\mathbf s) & 缩放矩阵 & 根据\mathbf s 沿着个坐标轴缩放。仿射 \\ \mathbf H_{ij}(s) & 切变矩阵 & 将i 分量进行为s倍数的依据j分量的切变操作。 仿射 \\ \mathbf E(h,p,r) & 欧拉变换 & 以欧拉角形式表示的指向矩阵。正交、仿射\\ \mathbf P_o(s) & 正交投影 & 向平面或空间内进行平行投影。仿射\\ \mathbf P_p(s) & 透视投影 & 向平面或空间内进行符合透视规则的投影\\ \mathrm{slerp}(\mathbf{\hat q},\mathbf{\hat r},t) & 球面线性插值 & 根据四元数\mathbf{\hat q}和\mathbf{\hat r}以及参数t来创建插值的四元数 \end{array} 标记T(t)Rx(ρ)S(s)Hij(s)E(h,p,r)Po(s)Pp(s)slerp(q^,r^,t)名称平移矩阵旋转矩阵缩放矩阵切变矩阵欧拉变换正交投影透视投影球面线性插值特点移动一个点,仿射围绕x轴旋转ρ弧度。正交、仿射根据s沿着个坐标轴缩放。仿射将i分量进行为s倍数的依据j分量的切变操作。仿射以欧拉角形式表示的指向矩阵。正交、仿射向平面或空间内进行平行投影。仿射向平面或空间内进行符合透视规则的投影根据四元数q^和r^以及参数t来创建插值的四元数
1.1 平移
将一个实体沿着向量
v
=
(
t
x
,
t
y
,
t
z
)
\mathbf v=(t_x,t_y,t_z)
v=(tx,ty,tz)平移的变换矩阵可以表示为:
T
(
t
)
=
T
(
t
x
,
t
y
,
t
z
)
=
(
1
0
0
t
x
0
1
0
t
y
0
0
1
t
z
0
0
0
1
)
\mathbf T(\mathbf t) = \mathbf T(t_x,t_y,t_z) = \begin{pmatrix}1 & 0 & 0 & t_x\\ 0 & 1 & 0 & t_y\\ 0 & 0 & 1 & t_z\\ 0 & 0 & 0 & 1 \end{pmatrix}
T(t)=T(tx,ty,tz)=⎝⎜⎜⎛100001000010txtytz1⎠⎟⎟⎞
需要注意的是,平移变换对向量
v
=
(
v
x
,
v
y
,
v
z
,
0
)
\mathbf v=(v_x,v_y,v_z,0)
v=(vx,vy,vz,0)不生效;也就是说,方向向量是不能被平移的。
平移矩阵的逆矩阵为: T − 1 ( t ) = T ( − t ) \mathbf T^{-1}(\mathbf t)=\mathbf T(-\mathbf t) T−1(t)=T(−t)。
1.2 旋转
旋转变换指的是将一个向量(位置或方向)以给定坐标轴为中心旋转特定角度。和平移变换一样,旋转变换也是刚体变换并且保持手性。
二维情况下的推导:假设一个向量
v
=
(
v
x
,
v
y
)
=
(
r
cos
(
θ
)
,
r
sin
(
θ
)
)
\mathbf v=(v_x, v_y)=(r\cos(\theta), r\sin(\theta))
v=(vx,vy)=(rcos(θ),rsin(θ)),对其逆时针旋转
ϕ
\phi
ϕ弧度,得到
u
=
(
r
cos
(
θ
+
ϕ
)
,
r
sin
(
θ
+
ϕ
)
)
\mathbf u=(r\cos(\theta+\phi),r\sin(\theta+\phi))
u=(rcos(θ+ϕ),rsin(θ+ϕ)):
u
=
(
r
cos
(
θ
+
ϕ
)
r
sin
(
θ
+
ϕ
)
)
=
(
r
(
cos
θ
cos
ϕ
−
sin
θ
sin
ϕ
)
r
(
sin
θ
cos
ϕ
+
cos
θ
sin
ϕ
)
)
=
(
cos
ϕ
−
sin
ϕ
sin
ϕ
cos
ϕ
)
⏟
R
(
ϕ
)
(
r
cos
θ
r
sin
θ
)
⏟
v
=
R
(
ϕ
)
v
\mathbf u=\begin{pmatrix}r\cos(\theta+\phi)\\r\sin(\theta+\phi)\end{pmatrix}=\begin{pmatrix}r(\cos\theta\cos\phi-\sin\theta\sin\phi)\\r(\sin\theta\cos\phi+\cos\theta\sin\phi)\end{pmatrix}\\ =\underbrace{\begin{pmatrix}\cos\phi & -\sin\phi\\\sin\phi & \cos\phi\end{pmatrix}}_{\mathbf R(\phi)}\underbrace{\begin{pmatrix}r\cos\theta\\r\sin\theta\end{pmatrix}}_{\mathbf v}=\mathbf R(\phi)\mathbf v
u=(rcos(θ+ϕ)rsin(θ+ϕ))=(r(cosθcosϕ−sinθsinϕ)r(sinθcosϕ+cosθsinϕ))=R(ϕ)
(cosϕsinϕ−sinϕcosϕ)v
(rcosθrsinθ)=R(ϕ)v
在三维情况下,一般会对围绕
x
,
y
,
z
x,y,z
x,y,z轴的旋转矩阵记为
R
x
(
ϕ
)
,
R
y
(
ϕ
)
,
R
z
(
ϕ
)
\mathbf R_x(\phi),\mathbf R_y(\phi),\mathbf R_z(\phi)
Rx(ϕ),Ry(ϕ),Rz(ϕ):
R
x
(
ϕ
)
=
(
1
0
0
0
0
cos
(
ϕ
)
−
sin
(
ϕ
)
0
0
sin
(
ϕ
)
cos
(
ϕ
)
0
0
0
0
1
)
,
R
y
(
ϕ
)
=
(
cos
(
ϕ
)
0
sin
(
ϕ
)
0
0
1
0
0
−
sin
(
ϕ
)
0
cos
(
ϕ
)
0
0
0
0
1
)
,
R
z
(
ϕ
)
=
(
cos
(
ϕ
)
−
sin
(
ϕ
)
0
0
sin
(
ϕ
)
cos
(
ϕ
)
0
0
0
0
1
0
0
0
0
1
)
\mathbf R_x(\phi)=\begin{pmatrix}1&0&0&0\\0&\cos(\phi)&-\sin(\phi)&0\\0&\sin(\phi)&\cos(\phi)&0\\0&0&0&1\end{pmatrix},\\ \mathbf R_y(\phi)=\begin{pmatrix}\cos(\phi)&0&\sin(\phi)&0\\0&1&0&0\\-\sin(\phi)&0&\cos(\phi)&0\\0&0&0&1\end{pmatrix},\\ \mathbf R_z(\phi)=\begin{pmatrix}\cos(\phi)&-\sin(\phi)&0&0\\\sin(\phi)&\cos(\phi)&0&0\\0&0&1&0\\0&0&0&1\end{pmatrix}
Rx(ϕ)=⎝⎜⎜⎛10000cos(ϕ)sin(ϕ)00−sin(ϕ)cos(ϕ)00001⎠⎟⎟⎞,Ry(ϕ)=⎝⎜⎜⎛cos(ϕ)0−sin(ϕ)00100sin(ϕ)0cos(ϕ)00001⎠⎟⎟⎞,Rz(ϕ)=⎝⎜⎜⎛cos(ϕ)sin(ϕ)00−sin(ϕ)cos(ϕ)0000100001⎠⎟⎟⎞
旋转矩阵的迹为: t r ( R ) = 1 + 2 cos ( ϕ ) \mathrm{tr}(\mathbf R)=1+2\cos(\phi) tr(R)=1+2cos(ϕ);行列式为1,且是正交的。
旋转矩阵的逆矩阵: R i − 1 ( ϕ ) = R i ( − ϕ ) \mathbf R_i^{-1}(\phi)=\mathbf R_i(-\phi) Ri−1(ϕ)=Ri(−ϕ)。
1.3 缩放
缩放矩阵: S ( s ) = S ( s x , s y , s z ) \mathbf S(\mathbf s)=\mathbf S(s_x,s_y,s_z) S(s)=S(sx,sy,sz),表示实体在 x , y , z x,y,z x,y,z轴方向上的缩放因子为 s x , s y , s z s_x,s_y,s_z sx,sy,sz。
S ( s ) = ( s x 0 0 0 0 s y 0 0 0 0 s z 0 0 0 0 1 ) \mathbf S(\mathbf s)=\begin{pmatrix}s_x&0&0&0\\0&s_y&0&0\\0&0&s_z&0\\0&0&0&1\end{pmatrix} S(s)=⎝⎜⎜⎛sx0000sy0000sz00001⎠⎟⎟⎞
如果 s x = s y = s z s_x=s_y=s_z sx=sy=sz,那么这个缩放矩阵是各向同性的(isotropic),否则是各向异性的(anisotropic)。逆矩阵为 S − 1 = S ( 1 / s x , 1 / s y , 1 / s z ) \mathbf S^{-1}=\mathbf S(1/s_x,1/s_y,1/s_z) S−1=S(1/sx,1/sy,1/sz)。
特殊的缩放矩阵:其中一个或三个缩放因子是负数时,称为反射矩阵或镜像矩阵。但如果是两个缩放因子是负数,那么相当于旋转了 π \pi π弧度。反射矩阵会导致顶点顺序发生改变。
判断一些列变换矩阵的组合是否为反射矩阵的方法:计算变换矩阵左上角 3 × 3 3\times3 3×3元素的行列式,如果行列式为负数,那么就为反射矩阵。
1.4 切变
六种基础的切变矩阵:
H
x
y
(
s
)
,
H
x
z
(
s
)
,
H
y
x
(
s
)
,
H
y
z
(
s
)
,
H
z
x
(
s
)
,
H
z
y
(
s
)
\mathbf H_{xy}(s),\mathbf H_{xz}(s),\mathbf H_{yx}(s),\mathbf H_{yz}(s),\mathbf H_{zx}(s),\mathbf H_{zy}(s)
Hxy(s),Hxz(s),Hyx(s),Hyz(s),Hzx(s),Hzy(s),第一个下标表示哪个坐标会因切变矩阵而改变,第二个下标表示哪个坐标执行切变。在切变矩阵中,这两个下标也对应着参数
s
s
s的行和列,如:
H
x
z
(
s
)
=
(
1
0
s
0
0
1
0
0
0
0
1
0
0
0
0
1
)
\mathbf H_{xz}(s)=\begin{pmatrix}1&0&s&0\\0&1&0&0\\0&0&1&0\\0&0&0&1\end{pmatrix}
Hxz(s)=⎝⎜⎜⎛10000100s0100001⎠⎟⎟⎞
逆矩阵为: H i j − 1 = H i j ( − s ) \mathbf H_{ij}^{-1}=\mathbf H_{ij}(-s) Hij−1=Hij(−s)。
另一种切变: H x y ′ ( s , t ) = ( 1 0 s 0 0 1 t 0 0 0 1 0 0 0 0 1 ) \mathbf H_{xy}^{\prime}(s,t)=\begin{pmatrix}1&0&s&0\\0&1&t&0\\0&0&1&0\\0&0&0&1\end{pmatrix} Hxy′(s,t)=⎝⎜⎜⎛10000100st100001⎠⎟⎟⎞,表示下标表示的两个坐标会被第三个坐标改变。可以看作是两个基础切变的组合: H i j ′ ( s , t ) = H i k ( s ) H j k ( t ) \mathbf H_{ij}^{\prime}(s,t)=\mathbf H_{ik}(s)\mathbf H_{jk}(t) Hij′(s,t)=Hik(s)Hjk(t)。
切变矩阵的行列式为1,所以是体积不变的变换。
1.5 变换的串联
变换矩阵串联是需要考虑顺序的。串联的目的是为了提升性能。
变换矩阵串联满足结合律: A B C = ( A B ) C \mathbf A\mathbf B\mathbf C=(\mathbf A\mathbf B)\mathbf C ABC=(AB)C。
1.6 刚体变换
刚体变换表示保持物体长度、角度、手性不变的变换,即仅有平移和旋转变换进行串联。可以表示为:
X
=
T
(
t
)
R
=
(
r
00
r
01
r
02
t
x
r
10
r
11
r
12
t
y
r
20
r
21
r
22
t
z
0
0
0
1
)
\mathbf X=\mathbf T(\mathbf t)\mathbf R=\begin{pmatrix}r_{00}&r_{01}&r_{02}&t_x\\r_{10}&r_{11}&r_{12}&t_y\\r_{20}&r_{21}&r_{22}&t_z\\0&0&0&1\end{pmatrix}
X=T(t)R=⎝⎜⎜⎛r00r10r200r01r11r210r02r12r220txtytz1⎠⎟⎟⎞
其逆变换为
X
−
1
=
(
T
(
t
)
R
)
−
1
=
R
T
T
(
−
t
)
\mathbf X^{-1}=(\mathbf T(\mathbf t)\mathbf R)^{-1}=\mathbf R^T\mathbf T(-\mathbf t)
X−1=(T(t)R)−1=RTT(−t),即左上角
3
×
3
3\times3
3×3的旋转矩阵转置,平移矩阵的元素值改变符号,并依照逆序相乘。
另一种计算方式:假设
R
\mathbf R
R为
3
×
3
3\times3
3×3的旋转矩阵,
X
\mathbf X
X表示刚体变换矩阵:
R
ˉ
=
(
r
,
0
r
,
1
r
,
2
)
=
(
r
0
,
T
r
1
,
T
r
2
,
T
)
,
X
=
(
R
ˉ
t
0
T
1
)
\bar{\mathbf R}=\begin{pmatrix}\mathbf r_{,0}&\mathbf r_{,1}&\mathbf r_{,2}\end{pmatrix}=\begin{pmatrix}\mathbf r_{0,}^T\\\\\mathbf r_{1,}^T\\\\\mathbf r_{2,}^T\end{pmatrix},\\ \mathbf X=\begin{pmatrix}\bar{\mathbf R}&\mathbf t\\\mathbf 0^T&1\end{pmatrix}
Rˉ=(r,0r,1r,2)=⎝⎜⎜⎜⎜⎛r0,Tr1,Tr2,T⎠⎟⎟⎟⎟⎞,X=(Rˉ0Tt1)
那么逆变换可以表示为:
X
−
1
=
(
r
0
,
r
1
,
r
2
,
−
R
ˉ
T
t
0
0
0
1
)
\mathbf X^{-1}=\begin{pmatrix}\mathbf r_{0,}&\mathbf r_{1,}&\mathbf r_{2,}&-\bar{\mathbf R}^T\mathbf t\\0&0&0&1\end{pmatrix}
X−1=(r0,0r1,0r2,0−RˉTt1)
1.7 法线变换
基础变换可以应用于线和面的切线向量,但是有些情况下不能直接应用于法线向量。
正确的应用方式应为使用变换矩阵的伴随矩阵的转置。在变化之后可能需要归一化。
简化:
- 使用逆矩阵代替伴随矩阵;因为伴随矩阵是逆矩阵除以行列式计算得来;
- 法线是一个向量,所以平移不会对其产生影响;大部分变换是仿射的,即无投影;所以可以仅计算左上 3 × 3 3\times3 3×3的部分;
- 如果变换完全是由平移、旋转和各向同性的缩放变换串联的;因为平移不影响法线,各向同性的缩放只影响法线长度,旋转矩阵的逆矩阵为其转置;这种情况下,原变换矩阵可以直接应用于法线;
- 如果只有平移和旋转,那么不需要重新归一化;如果有各向同性的缩放,那么归一化只需要除以缩放比例即可,或变换矩阵的左上 3 × 3 3\times3 3×3部分除以缩放比例即可。
1.8 逆变换的计算
三种计算方式:
- 如果矩阵是单一变换或是一些列已知参数的简单变换,那么可以通过各个矩阵的逆矩阵逆序得到。简单且保证精度;
- 如果已知变换矩阵是正交的,那么转置即为逆矩阵: M − 1 = M T \mathbf M^{-1}=\mathbf M^T M−1=MT;
- 如果以上都未知,那么可以使用伴随矩阵方法、克莱尔法则(Cramer’s rule)、LU分解、高斯消元法等方法来计算逆矩阵;其中高斯消元法和伴随矩阵方法更好一些,可以使用更少的if判断;
二、特殊的矩阵变换和操作
2.1 欧拉变换
欧拉变换是一种直观的描述实体方向的变换。默认以负
z
z
z轴作为视角方向,
y
y
y轴为上方向。
记为: E ( h , p , r ) = R z ( r ) R x ( p ) R y ( h ) \mathbf E(h,p,r)=\mathbf R_z(r)\mathbf R_x(p)\mathbf R_y(h) E(h,p,r)=Rz(r)Rx(p)Ry(h)。可以有24种不同顺序,这里是常用形式。
逆变换: E − 1 = E T = ( R z R x R y ) T = R y T R x T R z T \mathbf E^{-1}=\mathbf E^T=(\mathbf R_z\mathbf R_x\mathbf R_y)^T=\mathbf R_y^T\mathbf R_x^T\mathbf R_z^T E−1=ET=(RzRxRy)T=RyTRxTRzT。
欧拉角 h , p , r h,p,r h,p,r可以直观的表示为“摇头/偏航(飞行术语)”、“俯仰”、“翻滚”。
欧拉角的缺点:
- 视场的上方向并不能特指世界坐标的上方向;
- 两组欧拉角很难合并;如做两组欧拉角的过度动画;
- 多组欧拉角可以表示同一个方向;
- 可能会产生gimbal lock;
2.2 从欧拉变换中提取参数
该过程从正交矩阵中计算欧拉参数,即 h , p , r h,p,r h,p,r。因为都是旋转矩阵,所以可以用变换矩阵的左上角 3 × 3 3\times3 3×3来表示。
E ( h , p , r ) = ( e 00 e 01 e 02 e 10 e 11 e 12 e 20 e 21 e 22 ) = R z ( r ) R x ( p ) R y ( h ) \mathbf E(h,p,r)=\begin{pmatrix}e_{00}&e_{01}&e_{02}\\e_{10}&e_{11}&e_{12}\\e_{20}&e_{21}&e_{22}\end{pmatrix}=\mathbf R_z(r)\mathbf R_x(p)\mathbf R_y(h) E(h,p,r)=⎝⎛e00e10e20e01e11e21e02e12e22⎠⎞=Rz(r)Rx(p)Ry(h)
可以得到
E
=
(
cos
r
cosh
−
sin
r
sin
p
sin
h
−
sin
r
cos
p
cos
r
sin
h
+
sin
r
sin
p
cos
h
sin
r
cos
h
+
cos
r
sin
p
sin
h
cos
r
cos
p
sin
r
sin
h
−
cos
r
sin
p
cos
h
−
cos
p
sin
h
sin
p
cos
p
cos
h
)
\mathbf E=\begin{pmatrix}\cos r\cosh-\sin r\sin p\sin h & -\sin\ r\cos\ p & \cos r\sin h+\sin r\sin p\cos h\\ \sin r\cos h+\cos r\sin p\sin h & \cos r\cos p & \sin r\sin h-\cos r\sin p\cos h\\ -\cos p\sin h & \sin p & \cos p\cos h\end{pmatrix}
E=⎝⎛cosrcosh−sinrsinpsinhsinrcosh+cosrsinpsinh−cospsinh−sin rcos pcosrcospsinpcosrsinh+sinrsinpcoshsinrsinh−cosrsinpcoshcospcosh⎠⎞
从式中可以得到
e
01
e
11
=
−
sin
r
cos
r
=
−
tan
r
and
e
20
e
22
=
−
sin
h
cos
h
=
−
tan
h
\cfrac{ e_{01}}{ e_{11}}=\cfrac{-\sin r}{\cos r}=-\tan r \quad \textrm{and} \quad \cfrac{e_{20}}{e_{22}}=\cfrac{-\sin h}{\cos h}=-\tan h
e11e01=cosr−sinr=−tanrande22e20=cosh−sinh=−tanh。可得
h
=
a
t
a
n
2
(
−
e
20
,
e
22
)
,
p
=
arcsin
(
e
21
)
,
r
=
a
t
a
n
2
(
−
e
01
,
e
11
)
\begin{array}{lcl} h = \mathrm{atan2}(-e_{20},e_{22}),\\ p = \arcsin(e_{21}),\\ r = \mathrm{atan2}(-e_{01},e_{11}) \end{array}
h=atan2(−e20,e22),p=arcsin(e21),r=atan2(−e01,e11)
这里有一个特殊情况,当 cos p = 0 \cos p=0 cosp=0时,会出现gimbal lock,即 r r r和 h h h会围绕同一个轴旋转(可能是不同方向,取决于 p p p为 − π / 2 -\pi/2 −π/2或 π / 2 \pi/2 π/2)。那么会出现数值不稳定性。
产生gimbal lock的原因是丢失了一个自由度。从数字层面来看,当 cos p = 0 \cos p=0 cosp=0,即 p = ± π / 2 + 2 π k , k is an integer p=\pm\pi/2+2\pi k,\ k\ \textrm{is an integer} p=±π/2+2πk, k is an integer,会丢失一个自由度,矩阵 E \mathbf E E仅依赖与一个角度, r + h r+h r+h或 r − h r-h r−h。
2.3 矩阵分解
矩阵分解的任务是从一个串联的矩阵中找回一些列变换。
矩阵分解的用途:
- 提取物体的缩放比例;
- 找到特定系统所需要的变换;
- 确定模型是否仅有刚体变换;
- 动画的关键帧插值;
- 从旋转矩阵中移除切变;
简单的分解: 4 × 4 4\times4 4×4矩阵最右一列为平移矩阵;可以通过行列式正负性确定是否有反射;
2.4 围绕任意轴旋转
基本方法
假设围绕 r \mathbf r r轴旋转 α \alpha α弧度。首先变换矩阵 M \mathbf M M变换到可以围绕 x x x轴旋转的空间,然后执行旋转操作,最后再通过 M − 1 \mathbf M^{-1} M−1变换回原空间。
为了计算
M
\mathbf M
M,首先需要找到另外两个与
r
\mathbf r
r正交的轴,且这两轴互相正交。假设两轴为
s
,
t
\mathbf s, \mathbf t
s,t,则
t
=
r
×
s
\mathbf t=\mathbf r\times\mathbf s
t=r×s。
s
\mathbf s
s的计算方式:将
r
\mathbf r
r的绝对值最小分量置为
0
0
0;交换剩余两个分量,然后将它们之中的第一个分量改变正负性;
s
ˉ
=
{
(
0
,
−
r
z
,
r
y
)
,
if
∣
r
x
∣
≤
∣
r
y
∣
and
∣
r
x
∣
≤
∣
r
z
∣
,
(
−
r
z
,
0
,
r
x
)
,
if
∣
r
y
∣
≤
∣
r
x
∣
and
∣
r
y
∣
≤
∣
r
z
∣
,
(
−
r
y
,
r
x
,
0
)
,
if
∣
r
z
∣
≤
∣
r
x
∣
and
∣
r
z
∣
≤
∣
r
y
∣
,
s
=
s
ˉ
/
∥
s
ˉ
∥
,
t
=
r
×
s
\begin{array}{lcl} \bar{\mathbf s}=\begin{cases} (0,-r_z,r_y),\ \textrm{if}\ |r_x| \le |r_y|\ \textrm{and}\ |r_x| \le |r_z|,\\ (-r_z,0,r_x),\ \textrm{if}\ |r_y| \le |r_x|\ \textrm{and}\ |r_y| \le |r_z|,\\ (-r_y,r_x,0),\ \textrm{if}\ |r_z| \le |r_x|\ \textrm{and}\ |r_z| \le |r_y|,\end{cases}\\ \mathbf s=\bar{\mathbf s}/\|\bar{\mathbf s}\|,\\ \mathbf t=\mathbf r\times\mathbf s \end{array}
sˉ=⎩⎪⎨⎪⎧(0,−rz,ry), if ∣rx∣≤∣ry∣ and ∣rx∣≤∣rz∣,(−rz,0,rx), if ∣ry∣≤∣rx∣ and ∣ry∣≤∣rz∣,(−ry,rx,0), if ∣rz∣≤∣rx∣ and ∣rz∣≤∣ry∣,s=sˉ/∥sˉ∥,t=r×s
这可以保证 s ˉ \bar{\mathbf s} sˉ正交于 r \mathbf r r,且 ( r , s , t ) (\mathbf r,\mathbf s,\mathbf t) (r,s,t)是一组正交基。则旋转矩阵 M = ( r T s T t T ) \mathbf M=\begin{pmatrix}\mathbf r^T\\\mathbf s^T\\\mathbf t^T\end{pmatrix} M=⎝⎛rTsTtT⎠⎞,会将 r \mathbf r r转至 x x x轴, s \mathbf s s转至 y y y轴, t \mathbf t t转至 z z z轴,然后使用 R x ( α ) \mathbf R_x(\alpha) Rx(α)围绕 x x x轴旋转,最后使用 M \mathbf M M的逆矩阵变回原空间,这里 M − 1 = M T \mathbf M^{-1}=\mathbf M^T M−1=MT。即最终变换矩阵为: X = M T R x ( α ) M \mathbf X=\mathbf M^T\mathbf R_x(\alpha)\mathbf M X=MTRx(α)M。
另一种方法
假设围绕归一化的轴
r
\mathbf r
r旋转
ϕ
\phi
ϕ弧度,可以得到变换矩阵:
R
=
(
cos
ϕ
+
(
1
−
cos
ϕ
)
r
x
2
(
1
−
cos
ϕ
)
r
x
r
y
−
r
z
sin
ϕ
(
1
−
cos
ϕ
)
r
x
r
z
+
r
y
sin
ϕ
(
1
−
cos
ϕ
)
r
x
r
y
+
r
z
sin
ϕ
cos
ϕ
+
(
1
−
cos
ϕ
)
r
y
2
(
1
−
cos
ϕ
)
r
y
r
z
−
r
x
sin
ϕ
(
1
−
cos
ϕ
)
r
x
r
z
−
r
y
sin
ϕ
(
1
−
cos
ϕ
)
r
y
r
z
+
r
x
sin
ϕ
cos
ϕ
+
(
1
−
cos
ϕ
)
r
z
2
)
\mathbf R=\begin{pmatrix} \cos\phi+(1-\cos\phi)r_x^2 & (1-\cos\phi)r_xr_y-r_z\sin\phi & (1-\cos\phi)r_xr_z+r_y\sin\phi \\ (1-\cos\phi)r_xr_y+r_z\sin\phi & \cos\phi+(1-\cos\phi)r_y^2 & (1-\cos\phi)r_yr_z-r_x\sin\phi \\ (1-\cos\phi)r_xr_z-r_y\sin\phi & (1-\cos\phi)r_yr_z+r_x\sin\phi & \cos\phi+(1-\cos\phi)r_z^2 \end{pmatrix}
R=⎝⎛cosϕ+(1−cosϕ)rx2(1−cosϕ)rxry+rzsinϕ(1−cosϕ)rxrz−rysinϕ(1−cosϕ)rxry−rzsinϕcosϕ+(1−cosϕ)ry2(1−cosϕ)ryrz+rxsinϕ(1−cosϕ)rxrz+rysinϕ(1−cosϕ)ryrz−rxsinϕcosϕ+(1−cosϕ)rz2⎠⎞
三、四元数
四元数用于表示旋转和指向。任意的三维指向都可以表示为一个围绕特定轴的旋转。四元数可以用于稳定且恒定的指向插值。
四元数有四个部分,其中前三个值与旋转轴密切相关,二旋转的角度则会影响所有四个部分的值。
3.1 数学背景
定义:四元数
q
^
\hat{\mathbf q}
q^可以通过以下方式定义,
q
^
=
(
q
v
,
q
w
)
=
i
q
x
+
j
q
y
+
k
q
z
+
q
w
=
q
v
+
q
w
,
q
v
=
i
q
x
+
j
q
y
+
k
q
z
=
(
q
x
,
q
y
,
q
z
)
,
i
2
=
j
2
=
k
2
=
−
1
,
j
k
=
−
k
j
=
i
,
k
i
=
−
i
k
=
j
,
i
j
=
−
j
i
=
k
\hat{\mathbf q}=(\mathbf q_v,q_w)=iq_x+jq_y+kq_z+q_w=\mathbf q_v+q_w,\\ \mathbf q_v=iq_x+jq_y+kq_z=(q_x,q_y,q_z),\\ i^2=j^2=k^2=-1,\ jk=-kj=i,\ ki=-ik=j,\ ij=-ji=k
q^=(qv,qw)=iqx+jqy+kqz+qw=qv+qw,qv=iqx+jqy+kqz=(qx,qy,qz),i2=j2=k2=−1, jk=−kj=i, ki=−ik=j, ij=−ji=k
其中,
q
w
q_w
qw为四元数
q
^
\hat{\mathbf q}
q^的实部,
q
v
\mathbf q_v
qv是虚部,
i
,
j
,
k
i,j,k
i,j,k为虚数单位。
对于虚部
q
v
\mathbf q_v
qv,可以使使用所有单位向量的操作,如加法、减法、点乘、叉乘等。两个四元数
q
^
\hat{\mathbf q}
q^和
r
^
\hat{\mathbf r}
r^的乘法有如下定义,需要注意的是,四元数的虚部相乘不符合交换律:
q
^
r
^
=
(
i
q
x
+
j
q
y
+
k
q
z
+
q
w
)
(
i
r
x
+
j
r
y
+
k
r
z
+
r
w
)
=
i
(
q
y
r
z
−
q
z
r
y
+
r
w
q
x
+
q
w
r
x
)
+
j
(
q
z
r
x
−
q
x
r
z
+
r
w
q
y
+
q
w
r
y
)
+
k
(
q
x
r
y
−
q
y
r
x
+
r
w
q
z
+
q
w
r
z
)
+
q
w
r
w
−
q
x
r
x
−
q
y
r
y
−
q
z
r
z
=
(
q
v
×
r
v
+
r
w
q
v
+
q
w
r
v
,
q
w
r
w
−
q
v
⋅
r
v
)
\hat{\mathbf q}\hat{\mathbf r}=(iq_x+jq_y+kq_z+q_w)(ir_x+jr_y+kr_z+r_w)\\ =i(q_yr_z-q_zr_y+r_wq_x+q_wr_x)\\ +j(q_zr_x-q_xr_z+r_wq_y+q_wr_y)\\ +k(q_xr_y-q_yr_x+r_wq_z+q_wr_z)\\ +q_wr_w-q_xr_x-q_yr_y-q_zr_z\\ =(\mathbf q_v\times\mathbf r_v+r_w\mathbf q_v+q_w\mathbf r_v,\ q_wr_w-\mathbf q_v\cdot\mathbf r_v)
q^r^=(iqx+jqy+kqz+qw)(irx+jry+krz+rw)=i(qyrz−qzry+rwqx+qwrx)+j(qzrx−qxrz+rwqy+qwry)+k(qxry−qyrx+rwqz+qwrz)+qwrw−qxrx−qyry−qzrz=(qv×rv+rwqv+qwrv, qwrw−qv⋅rv)
加法:
q
^
+
r
^
=
(
q
v
,
q
w
)
+
(
r
v
,
r
w
)
=
(
q
v
+
r
v
,
q
w
+
r
w
)
\hat{\mathbf q}+\hat{\mathbf r}=(\mathbf q_v,q_w)+(\mathbf r_v,r_w)=(\mathbf q_v+\mathbf r_v,q_w+r_w)
q^+r^=(qv,qw)+(rv,rw)=(qv+rv,qw+rw)
共轭:
q
^
∗
=
(
q
v
,
q
w
)
∗
=
(
−
q
v
,
q
w
)
\hat{\mathbf q}^*=(\mathbf q_v,q_w)^*=(-\mathbf q_v,q_w)
q^∗=(qv,qw)∗=(−qv,qw)
范数:
n
(
q
^
)
=
q
^
q
^
∗
=
q
^
∗
q
^
=
q
v
⋅
q
v
+
q
w
2
=
q
x
2
+
q
y
2
+
q
z
2
+
q
w
2
\begin{array}{c}n(\hat{\mathbf q})=\sqrt{\hat{\mathbf q}\hat{\mathbf q}^*}=\sqrt{\hat{\mathbf q}^*\hat{\mathbf q}}=\sqrt{\mathbf q_v\cdot\mathbf q_v+q_w^2}\\ =\sqrt{q_x^2+q_y^2+q_z^2+q_w^2}\end{array}
n(q^)=q^q^∗=q^∗q^=qv⋅qv+qw2=qx2+qy2+qz2+qw2,有时记为:
∥
q
^
∥
\|\hat{\mathbf q}\|
∥q^∥
单位四元数:
i
^
=
(
0
,
1
)
\hat{\mathbf i}=(\mathbf 0,1)
i^=(0,1)
乘法倒数:
q
^
−
1
=
1
n
(
q
^
)
2
q
^
∗
\hat{\mathbf q}^{-1}=\cfrac{1}{n(\hat{\mathbf q})^2}\hat{\mathbf q}^*
q^−1=n(q^)21q^∗
标量乘法符合交换律:
q
^
s
=
s
q
^
=
(
s
q
v
,
s
q
w
)
\hat{\mathbf q}s=s\hat{\mathbf q}=(s\mathbf q_v,sq_w)
q^s=sq^=(sqv,sqw)
共轭规则:
(
q
^
∗
)
∗
=
q
^
,
(
q
^
+
r
^
)
∗
=
q
^
∗
+
r
^
∗
,
(
q
^
r
^
)
∗
=
r
^
∗
q
^
∗
\begin{array}{l}(\hat{\mathbf q}^*)^*=\hat{\mathbf q},\\ (\hat{\mathbf q}+\hat{\mathbf r})^*=\hat{\mathbf q}^*+\hat{\mathbf r}^*,\\ (\hat{\mathbf q}\hat{\mathbf r})^*=\hat{\mathbf r}^*\hat{\mathbf q}^*\end{array}
(q^∗)∗=q^,(q^+r^)∗=q^∗+r^∗,(q^r^)∗=r^∗q^∗
范数规则:
n
(
q
^
∗
)
=
n
(
q
^
)
,
n
(
q
^
r
^
)
=
n
(
q
^
)
n
(
r
^
)
\begin{array}{l}n(\hat{\mathbf q}^*)=n(\hat{\mathbf q}),\\ n(\hat{\mathbf q}\hat{\mathbf r})=n(\hat{\mathbf q})n(\hat{\mathbf r})\end{array}
n(q^∗)=n(q^),n(q^r^)=n(q^)n(r^)
乘法线性:
p
^
(
s
q
^
+
t
r
^
)
=
s
p
^
q
^
+
t
p
^
r
^
,
(
s
p
^
+
t
q
^
)
r
^
=
s
p
^
r
^
+
t
q
^
r
^
\begin{array}{l}\hat{\mathbf p}(s\hat{\mathbf q}+t\hat{\mathbf r})=s\hat{\mathbf p}\hat{\mathbf q}+t\hat{\mathbf p}\hat{\mathbf r},\\ (s\hat{\mathbf p}+t\hat{\mathbf q})\hat{\mathbf r}=s\hat{\mathbf p}\hat{\mathbf r}+t\hat{\mathbf q}\hat{\mathbf r}\end{array}
p^(sq^+tr^)=sp^q^+tp^r^,(sp^+tq^)r^=sp^r^+tq^r^
乘法结合律:
p
^
(
q
^
r
^
)
=
(
p
^
q
^
)
r
^
\hat{\mathbf p}(\hat{\mathbf q}\hat{\mathbf r})=(\hat{\mathbf p}\hat{\mathbf q})\hat{\mathbf r}
p^(q^r^)=(p^q^)r^
单位四元数,即 n ( q ^ ) = 1 n(\hat{\mathbf q})=1 n(q^)=1,可以记为: q ^ = ( sin ϕ u q , cos ϕ ) = sin ϕ u q + cos ϕ \hat{\mathbf q}=(\sin\phi\mathbf u_q,\cos\phi)=\sin\phi\mathbf u_q+\cos\phi q^=(sinϕuq,cosϕ)=sinϕuq+cosϕ,其中 ∥ u q ∥ = 1 \|\mathbf u_q\|=1 ∥uq∥=1
单位四元数的欧拉公式:
q
^
=
sin
ϕ
u
q
+
cos
ϕ
=
e
ϕ
u
q
\hat{\mathbf q}=\sin\phi\mathbf u_q+\cos\phi=e^{\phi\mathbf u_q}
q^=sinϕuq+cosϕ=eϕuq
对数:
log
(
q
^
)
=
log
(
e
ϕ
u
q
)
=
ϕ
u
q
\log(\hat{\mathbf q})=\log(e^{\phi\mathbf u_q})=\phi\mathbf u_q
log(q^)=log(eϕuq)=ϕuq
幂:
q
^
t
=
(
sin
ϕ
u
q
+
cos
ϕ
)
t
=
e
ϕ
t
u
q
=
sin
(
ϕ
t
)
u
q
+
cos
(
ϕ
t
)
\hat{\mathbf q}^t=(\sin\phi\mathbf u_q+\cos\phi)^t=e^{\phi t\mathbf u_q}=\sin(\phi t)\mathbf u_q+\cos(\phi t)
q^t=(sinϕuq+cosϕ)t=eϕtuq=sin(ϕt)uq+cos(ϕt)
3.2 四元数变换
四元数变换针对的是有单位长度的四元数,即单位四元数(unit quaternions)。单位四元数可以表示任何的三维旋转,并且表示形式非常简单紧凑。
假设一个点或向量 p = ( p x p y p z p w ) T \mathbf p=\begin{pmatrix}p_x&p_y&p_z&p_w\end{pmatrix}^T p=(pxpypzpw)T,将其各分量表示为四元数 p ^ \hat{\mathbf p} p^,给定单位四元数 q ^ = ( sin ϕ u q , cos ϕ ) \hat{\mathbf q}=(\sin\phi\mathbf u_q,\cos\phi) q^=(sinϕuq,cosϕ),那么 q ^ p ^ q ^ − 1 \hat{\mathbf q}\hat{\mathbf p}\hat{\mathbf q}^{-1} q^p^q^−1表示 p \mathbf p p围绕轴 u q \mathbf u_q uq旋转 2 ϕ 2\phi 2ϕ弧度。
q ^ \hat{\mathbf q} q^乘以任意非零数,都表示同样的旋转;也就是说将旋转轴 u q \mathbf u_q uq方向指向其负方向,并且将 q w q_w qw改变符号,那么可以创造出与原四元数表示相同旋转的四元数;同样也意味着,一个旋转矩阵可以推导出两个四元数,即 q ^ , − q ^ \hat{\mathbf q},-\hat{\mathbf q} q^,−q^。
给定两个单位四元数 q ^ \hat{\mathbf q} q^和 r ^ \hat{\mathbf r} r^,串联应用: r ^ ( q ^ p ^ q ^ ∗ ) r ^ ∗ = ( r ^ q ^ ) p ^ ( r ^ q ^ ) ∗ = c ^ q ^ c ^ ∗ \hat{\mathbf r}(\hat{\mathbf q}\hat{\mathbf p}\hat{\mathbf q}^*)\hat{\mathbf r}^*=(\hat{\mathbf r}\hat{\mathbf q})\hat{\mathbf p}(\hat{\mathbf r}\hat{\mathbf q})^*=\hat{\mathbf c}\hat{\mathbf q}\hat{\mathbf c}^* r^(q^p^q^∗)r^∗=(r^q^)p^(r^q^)∗=c^q^c^∗,其中 c ^ = r ^ q ^ \hat{\mathbf c}=\hat{\mathbf r}\hat{\mathbf q} c^=r^q^,且为单位四元数。
矩阵转换
从单位四元数转化为变换矩阵
q
^
p
^
q
^
−
1
\hat{\mathbf q}\hat{\mathbf p}\hat{\mathbf q}^{-1}
q^p^q^−1可以表示如下矩阵:
M
q
=
(
1
−
s
(
q
y
2
+
q
z
2
)
s
(
q
x
q
y
−
q
w
q
z
)
s
(
q
x
q
z
+
q
w
q
y
)
0
s
(
q
x
q
y
+
q
w
q
z
)
1
−
s
(
q
x
2
+
q
z
2
)
s
(
q
y
q
z
−
q
w
q
x
)
0
s
(
q
x
q
z
−
q
w
q
y
)
s
(
q
y
q
z
+
q
w
q
x
)
1
−
s
(
q
x
2
+
q
y
2
)
0
0
0
0
1
)
\mathbf M^q=\begin{pmatrix}1-s(q_y^2+q_z^2)&s(q_xq_y-q_wq_z)&s(q_xq_z+q_wq_y)&0\\ s(q_xq_y+q_wq_z)&1-s(q_x^2+q_z^2)&s(q_yq_z-q_wq_x)&0\\ s(q_xq_z-q_wq_y)&s(q_yq_z+q_wq_x)&1-s(q_x^2+q_y^2)&0\\ 0&0&0&1\end{pmatrix}
Mq=⎝⎜⎜⎛1−s(qy2+qz2)s(qxqy+qwqz)s(qxqz−qwqy)0s(qxqy−qwqz)1−s(qx2+qz2)s(qyqz+qwqx)0s(qxqz+qwqy)s(qyqz−qwqx)1−s(qx2+qy2)00001⎠⎟⎟⎞
其中,标量
s
=
2
/
(
n
(
q
^
)
)
2
s=2/(n(\hat{\mathbf q}))^2
s=2/(n(q^))2,那么对于单位矩阵,则可以简化为:
M
q
=
(
1
−
2
(
q
y
2
+
q
z
2
)
2
(
q
x
q
y
−
q
w
q
z
)
2
(
q
x
q
z
+
q
w
q
y
)
0
2
(
q
x
q
y
+
q
w
q
z
)
1
−
2
(
q
x
2
+
q
z
2
)
2
(
q
y
q
z
−
q
w
q
x
)
0
2
(
q
x
q
z
−
q
w
q
y
)
2
(
q
y
q
z
+
q
w
q
x
)
1
−
2
(
q
x
2
+
q
y
2
)
0
0
0
0
1
)
\mathbf M^q=\begin{pmatrix}1-2(q_y^2+q_z^2)&2(q_xq_y-q_wq_z)&2(q_xq_z+q_wq_y)&0\\ 2(q_xq_y+q_wq_z)&1-2(q_x^2+q_z^2)&2(q_yq_z-q_wq_x)&0\\ 2(q_xq_z-q_wq_y)&2(q_yq_z+q_wq_x)&1-2(q_x^2+q_y^2)&0\\ 0&0&0&1\end{pmatrix}
Mq=⎝⎜⎜⎛1−2(qy2+qz2)2(qxqy+qwqz)2(qxqz−qwqy)02(qxqy−qwqz)1−2(qx2+qz2)2(qyqz+qwqx)02(qxqz+qwqy)2(qyqz−qwqx)1−2(qx2+qy2)00001⎠⎟⎟⎞
因为四元数已经建立,所以不需要计算任何的三角函数,所以在实际应用中,效率更高。
从变换矩阵转化为单位四元数
可从
M
q
\mathbf M^q
Mq中得到如下关键信息:
m
21
q
−
m
12
q
=
4
q
w
q
x
,
m
02
q
−
m
20
q
=
4
q
w
q
y
,
m
10
q
−
m
01
q
=
4
q
w
q
z
\begin{array}{l}m_{21}^q-m_{12}^q=4q_wq_x,\\ m_{02}^q-m_{20}^q=4q_wq_y,\\ m_{10}^q-m_{01}^q=4q_wq_z\end{array}
m21q−m12q=4qwqx,m02q−m20q=4qwqy,m10q−m01q=4qwqz
M
q
\mathbf M^q
Mq的迹可以如下计算:
t
r
(
M
q
)
=
4
−
2
s
(
q
x
2
+
q
y
2
+
q
z
2
)
=
4
(
1
−
q
x
2
+
q
y
2
+
q
z
2
q
x
2
+
q
y
2
+
q
z
2
+
q
w
2
)
=
4
q
w
2
q
x
2
+
q
y
2
+
q
z
2
+
q
w
2
=
4
q
w
2
(
n
(
q
^
)
)
2
\mathrm{tr}(\mathbf M^q)=4-2s(q_x^2+q_y^2+q_z^2)=4\left(1-\frac{q_x^2+q_y^2+q_z^2}{q_x^2+q_y^2+q_z^2+q_w^2}\right)\\ =\frac{4q_w^2}{q_x^2+q_y^2+q_z^2+q_w^2}=\frac{4q_w^2}{(n(\hat{\mathbf q}))^2}
tr(Mq)=4−2s(qx2+qy2+qz2)=4(1−qx2+qy2+qz2+qw2qx2+qy2+qz2)=qx2+qy2+qz2+qw24qw2=(n(q^))24qw2
综上可以得到:
q
w
=
1
2
t
r
(
M
q
)
,
q
x
=
m
21
q
−
m
12
q
4
q
w
,
q
y
=
m
01
q
−
m
20
q
4
q
w
,
q
z
=
m
10
q
−
m
01
q
4
q
w
q_w=\frac{1}{2}\sqrt{\mathrm{tr}(\mathbf M^q)},\quad q_x=\frac{m_{21}^q-m_{12}^q}{4q_w},\\ q_y=\frac{m_{01}^q-m_{20}^q}{4q_w},\quad q_z=\frac{m_{10}^q-m_{01}^q}{4q_w}
qw=21tr(Mq),qx=4qwm21q−m12q,qy=4qwm01q−m20q,qz=4qwm10q−m01q
从数值稳定性方面来看,需要避免除以小数字,因此另一种计算方式:首先设
t
=
q
w
2
−
q
x
2
−
q
y
2
−
q
z
2
t=q_w^2-q_x^2-q_y^2-q_z^2
t=qw2−qx2−qy2−qz2,那么可以得到如下:
m
00
=
t
+
2
q
x
2
,
m
11
=
t
+
2
q
y
2
,
m
22
=
t
+
2
q
z
2
,
u
=
m
00
+
m
11
+
m
22
=
t
+
2
q
w
2
m_{00}=t+2q_x^2,\\ m_{11}=t+2q_y^2,\\ m_{22}=t+2q_z^2,\\ u=m_{00}+m_{11}+m_{22}=t+2q_w^2
m00=t+2qx2,m11=t+2qy2,m22=t+2qz2,u=m00+m11+m22=t+2qw2
这里通过
m
00
,
m
11
,
m
22
,
u
m_{00},m_{11},m_{22},u
m00,m11,m22,u来确定
q
x
,
q
y
,
q
z
,
q
w
q_x,q_y,q_z,q_w
qx,qy,qz,qw哪个最大,如果是
q
w
q_w
qw最大,则使用上述的结论;否则,使用如下方式计算:
4
q
x
2
=
+
m
00
−
m
11
−
m
22
+
m
33
,
4
q
y
2
=
−
m
00
+
m
11
−
m
22
+
m
33
,
4
q
z
2
=
−
m
00
−
m
11
+
m
22
+
m
33
,
4
q
w
2
=
t
r
(
M
q
)
4q_x^2=+m_{00}-m_{11}-m_{22}+m_{33},\\ 4q_y^2=-m_{00}+m_{11}-m_{22}+m_{33},\\ 4q_z^2=-m_{00}-m_{11}+m_{22}+m_{33},\\ 4q_w^2=\mathrm{tr}(\mathbf M^q)
4qx2=+m00−m11−m22+m33,4qy2=−m00+m11−m22+m33,4qz2=−m00−m11+m22+m33,4qw2=tr(Mq)
使用相应的方程计算出最大 q x , q y , q z , q w q_x,q_y,q_z,q_w qx,qy,qz,qw的最大值,然后使用最开始的关键信息计算出 q ^ \hat{\mathbf q} q^的其余分量。
球面线性插值
球面线性插值表示的是,给定两个单位四元数 q ^ , r ^ \hat{\mathbf q},\hat{\mathbf r} q^,r^,以及一个参数 t ∈ [ 0 , 1 ] t\in [0,1] t∈[0,1],来计算一个插值后的四元数。
代数形式的表示:
s
^
(
q
^
,
r
^
,
t
)
=
(
r
^
q
^
−
1
)
t
q
^
\hat{\mathbf s}(\hat{\mathbf q},\hat{\mathbf r},t)=(\hat{\mathbf r}\hat{\mathbf q}^{-1})^t\hat{\mathbf q}
s^(q^,r^,t)=(r^q^−1)tq^。
对于软件实现来说,使用
s
l
e
r
p
slerp
slerp函数来表示更合适:
s
^
(
q
^
,
r
^
,
t
)
=
s
l
e
r
p
(
q
^
,
r
^
,
t
)
=
sin
(
ϕ
(
1
−
t
)
)
sin
ϕ
q
^
+
sin
(
ϕ
t
)
sin
ϕ
r
^
\hat{\mathbf s}(\hat{\mathbf q},\hat{\mathbf r},t)=\mathrm{slerp}(\hat{\mathbf q},\hat{\mathbf r},t)=\cfrac{\sin(\phi(1-t))}{\sin\phi}\hat{\mathbf q}+\cfrac{\sin(\phi t)}{\sin\phi}\hat{\mathbf r}
s^(q^,r^,t)=slerp(q^,r^,t)=sinϕsin(ϕ(1−t))q^+sinϕsin(ϕt)r^。可以利用
cos
ϕ
=
q
x
r
x
+
q
y
r
y
+
q
z
r
z
+
q
w
r
w
\cos\phi=q_xr_x+q_yr_y+q_zr_z+q_wr_w
cosϕ=qxrx+qyry+qzrz+qwrw来计算
ϕ
\phi
ϕ。对于
t
∈
[
0
,
1
]
t\in [0,1]
t∈[0,1]来说可以插值出唯一的四元数,且这些插值可以组成在单位四元数
q
^
(
t
=
0
)
,
r
^
(
t
=
1
)
\hat{\mathbf q}(t=0),\hat{\mathbf r}(t=1)
q^(t=0),r^(t=1)之间的最短球面弧。且计算出的插值,是围绕固定轴以恒定速度旋转,无加速度;这种曲线称为测地曲率(geodesic curve)。
Slerp函数特别适合做两个指向的插值。但是在实际应用中,直接计算球面线性插值是比较消耗的操作,因为包含快乐三角函数的计算。有些优化方法可以用来优化性能。
当两个以上指向,如:
q
^
0
,
q
^
1
,
.
.
.
,
q
^
n
−
1
\hat{\mathbf q}_0,\hat{\mathbf q}_1,...,\hat{\mathbf q}_{n-1}
q^0,q^1,...,q^n−1,依次插值,可以直接使用slerp函数;但是直接使用,会产生突变。更好的办法是使用样条来插值。在
q
^
i
,
q
^
i
+
1
\hat{\mathbf q}_i,\hat{\mathbf q}_{i+1}
q^i,q^i+1之间引入
a
^
i
,
a
^
i
+
1
\hat{\mathbf a}_i,\hat{\mathbf a}_{i+1}
a^i,a^i+1,可以通过他们实现球面三次插值。其中
a
^
i
=
q
^
i
exp
[
−
log
(
q
^
i
−
1
q
^
i
−
1
)
+
log
(
q
^
i
−
1
q
^
i
+
1
)
4
]
\hat{\mathbf a}_i=\hat{\mathbf q}_i\exp\left[-\cfrac{\log(\hat{\mathbf q}_i^{-1}\hat{\mathbf q}_{i-1})+\log(\hat{\mathbf q}_i^{-1}\hat{\mathbf q}_{i+1})}{4}\right]
a^i=q^iexp[−4log(q^i−1q^i−1)+log(q^i−1q^i+1)]。
使用光滑的三次样条来进行球面线性插值可如下计算:
s
q
u
a
d
(
q
^
i
,
q
^
i
+
1
,
a
^
i
,
a
^
i
+
1
,
t
)
=
s
l
e
r
p
(
s
l
e
r
p
(
q
^
i
,
q
^
i
+
1
,
t
)
,
s
l
e
r
p
(
a
^
i
,
a
^
i
+
1
,
t
)
,
2
t
(
1
−
t
)
)
\begin{array}{l} \mathrm{squad}(\hat{\mathbf q}_i,\hat{\mathbf q}_{i+1},\hat{\mathbf a}_i,\hat{\mathbf a}_{i+1},t)=\\ \quad\mathrm{slerp}(\mathrm{slerp}(\hat{\mathbf q}_i,\hat{\mathbf q}_{i+1},t),\mathrm{slerp}(\hat{\mathbf a}_i,\hat{\mathbf a}_{i+1},t),2t(1-t)) \end{array}
squad(q^i,q^i+1,a^i,a^i+1,t)=slerp(slerp(q^i,q^i+1,t),slerp(a^i,a^i+1,t),2t(1−t))
这个插值会经过
q
^
i
,
i
∈
[
0
,
.
.
.
,
n
−
1
]
\hat{\mathbf q}_i,\ i\in [0,...,n-1]
q^i, i∈[0,...,n−1],但不会经过
a
^
i
\hat{\mathbf a}_i
a^i,
a
^
i
\hat{\mathbf a}_i
a^i表示的是切线方向。
从一个向量旋转至另一个
计算从
s
\mathbf s
s方向以最短路径变换至另一个方向
t
\mathbf t
t。
首先,归一化
s
,
t
\mathbf s,\mathbf t
s,t;然后计算单位旋转轴
u
=
(
s
×
t
)
/
∥
s
×
t
∥
\mathbf u=(\mathbf s\times\mathbf t)/\|\mathbf s\times\mathbf t\|
u=(s×t)/∥s×t∥;
第二步,
e
=
s
⋅
t
=
cos
(
2
ϕ
)
,
∥
s
×
t
∥
=
sin
(
2
ϕ
)
e=\mathbf s\cdot\mathbf t=\cos(2\phi),\ \|\mathbf s\times\mathbf t\|=\sin(2\phi)
e=s⋅t=cos(2ϕ), ∥s×t∥=sin(2ϕ),其中
2
ϕ
2\phi
2ϕ是
s
,
t
\mathbf s,\mathbf t
s,t之间的旋转角;
可得到四元数
q
^
=
(
sin
ϕ
u
,
cos
ϕ
)
=
(
sin
ϕ
sin
2
ϕ
(
s
×
t
)
,
cos
ϕ
)
\hat{\mathbf q}=(\sin\phi\mathbf u,\cos\phi)=(\cfrac{\sin\phi}{\sin2\phi}(\mathbf s\times\mathbf t),\cos\phi)
q^=(sinϕu,cosϕ)=(sin2ϕsinϕ(s×t),cosϕ),通过三角函数的半角公式可以化简为:
q
^
=
(
q
v
,
q
w
)
=
(
1
2
(
1
+
e
)
(
s
×
t
)
,
2
(
1
+
e
)
2
)
\hat{\mathbf q}=(\mathbf q_v,q_w)=\left(\cfrac{1}{\sqrt{2(1+e)}}(\mathbf s\times\mathbf t),\cfrac{\sqrt{2(1+e)}}{2}\right)
q^=(qv,qw)=(2(1+e)1(s×t),22(1+e))
直接通过这种方法生成四元数,相较于归一化叉乘,可在 s , t \mathbf s,\mathbf t s,t指向相近方向时,避免出现数值不稳定性。但是当 s , t \mathbf s,\mathbf t s,t指向相反方向时,两种方法都无法避免数值不稳定性。当这种情况出现时,任何垂直于 s \mathbf s s的轴,都可以用来作为旋转轴。
对应的旋转矩阵为: R ( s , t ) = ( e + h v x 2 h v x v y − v z h v x v z + v y 0 h v x v y + v z e + h v y 2 h v y v z − v x 0 h v x v z − v y h v y v z + v x e + h v z 2 0 0 0 0 1 ) \mathbf R(\mathbf s,\mathbf t)=\begin{pmatrix}e+hv_x^2 & hv_xv_y-v_z & hv_xv_z+v_y & 0\\ hv_xv_y+v_z & e+hv_y^2 & hv_yv_z-v_x & 0\\ hv_xv_z-v_y & hv_yv_z+v_x & e+hv_z^2 & 0\\ 0 & 0 & 0 & 1\end{pmatrix} R(s,t)=⎝⎜⎜⎛e+hvx2hvxvy+vzhvxvz−vy0hvxvy−vze+hvy2hvyvz+vx0hvxvz+vyhvyvz−vxe+hvz200001⎠⎟⎟⎞,其中, v = s × t , e = cos ( 2 ϕ ) = s ⋅ t , h = 1 − cos ( 2 ϕ ) sin 2 ( 2 ) ϕ = 1 − e v ⋅ v = 1 1 + e \mathbf v=\mathbf s\times\mathbf t,\ e=\cos(2\phi)=\mathbf s\cdot\mathbf t,\ h=\cfrac{1-\cos(2\phi)}{\sin^2(2)\phi}=\cfrac{1-e}{\mathbf v\cdot\mathbf v}=\cfrac{1}{1+e} v=s×t, e=cos(2ϕ)=s⋅t, h=sin2(2)ϕ1−cos(2ϕ)=v⋅v1−e=1+e1。可以看到,通过简化,所有的开方和三角函数都消失了,所以该方法非常高效。