3D 中的方位与角位移(旋转矩阵、欧拉角、四元数)

2023-09-24 20:37:45

文章目录

一、3D 中的方位与角位移
引用

一、3D 中的方位与角位移

方位：从上一方位旋转后的结果值（单一状态，用欧拉角表示）
角位移：相对于上一方位旋转后的偏移量（用四元数、矩阵表示）

1. 欧拉角 (Euler angles)

定义：

欧拉角可以用来描述任意旋转，将一个角位移分解为三个互相垂直轴的顺序旋转步骤
旋转后，原来互相垂直的轴可能不再垂直，当前步骤只能影响下一个旋转步骤，不能影响之前的旋转步骤，通过这个特性我们可以选择**适合的旋转方式 (如：YXZ）**来避免万像锁的发生
这里默认右手坐标系，逆时针为正，任意三个轴可以作为旋转轴，下图仅为举例
heading：绕惯性坐标系的 Y 轴旋转
yaw：绕模型坐标系的 Y 轴旋转

优点：

仅需要三个角度值，节省内存
表示直观（便于显示和输入方位）
任意三个数对于欧拉角都是合法的

缺点：

欧拉角之间求差值（角位移）困难
欧拉角的方位表达方式不唯一（n = n + 360）
由于三个角度不互相独立，可能产生：pich 135 = heading 180 + pich 45 + bank 180 的情况
（通过限制角的范围解决，heading 和 bank 限制在 -180 ～ 180，pitch 限制在 -90 ～90）
万向锁问题（避免方法：重新排列角度的旋转顺序，但万向锁仍可能产生）

万向锁：

当沿着任意角位移 90 度时，导致两个方向的旋转轴同线，导致三次旋转中有两次旋转的效果是一样的
即：少了一个维度的旋转变化
在万向锁的情况下仍可以旋转到想要的位置，但必须同时旋转三个轴向，这时物体没有按照期望的方式旋转，下图的箭头如果是相机，则相机会发生抖动

万向锁

发生万向锁后的旋转

期望的旋转

2. 四元数的相关知识

2.1 复数

复数是一种复合的数字， C 复数 = a + b ⋅ i C_{复数} = a + b \cdot i C复数=a+b⋅i ，其中 a、b 为实数， i i i 为虚数， i 2 = − 1 i^2 = -1 i2=−1

复数通过 实部 a 和虚部 b 构成的二维虚平面，将一维的数扩展到二维平面
i i i 只是区分实部 a 和虚部 b 的标记，这样可以把复数用二维的向量表示
加法： ( a + b i ) + ( c + d i ) = ( a + c ) + ( b + d ) i (a + bi) + (c + di) = (a + c) + (b+d)i (a+bi)+(c+di)=(a+c)+(b+d)i
乘法： ( a + b i ) ∗ ( c + d i ) = ( a c − b d ) + ( b c + a d ) i (a + bi) *(c + di) = (ac - bd) + (bc+ad)i (a+bi)∗(c+di)=(ac−bd)+(bc+ad)i
复数乘以 i i i 的几何意义，在二维平面上逆时针旋转 90 度
( a + b ⋅ i ) ∗ i = a ∗ i + b ∗ i 2 = − b + a ⋅ i (a + b \cdot i) * i = a*i + b*i^2 = -b + a \cdot i (a+b⋅i)∗i=a∗i+b∗i2=−b+a⋅i

2.2 欧拉旋转定理

这里 a = c o s ϕ , b = s i n ϕ , ϕ a = cos \phi, b = sin \phi, \phi a=cosϕ,b=sinϕ,ϕ 是从上一方位到当前方位的角位移，复数值表示当前方位则 c 方位 = a + b ⋅ i c_{方位} = a + b \cdot i c方位=a+b⋅i 在极坐标下表示为 e 方位 = c o s ϕ + s i n ϕ ⋅ i e_{方位} = cos \phi + sin \phi \cdot i e方位=cosϕ+sinϕ⋅i
在极坐标下，复数能够更方便的表示旋转角的变化：

连续的旋转可以用两个复数的乘积表示
例： e 1 e_1 e1 为先旋转 ϕ \phi ϕ， e 2 e_2 e2 再旋转 θ \theta θ，则根据 i 2 = − 1 i^2 = -1 i2=−1 以及和差公式可得最后的方位
e 1 = c o s ϕ + s i n ϕ ⋅ i e 2 = c o s θ + s i n θ ⋅ i e = c o s ( ϕ + θ ) + s i n ( ϕ + θ ) ⋅ i = ( c o s ϕ ⋅ c o s θ − s i n ϕ ⋅ s i n θ ) + ( s i n ϕ ⋅ c o s θ + c o s ϕ ⋅ s i n θ ) ⋅ i = ( c o s ϕ + s i n ϕ ⋅ i ) ( c o s θ + s i n θ ⋅ i ) = e 1 ⋅ e 2 \begin{aligned} e_1 &= cos \phi + sin \phi \cdot i \\ e_2 &= cos \theta + sin \theta \cdot i \\ e &= cos(\phi + \theta) + sin(\phi + \theta) \cdot i \\ &= (cos \phi \cdot cos \theta - sin \phi \cdot sin \theta) +(sin \phi \cdot cos \theta + cos \phi \cdot sin \theta) \cdot i \\ &= (cos \phi + sin \phi \cdot i )(cos \theta + sin \theta \cdot i) \\ &= e_1 \cdot e_2 \end{aligned} e1e2e=cosϕ+sinϕ⋅i=cosθ+sinθ⋅i=cos(ϕ+θ)+sin(ϕ+θ)⋅i=(cosϕ⋅cosθ−sinϕ⋅sinθ)+(sinϕ⋅cosθ+cosϕ⋅sinθ)⋅i=(cosϕ+sinϕ⋅i)(cosθ+sinθ⋅i)=e1⋅e2
在不知道当前角位移的情况下，可以通过当前方位+角位移计算出之后的方位

2.3 三维空间旋转的拆分

四元数在表示三维空间旋转的方式时采用轴角式——Axis-angle的旋转
轴角式旋转方法如下图，v 绕过原点的方向向量 u 逆时针旋转 θ \theta θ 得到 v ’ （图中采用右手坐标系，逆时针旋转方向为正方向）

拆分轴角式旋转：

如下图 A，假设 u 是单位向量（求 v 到 u 的投影向量 v||）
v = v ∣ ∣ + v ⊥ v ∣ ∣ = v ∣ ∣ ′ v ∣ ∣ = p r o j u ( v ) = u ⋅ v ∣ ∣ u ∣ ∣ ⋅ u ∣ ∣ u ∣ ∣ = ( u ⋅ v ) u ∣ ∣ u ∣ ∣ 2 = ( u ⋅ v ) u v ⊥ = v − v ∣ ∣ = v − ( u ⋅ v ) u \begin{aligned} v &= v_{||} + v_{\bot}\\ v_{||} &= v_{||}'\\ v_{||} &= proj_u(v) = {u \cdot v \over ||u||} \cdot {u \over ||u||} = {(u \cdot v)u \over ||u||^2} = (u \cdot v)u\\ v_{\bot} &= v - v_{||} = v - (u \cdot v)u \end{aligned} vv∣∣v∣∣v⊥=v∣∣+v⊥=v∣∣′=proju(v)=∣∣u∣∣u⋅v⋅∣∣u∣∣u=∣∣u∣∣2(u⋅v)u=(u⋅v)u=v−v∣∣=v−(u⋅v)u
如下图 B，C：w 既垂直于 u 又垂直于 v ⊥ v_{\bot} v⊥
w = u × v ⊥ ∣ ∣ w ∣ ∣ = ∣ ∣ u × v ⊥ ∣ ∣ = ∣ ∣ u ∣ ∣ ⋅ ∣ ∣ v ⊥ ∣ ∣ ⋅ s i n π 2 = ∣ ∣ v ⊥ ∣ ∣ v ⊥ ′ = v v ′ + v w ′ = v ⊥ ⋅ c o s θ + w ⋅ s i n θ = v ⊥ ⋅ c o s θ + ( u × v ⊥ ) s i n θ \begin{aligned} w &= u \times v_{\bot}\\ ||w|| &= ||u \times v_{\bot}|| \\ &= ||u|| \cdot ||v_{\bot}|| \cdot sin{\pi \over 2}\\ &= ||v_{\bot}|| \\\\ v_{\bot}' &= v_v' + v_w'\\ &= v_{\bot} \cdot cos\theta + w \cdot sin\theta \\ &= v_{\bot} \cdot cos\theta + (u \times v_{\bot})sin\theta \end{aligned} w∣∣w∣∣v⊥′=u×v⊥=∣∣u×v⊥∣∣=∣∣u∣∣⋅∣∣v⊥∣∣⋅sin2π=∣∣v⊥∣∣=vv′+vw′=v⊥⋅cosθ+w⋅sinθ=v⊥⋅cosθ+(u×v⊥)sinθ
结合 1，2 可得
v ′ = v ⋅ c o s θ + ( u ⋅ v ) u ( 1 − c o s θ ) + ( u × v ) s i n θ v' = v \cdot cos\theta + (u\cdot v)u(1 - cos\theta)+(u \times v)sin\theta v′=v⋅cosθ+(u⋅v)u(1−cosθ)+(u×v)sinθ

3. 四元数 (Quaternion)

相对于复数的二维空间，为了解决三维空间的旋转变化问题，爱尔兰数学家 William Rowan Hamilton 把复数进行了推广，也就是四元数

四元数包含旋转方向： 3D 中的一个旋转对应正向和反向旋转的两个四元数，不是一一对应的

定义：四元数表示角位移的大小

与复数类似，四元数由 1 个实部和 3 个虚部构成。其中，a、b、c 、d 为实数， i i i 为虚数
Q ⃗ 四元数 = a + b ⋅ i x + c ⋅ i y + d ⋅ i z \vec Q_{四元数} = a + b \cdot i_x + c \cdot i_y + d \cdot i_z Q 四元数=a+b⋅ix+c⋅iy+d⋅iz
i i i 代表旋转， − i -i −i 代表反向旋转
i x ∗ i y ∗ i z = i x ∗ i x = i y ∗ i y = i z ∗ i z = − 1 i x = i y ∗ i z = − i z ∗ i y = − i x − i y = i x ∗ i z \begin{aligned} i_x * i_y * i_z &= i_x * i_x = i_y * i_y = i_z * i_z = -1\\ i_x &= i_y * i_z = - i_z * i_y = - i_x\\ -i_y &= i_x * i_z \\ \end{aligned} ix∗iy∗izix−iy=ix∗ix=iy∗iy=iz∗iz=−1=iy∗iz=−iz∗iy=−ix=ix∗iz
四元数的 3 个虚数 i i i 之间的乘法与向量之间的点积结果形式很类似，于是四元数有了另外一种表示形式

Q ⃗ 四元数 = w + x ⋅ i x + y ⋅ i y + z ⋅ i z = ( x , y , z , w ) = ( u ⃗ , w ) \begin{aligned} \vec Q_{四元数} &= w + x \cdot i_x + y \cdot i_y + z \cdot i_z \\ &= (x, y, z, w) \\ &= (\vec u, w) \end{aligned} Q 四元数=w+x⋅ix+y⋅iy+z⋅iz=(x,y,z,w)=(u ,w)

优点：

平滑差值：slerp 和 squad 提供了方位间的平滑差值，矩阵和欧拉角都没有这个功能
快速连接和角位移求逆：
多个角位移四元数叉乘 → {四元数叉乘 \over \to} →四元数叉乘单个角位移（比矩阵快）
反角位移 = 四元数的共轭（比矩阵快）

缺点：

四元数比欧拉角多存储一个数（当保存大量角位移时尤为明显，如存储动画数据）
浮点数舍入误差和随意输入，会导致四元数不合法（可以通过四元数标准化解决，确保四元数为单位大小）
难以直接使用

3.1 四元数的运算

乘法，合并两个四元数的偏移量，得到总的角位移
四元数的乘法有很多种，其中最常用的一种是格拉丝曼积，与数学多项式乘法相同（与复数乘法概念相同）
乘法满足结合律：abc = (ab)c = a(bc)
但不符合交换律：ab != ba
Q ⃗ 1 Q ⃗ 2 = w 1 w 2 − u ⃗ 1 ⋅ u ⃗ 2 + w 1 u ⃗ 2 + w 2 u ⃗ 1 + u ⃗ 1 × u ⃗ 2 = ( ( w 1 u ⃗ 2 + w 2 u ⃗ 1 + u ⃗ 1 × u ⃗ 2 ) , ( w 1 w 2 − u ⃗ 1 ⋅ u ⃗ 2 ) ) \begin{aligned} \vec Q_1 \vec Q_2 &= w_1w_2 - \vec u_1 \cdot \vec u_2 + w_1 \vec u_2 + w_2 \vec u_1 + \vec u_1 \times \vec u_2\\ &= ((w_1 \vec u_2 + w_2 \vec u_1 + \vec u_1 \times \vec u_2), (w_1w_2 - \vec u_1 \cdot \vec u_2)) \end{aligned} Q 1Q 2=w1w2−u 1⋅u 2+w1u 2+w2u 1+u 1×u 2=((w1u 2+w2u 1+u 1×u 2),(w1w2−u 1⋅u 2))
四元数与标量相乘、点积、加法、叉乘、单位化，均与四维向量的点积相同，以点积为例
Q ⃗ 1 ⋅ Q ⃗ 2 = ( w 1 , x 1 , y 1 , z 1 ) ( w 2 , x 2 , y 2 , z 2 ) = w 1 w 2 + x 1 x 2 + y 1 y 2 + z 1 z 2 \begin{aligned} \vec Q_1 \cdot \vec Q_2 &= (w_1, x_1, y_1, z_1)(w_2, x_2, y_2, z_2) \\ &= w_1w_2 + x_1 x_2 + y_1y_2 + z_1z_2 \end{aligned} Q 1⋅Q 2=(w1,x1,y1,z1)(w2,x2,y2,z2)=w1w2+x1x2+y1y2+z1z2
求模：代表一个四维的长度，与向量的求模方法一致
∣ ∣ Q ⃗ ∣ ∣ = w 2 + u ⃗ ⋅ u ⃗ = w 2 + x 2 + y 2 + z 2 ||\vec Q|| = \sqrt{w^2 + \vec u \cdot \vec u} = \sqrt{w^2 + x^2 + y^2 + z^2} ∣∣Q ∣∣=w2+u ⋅u =w2+x2+y2+z2
共轭：实部相同，虚部相反（与复数共轭概念相同）
Q ⃗ ∗ Q ⃗ = ( − u ⃗ , w ) ( u ⃗ , w ) = ∣ ∣ Q ⃗ ∣ ∣ 2 = Q ⃗ ⋅ Q ⃗ \vec Q^* \vec Q = (-\vec u, w) (\vec u,w) = ||\vec Q||^2 = \vec Q \cdot \vec Q Q ∗Q =(−u ,w)(u ,w)=∣∣Q ∣∣2=Q ⋅Q
求逆：为了计算四元数的 “差”，例
给定方位 A 和 B，求 A 旋转到 B 的角位移 d，即：Ad = B， d = A − 1 B d = A^{-1}B d=A−1B
Q ⃗ Q ⃗ − 1 = 1 Q ⃗ ∗ Q ⃗ Q ⃗ − 1 = Q ⃗ ∗ ∣ ∣ Q ⃗ ∣ ∣ 2 ⋅ Q ⃗ − 1 = Q ⃗ ∗ Q ⃗ − 1 = Q ⃗ ∗ ∣ ∣ Q ⃗ ∣ ∣ 2 \begin{aligned} \vec Q\vec Q^{-1} &= 1\\ \vec Q^* \vec Q\vec Q^{-1} &= \vec Q^*\\ ||\vec Q||^2 \cdot \vec Q^{-1} &= \vec Q^*\\ \vec Q^{-1} &= {\vec Q^* \over ||\vec Q||^2}\\ \end{aligned} Q Q −1Q ∗Q Q −1∣∣Q ∣∣2⋅Q −1Q −1=1=Q ∗=Q ∗=∣∣Q ∣∣2Q ∗
单位四元数：任意四元数乘以单位四元数后保持不变， ( 0 ⃗ , ± 1 ) (\vec 0, \pm 1) (0 ,±1)，模为 1
单位四元数的逆 = 共轭，由于共轭比逆好求出，一般用四元数的共轭代替逆使用
几何上存在两个单位四元数 -u 和 u，因为他们几何意义相同都表示没有位移，但数学上只有 u
Q 单位 = Q ∣ ∣ Q ∣ ∣ Q 单位 Q 1 = Q 1 Q 单位 = Q 1 ( ( w 1 u ⃗ + w u ⃗ 1 + u ⃗ 1 × u ⃗ ) , ( w 1 w − u ⃗ 1 ⋅ u ⃗ ) ) = ( u ⃗ 1 , w 1 ) Q_{单位} = {Q \over ||Q||}\\ Q_{单位}Q_1 = Q_1Q_{单位} = Q_1\\ ((w_1 \vec u + w \vec u_1 + \vec u_1 \times \vec u), (w_1w - \vec u_1 \cdot \vec u)) = (\vec u_1, w_1) Q单位=∣∣Q∣∣QQ单位Q1=Q1Q单位=Q1((w1u +wu 1+u 1×u ),(w1w−u 1⋅u ))=(u 1,w1)

3.2 四元数默认在极坐标下

极坐标下的优势：使四元数的运算和向量的运算方法一致

由 4.2.1 复数在笛卡尔坐标和极坐标的转换方式可得，四元数在

笛卡尔坐标下为
Q ⃗ = ( x , y , z , w ) = ( u ⃗ , w ) \vec Q = (x, y, z, w) = (\vec u, w) Q =(x,y,z,w)=(u ,w)
极坐标下为，其中 θ \theta θ 为绕 u ⃗ \vec u u 旋转后的角位移（旋转方式见 4.2.3，推导到极坐标）
Q ⃗ = ( x s i n θ 2 , y s i n θ 2 , z s i n θ 2 , c o s θ 2 ) = ( u ⃗ s i n θ 2 , c o s θ 2 ) \vec Q = (x sin{\theta \over 2}, y sin{\theta \over 2},z sin{\theta \over 2}, cos{\theta \over 2}) = (\vec u sin{\theta \over 2}, cos{\theta \over 2}) Q =(xsin2θ,ysin2θ,zsin2θ,cos2θ)=(u sin2θ,cos2θ)

只有旋转轴 u 为单位向量时，下面公式才成立

用指数代替四元数：根据旋转角位移 $ \theta $ 和旋转轴 u 求出四元数
e ( u ⃗ θ 2 , 0 ) = ( u ⃗ s i n θ 2 , c o s θ 2 ) e^{(\vec u {\theta \over 2}, 0)} = (\vec usin{\theta \over 2}, cos{\theta \over 2}) e(u 2θ,0)=(u sin2θ,cos2θ)
对数：根据四元数和旋转轴 u 求出旋转角位移 θ \theta θ
l o g e ( （ u ⃗ s i n θ 2 , c o s θ 2 ） ) = ( u ⃗ θ 2 , 0 ) log_e(（\vec u sin{\theta \over 2}, cos{\theta \over 2}）) = (\vec u {\theta \over 2}, 0) loge(（u sin2θ,cos2θ）)=(u 2θ,0)
将点 P 绕 u ⃗ \vec u u 旋转 θ \theta θ 度
P 旋转后 = a P a − 1 = a P a ∗ , a = ( u ⃗ s i n θ 2 , c o s θ 2 ) P_{旋转后} = aPa^{-1} = aPa^*, a = (\vec u sin{\theta \over 2}, cos{\theta \over 2}) P旋转后=aPa−1=aPa∗,a=(u sin2θ,cos2θ)
将点 P 绕 u ⃗ \vec u u 旋转 θ \theta θ 度后再旋转 α \alpha α 度（方位的叠加是点乘）
P 旋转后 = b a P a − 1 b − 1 = ( b a ) P ( b a ) − 1 , a = ( u ⃗ s i n θ 2 , c o s θ 2 ) , b = ( u ⃗ s i n α 2 , c o s α 2 ) P_{旋转后} = baPa^{-1}b^{-1} = (ba)P(ba)^{-1},a = (\vec u sin{\theta \over 2},cos{\theta \over 2}),b = (\vec u sin{\alpha \over 2},cos{\alpha \over 2}) P旋转后=baPa−1b−1=(ba)P(ba)−1,a=(u sin2θ,cos2θ),b=(u sin2α,cos2α)
四元数求幂：四元数的 x 次幂等同于将它的旋转角缩放 x 倍
( u ⃗ s i n θ 2 , c o s θ 2 ) x = ( u ⃗ s i n ( θ 2 x ) s i n θ 2 , c o s ( θ 2 x ) ) (\vec u sin{\theta \over 2}, cos{\theta \over 2})^x = (\vec u {sin({\theta \over 2}x)\over sin{\theta \over 2}}, cos({\theta \over 2}x)) (u sin2θ,cos2θ)x=(u sin2θsin(2θx),cos(2θx))

四元数 * 向量 = 旋转后的向量，例：
设用四元数 q = (u, w)，u 为单位向量，旋转三维向量 v，则
p = ( v , 0 ) p ′ = q p q − 1 = q p q ∗ = ( u , w ) ( v , 0 ) ( − u , w ) = ( 2 ( u ⋅ v ) u + ( w 2 − u ⋅ v ) v + 2 w ( u × v ) , . . . ) = ( v ′ , . . . ) \begin{aligned} p &= (v, 0)\\ p'&= qpq^{-1} = qpq^* = (u,w)(v,0)(-u,w)\\ &= (2(u\cdot v)u + (w^2 -u\cdot v)v + 2w(u \times v),...)\\ &= (v',...) \end{aligned} pp′=(v,0)=qpq−1=qpq∗=(u,w)(v,0)(−u,w)=(2(u⋅v)u+(w2−u⋅v)v+2w(u×v),...)=(v′,...)

3.3 四元数的常用插值方法

所有插值用的旋转四元数都是单位四元数
插值要采用弧面最短路径

Q Q Q 和 − Q -Q −Q 代表不同的旋转角度得到的相同方位，在插值时会有不同的结果，如下图：可以将 q0 和 q1（蓝色的弧）插值，这会导致 3D 空间的向量旋转接近 360 度，而实际上这两个旋转相差并没有那么多，所以 q0 和 -q1（红色的弧）的插值才是插值的最短路径
每次插值前要通过 c o s θ = q 0 ⋅ q 1 cos\theta = q_0 \cdot q_1 cosθ=q0⋅q1 判断 q0 和 q1 的夹角是否为钝角，若为钝角将 q1 改为 -q1 来计算插值

线形插值（Lerp：Linear Interpolation）

对四元数插值后，得到的结果不是单位四元数，插值的弧度越大缺点越明显
公式： Q t = ( 1 − t ) Q 0 + t Q 1 , t Q_t = (1- t)Q_0 + t Q_1, t Qt=(1−t)Q0+tQ1,t 为插值比例

正规化线性插值（Nlerp：Normalized Linear Interpolation）

对四元数插值后，虽然把弦等分了，但是弦上对应的弧却不是等分的，插值的弧度越大缺点越明显
公式： Q t = ( 1 − t ) Q 0 + t Q 1 ∣ ∣ ( 1 − t ) Q 0 + t Q 1 ∣ ∣ , t Q_t = {{ (1- t)Q_0 + tQ_1}\over{||(1- t)Q_0 + tQ_1||}}, t Qt=∣∣(1−t)Q0+tQ1∣∣(1−t)Q0+tQ1,t 为插值比例

旋转角度球面线性插值（Slerp：Spherical Linear Interpolation）

对单个角度做线形插值，做到固定角速度，无法平滑过渡连接不同方向的角度
公式 1：这里用的四元数都是单位四元数，所以有 Q − 1 = Q ∗ , t Q^{-1} = Q^*, t Q−1=Q∗,t 为插值比例
Q t = Q 0 ( Q 0 − 1 Q 1 ) t = Q 0 ( Q 0 ∗ Q 1 ) t Q_t = Q_0(Q_0^{-1}Q_1)^t = Q_0(Q_0^* Q_1)^t Qt=Q0(Q0−1Q1)t=Q0(Q0∗Q1)t
公式 2：效率比公式 1 高，其中 θ = c o s − 1 ( Q 0 ⋅ Q 1 ) \theta = cos^{-1}(Q_0\cdot Q_1) θ=cos−1(Q0⋅Q1)
Q t = s i n ( ( 1 − t ) θ ) s i n θ Q 0 + s i n ( t θ ) s i n θ Q 1 Q_t = {sin((1 - t)\theta) \over sin\theta} Q_0 + {sin(t\theta) \over sin\theta}Q_1 Qt=sinθsin((1−t)θ)Q0+sinθsin(tθ)Q1

Slerp 和 Nlerp 的比较

效率上 Nlerp 比 Slerp 高
效果上 Nlerp 和 Slerp 在单位四元数之间的夹⻆ θ 非常小时差别不大，夹角越大 Nlerp 的效果越差
单位四元数之间的夹⻆ θ 非常小，那么 sinθ 可能会由于浮点数的误差被近似为 0.0，从而导致除以 0 的错误。我们在实施 Slerp 之前，需要检查两个四元数的夹⻆是否过小一旦发现这种问题，我们就必须改用 Nlerp 对两个四元数进行插值，这时候 Nlerp 的误差非常小所以基本不会与真正的 Slerp 有什么区别

3.4 贝塞尔曲线和 Squad 插值

样条（Spline）：在一个向量序列 v 0 , v 1 , . . . , v n v_0,v_1,...,v_n v0,v1,...,vn 中分别对每对向量 v i , v i + 1 v_i,v_{i+1} vi,vi+1 进行插值后互相连接得到的曲线

贝塞尔曲线（Bézier）：通过不断在现有点的基础上添加控制点，使最终得到的曲线更加平滑

三次贝塞尔曲线：

插值方式可以用 lerp、Slerp 等方式，上图采用 de Casteljau 算法构造贝塞尔曲线
上图采用 lerp 方式插值，插值方程为：
v t = ( 1 − t ) 3 v 0 + 3 ( 1 − t ) 2 t v 1 + 3 ( 1 − t ) t 2 v 2 + t 3 v 3 v_t = (1 − t)^3v_0 + 3(1 − t)^2tv_1 + 3(1 − t)t^2v_2 + t^3v_3 vt=(1−t)3v0+3(1−t)2tv1+3(1−t)t2v2+t3v3
d e C a s t e l j a u de Casteljau deCasteljau 算法构造贝塞尔曲线的过程为：

第一次贝塞尔曲线，由相邻的基础点得到插值点 v 01 、 v 12 、 v 23 v_{01}、v_{12}、v_{23} v01、v12、v23
第二次贝塞尔曲线，由上次贝塞尔的插值点得到本次的插值点 v 012 、 v 123 v_{012}、v_{123} v012、v123
第三次贝塞尔曲线，由上次贝塞尔的插值点得到本次的插值点 v 0123 v_{0123} v0123

球面四边形插值（Squad：Spherical and Quadrangle）

平滑过渡连接不同方向的旋转角，效率最低，效果最好
Squad 的插值方法类似贝塞尔曲线的构造过程，由于取基础点的方式不同，效率比构造贝塞尔曲线要高
Squad 的插值过程中可以用 lerp、Slerp 等方式插值
如果使用 lerp 插值，插值参数为 2t(1-t)，而非 t
插值方程为：

v t = ( 2 t 2 − 2 t + 1 ) ( 1 − t ) v 0 + 2 ( 1 − t ) 2 t v 1 + 2 ( 1 − t ) t 2 v 2 + t ( 2 t 2 − 2 t + 1 ) v 3 v_t = (2t^2 − 2t + 1)(1 − t)v_0 + 2(1 − t)^2tv_1 + 2(1 − t)t^2v_2 + t(2t^2 − 2t + 1)v_3 vt=(2t2−2t+1)(1−t)v0+2(1−t)2tv1+2(1−t)t2v2+t(2t2−2t+1)v3

插值步骤为：

由基础点得到插值点 v 12 ， v 03 v_{12}，v_{03} v12，v03
根据上次的插值点得出本次的插值点 v 0312 v_{0312} v0312

三次贝塞尔曲线和 Squad 插值构造的曲线对比：

4 欧拉角、旋转矩阵、四元数的互相转换

4.1 欧拉角和旋转矩阵

欧拉角 → \to → 旋转矩阵

这里的旋转矩阵和Rotate类似，这里的欧拉角操作的模型的坐标系
设在右手坐标系，矩阵列向量存储，旋转角逆时针为正方向（改变的角度方向取反），则
最后的转换矩阵为 Heading/Pich/Roll 按需要的顺序相乘的结果（HPR 为相机避免万向死锁的最佳顺序）
H e a d i n g = [ c o s H 0 s i n H 0 1 0 − s i n H 0 c o s H ] P i c h = [ 1 0 0 0 c o s P − s i n P 0 s i n P c o s P ] R o l l = [ c o s R − s i n R 0 s i n R c o s R 0 0 0 1 ] M H P R = [ c o s H c o s R + s i n H s i n P s i n R − c o s H s i n R + s i n H s i n P c o s R s i n H c o s P s i n R c o s P c o s R c o s P − s i n P − s i n H c o s R + c o s H s i n P s i n R s i n R s i n H + c o s H s i n P c o s R c o s H c o s P ] \begin{aligned} Heading &= \begin{bmatrix} cosH & 0 & sinH\\ 0 & 1 & 0\\ -sinH & 0 & cosH \end{bmatrix} \\ Pich &= \begin{bmatrix} 1 & 0 & 0\\ 0 & cosP & -sinP\\ 0 & sinP & cosP \end{bmatrix} \\ Roll &= \begin{bmatrix} cosR & -sinR & 0\\ sinR & cosR & 0\\ 0 & 0 & 1 \end{bmatrix} \\ M_{HPR} &= \begin{bmatrix} cosHcosR+sinHsinPsinR & -cosHsinR+sinHsinPcosR & sinHcosP \\ sinRcosP & cosRcosP & -sinP\\ -sinHcosR+cosHsinPsinR & sinRsinH+cosHsinPcosR & cosHcosP \end{bmatrix} \end{aligned}\\ HeadingPichRollMHPR=⎣⎡cosH0−sinH010sinH0cosH⎦⎤=⎣⎡1000cosPsinP0−sinPcosP⎦⎤=⎣⎡cosRsinR0−sinRcosR0001⎦⎤=⎣⎡cosHcosR+sinHsinPsinRsinRcosP−sinHcosR+cosHsinPsinR−cosHsinR+sinHsinPcosRcosRcosPsinRsinH+cosHsinPcosRsinHcosP−sinPcosHcosP⎦⎤

旋转矩阵 → \to → 欧拉角

转换后的欧拉角是限制欧拉角，即 Heading 和 Roll 范围为 $\pm$180 度，Pich 的范围为 90 度

当矩阵每列都是单位向量时，矩阵为正交矩阵，则
M ⋅ M − 1 = M ⋅ M T = I M − 1 = M T M \cdot M^{-1} = M \cdot M^T = I\\ M^{-1} = M^T\\ M⋅M−1=M⋅MT=IM−1=MT

若 P i c h ≠ ± 90 Pich \neq \pm 90 Pich=±90，由欧拉角转矩阵的公式可得：

[ m 11 m 12 m 13 m 21 m 22 m 23 m 31 m 32 m 33 ] = [ c o s H c o s R + s i n H s i n P s i n R − c o s H s i n R + s i n H s i n P c o s R s i n H c o s P s i n R c o s P c o s R c o s P − s i n P − s i n H c o s R + c o s H s i n P s i n R s i n R s i n H + c o s H s i n P c o s R c o s H c o s P ] m 23 = − s i n P a r c s i n ( − m 23 ) = P m 13 = s i n H c o s P m 33 = c o s H c o s P m 13 m 33 = t a n H a r c t a n ( m 13 m 33 ) = H m 21 = s i n R c o s P m 22 = c o s R c o s P a r c t a n ( m 21 m 22 ) = R \begin{aligned} \begin{bmatrix} m11 & m12 & m13\\ m21 & m22 & m23\\ m31 & m32 & m33 \end{bmatrix} &= \begin{bmatrix} cosHcosR+sinHsinPsinR & -cosHsinR+sinHsinPcosR & sinHcosP \\ sinRcosP & cosRcosP & -sinP\\ -sinHcosR+cosHsinPsinR & sinRsinH+cosHsinPcosR & cosHcosP \end{bmatrix}\\ \\ m23 &= -sinP\\ arcsin(-m23) &= P\\ \\ m13 &= sinHcosP\\ m33 &= cosHcosP\\ {m13 \over m33} &= tanH\\ arctan({m13 \over m33}) &= H\\ \\ m21 &= sinRcosP\\ m22 &= cosRcosP\\ arctan({m21 \over m22}) &= R\\ \end{aligned} ⎣⎡m11m21m31m12m22m32m13m23m33⎦⎤m23arcsin(−m23)m13m33m33m13arctan(m33m13)m21m22arctan(m22m21)=⎣⎡cosHcosR+sinHsinPsinRsinRcosP−sinHcosR+cosHsinPsinR−cosHsinR+sinHsinPcosRcosRcosPsinRsinH+cosHsinPcosRsinHcosP−sinPcosHcosP⎦⎤=−sinP=P=sinHcosP=cosHcosP=tanH=H=sinRcosP=cosRcosP=R

若 P i c h = ± 90 Pich = \pm 90 Pich=±90，是万向锁情况，此时 Heading 和 Roll 绕同样的轴竖直旋转，默认旋转的最后一步 Roll 不转，即 Roll = 0 ，由欧拉角转矩阵的公式可得：

[ m 11 m 12 m 13 m 21 m 22 m 23 m 31 m 32 m 33 ] = [ c o s H s i n H s i n P 0 0 0 − s i n P − s i n H c o s H s i n P 0 ] m 11 = c o s H m 13 = − s i n H − a r c t a n ( m 13 m 11 ) = H \begin{aligned} \begin{bmatrix} m11 & m12 & m13\\ m21 & m22 & m23\\ m31 & m32 & m33 \end{bmatrix} &= \begin{bmatrix} cosH & sinHsinP & 0 \\ 0 & 0 & -sinP\\ -sinH & cosHsinP & 0 \end{bmatrix}\\ \\ m11 &= cosH\\ m13 &= -sinH\\ -arctan({m13\over m11}) &= H \end{aligned} ⎣⎡m11m21m31m12m22m32m13m23m33⎦⎤m11m13−arctan(m11m13)=⎣⎡cosH0−sinHsinHsinP0cosHsinP0−sinP0⎦⎤=cosH=−sinH=H

4.2 四元数和旋转矩阵

四元数 → \to → 旋转矩阵

设四元数为 Q ⃗ = ( n ⃗ , c o s θ 2 ) = ( x , y , z , w ) \vec Q = (\vec n, cos{ \theta \over 2 }) = (x,y,z,w) Q =(n ,cos2θ)=(x,y,z,w)，绕 n 旋转 θ \theta θ ，由Rotate沿任意方向旋转的矩阵得旋转矩阵 M：
[ ( 1 − c o s θ ) x 2 + c o s θ ( 1 − c o s θ ) y x + s i n θ z ( 1 − c o s θ ) z x − s i n θ y ( 1 − c o s θ ) x y − s i n θ z ( 1 − c o s θ ) y 2 + c o s θ − ( 1 − c o s θ ) z y + s i n θ x ( 1 − c o s θ ) x z + s i n θ y ( 1 − c o s θ ) y z − s i n θ x ( 1 − c o s θ ) z 2 + c o s θ ] → M = [ 1 − 2 y 2 − 2 z 2 2 x y + 2 w z 2 x z − 2 w y 2 x y − 2 w z 1 − 2 x 2 − 2 z 2 2 y z + 2 w x 2 x z + 2 w y 2 y z − 2 w x 1 − 2 x 2 − 2 y 2 ] \begin{bmatrix} (1-cos\theta)x^2 + cos\theta & (1-cos\theta)yx+sin\theta z & (1-cos\theta)zx - sin\theta y\\ (1-cos\theta)xy - sin\theta z & (1-cos\theta)y^2 + cos\theta & -(1-cos\theta)zy + sin\theta x\\ (1-cos\theta)xz + sin\theta y & (1-cos\theta)yz - sin\theta x & (1-cos\theta)z^2 + cos\theta \end{bmatrix} \to M = \begin{bmatrix} 1-2y^2-2z^2 & 2xy+2wz & 2xz-2wy\\ 2xy-2wz & 1-2x^2-2z^2 & 2yz+2wx\\ 2xz+2wy & 2yz-2wx & 1-2x^2-2y^2 \end{bmatrix} ⎣⎡(1−cosθ)x2+cosθ(1−cosθ)xy−sinθz(1−cosθ)xz+sinθy(1−cosθ)yx+sinθz(1−cosθ)y2+cosθ(1−cosθ)yz−sinθx(1−cosθ)zx−sinθy−(1−cosθ)zy+sinθx(1−cosθ)z2+cosθ⎦⎤→M=⎣⎡1−2y2−2z22xy−2wz2xz+2wy2xy+2wz1−2x2−2z22yz−2wx2xz−2wy2yz+2wx1−2x2−2y2⎦⎤

旋转矩阵 → \to →四元数

由四元数转旋转矩阵可知：

[ m 11 m 12 m 13 m 21 m 22 m 23 m 31 m 32 m 33 ] = [ 1 − 2 y 2 − 2 z 2 2 x y + 2 w z 2 x z − 2 w y 2 x y − 2 w z 1 − 2 x 2 − 2 z 2 2 y z + 2 w x 2 x z + 2 w y 2 y z − 2 w x 1 − 2 x 2 − 2 y 2 ] m 11 + m 22 + m 33 = ( 1 − 2 y 2 − 2 z 2 ) + ( 1 − 2 x 2 − 2 z 2 ) + ( 1 − 2 x 2 − 2 y 2 ) = 3 − 4 ( x 2 + y 2 + z 2 ) = 3 − 4 ( 1 − w 2 ) = 4 w 2 − 1 w = m 11 + m 22 + m 33 + 1 2 \begin{aligned} \begin{bmatrix} m11 & m12 & m13\\ m21 & m22 & m23\\ m31 & m32 & m33 \end{bmatrix} &= \begin{bmatrix} 1-2y^2-2z^2 & 2xy+2wz & 2xz-2wy\\ 2xy-2wz & 1-2x^2-2z^2 & 2yz+2wx\\ 2xz+2wy & 2yz-2wx & 1-2x^2-2y^2 \end{bmatrix}\\\\ m11+m22+m33 &= (1-2y^2-2z^2)+(1-2x^2-2z^2)+(1-2x^2-2y^2)\\ &= 3-4(x^2+y^2+z^2)\\ &= 3-4(1-w^2)\\ &= 4w^2-1\\ \\ w&={\sqrt{m11+m22+m33+1} \over 2} \end{aligned} ⎣⎡m11m21m31m12m22m32m13m23m33⎦⎤m11+m22+m33w=⎣⎡1−2y2−2z22xy−2wz2xz+2wy2xy+2wz1−2x2−2z22yz−2wx2xz−2wy2yz+2wx1−2x2−2y2⎦⎤=(1−2y2−2z2)+(1−2x2−2z2)+(1−2x2−2y2)=3−4(x2+y2+z2)=3−4(1−w2)=4w2−1=2m11+m22+m33+1

使 m11、m22、m33 其中两个为负，一个为正可得：

x = m 11 − m 22 − m 33 + 1 2 y = − m 11 + m 22 − m 33 + 1 2 z = − m 11 − m 22 + m 33 + 1 2 \begin{aligned} x &={\sqrt{m11-m22-m33+1} \over 2}\\ y &={\sqrt{-m11+m22-m33+1} \over 2}\\ z &={\sqrt{-m11-m22+m33+1} \over 2} \end{aligned} xyz=2m11−m22−m33+1 =2−m11+m22−m33+1 =2−m11−m22+m33+1

以上方法得到的四元数总是正的，没有判断四元数符号的依据

在由：

m 12 + m 21 = 4 x y m 12 − m 21 = 4 w z m 31 + m 13 = 4 x z m 31 − m 13 = 4 w y m 23 + m 32 = 4 y z m 23 − m 32 = 4 w x \begin{aligned} m12 + m21 &= 4xy\\ m12 - m21 &= 4wz\\ m31 + m13 &= 4xz\\ m31 - m13 &= 4wy\\ m23 + m32 &= 4yz\\ m23 - m32 &= 4wx\\ \end{aligned} m12+m21m12−m21m31+m13m31−m13m23+m32m23−m32=4xy=4wz=4xz=4wy=4yz=4wx

综上可得：

x = m 23 − m 32 4 w 情况一、 w = m 11 + m 22 + m 33 + 1 2 → y = m 31 − m 13 4 w z = m 12 − m 21 4 w w = m 23 − m 32 4 x 情况二、 x = m 11 − m 22 − m 33 + 1 2 → y = m 12 + m 21 4 x z = m 31 + m 13 4 x w = m 31 − m 13 4 y 情况三、 y = − m 11 + m 22 − m 33 + 1 2 → x = m 12 + m 21 4 y z = m 23 + m 32 4 y w = m 12 − m 21 4 z 情况四、 z = − m 11 − m 22 + m 33 + 1 2 → x = m 31 + m 13 4 z y = m 23 + m 32 4 z \begin{aligned} x &= {m23-m32 \over 4w}\\ 情况一、w ={\sqrt{m11+m22+m33+1} \over 2} \to y &= {m31-m13 \over 4w}\\ z &= {m12-m21 \over 4w}\\ \\ w &= {m23-m32 \over 4x}\\ 情况二、x ={\sqrt{m11-m22-m33+1} \over 2} \to y &= {m12+m21 \over 4x}\\ z &= {m31+m13 \over 4x}\\ \\ w &= {m31-m13 \over 4y}\\ 情况三、y ={\sqrt{-m11+m22-m33+1} \over 2} \to x &= {m12+m21 \over 4y}\\ z &= {m23+m32 \over 4y}\\ \\ w &= {m12-m21 \over 4z}\\ 情况四、z ={\sqrt{-m11-m22+m33+1} \over 2} \to x &= {m31+m13 \over 4z}\\ y &= {m23+m32 \over 4z} \end{aligned} x情况一、w=2m11+m22+m33+1 →yzw情况二、x=2m11−m22−m33+1 →yzw情况三、y=2−m11+m22−m33+1 →xzw情况四、z=2−m11−m22+m33+1 →xy=4wm23−m32=4wm31−m13=4wm12−m21=4xm23−m32=4xm12+m21=4xm31+m13=4ym31−m13=4ym12+m21=4ym23+m32=4zm12−m21=4zm31+m13=4zm23+m32

取 1、2 中得到的 w、x、y、z 的最大值是哪个来判断，选择哪种情况

4.3 欧拉角和四元数

四元数 → \to → 欧拉角

由旋转矩阵转四元数和旋转矩阵转欧拉角的条件可得：
P = a s i n ( − 2 ( y z + w x ) ) H = { a t a n 2 ( x z − w y , 0.5 − x 2 − y 2 ) , cos ⁡ P ≠ 0 a t a n 2 ( − x z − w y , 0.5 − y 2 − z 2 ) , cos ⁡ P = 0 R = { a t a n 2 ( x y − w z , 0.5 − x 2 − z 2 ) , cos ⁡ P ≠ 0 0 , cos ⁡ P = 0 \begin{aligned} P &= asin(-2(yz+wx)) \\ H &= \begin{cases} atan2(xz-wy, 0.5 - x^2 - y^2),& \cos P \neq 0 \\ atan2(-xz-wy, 0.5 - y^2 - z^2),& \cos P = 0 \end{cases}\\ R &= \begin{cases} atan2(xy-wz,0.5 - x^2 - z^2),& \cos P \neq 0\\ 0,& \cos P = 0 \end{cases} \end{aligned} PHR=asin(−2(yz+wx))={atan2(xz−wy,0.5−x2−y2),atan2(−xz−wy,0.5−y2−z2),cosP=0cosP=0={atan2(xy−wz,0.5−x2−z2),0,cosP=0cosP=0

欧拉角 → \rightarrow → 四元数

由四元数的公式得，在右手坐标系下的欧拉角列向量 H、P、R 为：

H P R = [ c o s H 2 0 − s i n H 2 0 ] [ c o s P 2 − s i n H 2 0 0 ] [ c o s R 2 0 0 − s i n H 2 ] = [ c o s H 2 c o s P 2 c o s R 2 + s i n H 2 s i n P 2 s i n R 2 − c o s H 2 s i n P 2 c o s R 2 − s i n H 2 c o s P 2 s i n R 2 c o s H 2 s i n P 2 s i n R 2 − s i n H 2 c o s P 2 c o s R 2 s i n H 2 s i n P 2 c o s R 2 − c o s H 2 c o s P 2 s i n R 2 ] HPR= \begin{bmatrix} cos{H \over 2}\\\\ 0 \\-sin{H \over 2}\\ 0 \end{bmatrix} \begin{bmatrix} cos{P \over 2}\\\\ -sin{H \over 2} \\0 \\0 \end{bmatrix} \begin{bmatrix} cos{R \over 2}\\\\ 0\\ 0\\-sin{H \over 2} \end{bmatrix} = \begin{bmatrix} cos{H \over 2}cos{P \over 2}cos{R \over 2}+sin{H \over 2}sin{P \over 2}sin{R \over 2}\\\\ -cos{H \over 2}sin{P \over 2}cos{R \over 2}-sin{H \over 2}cos{P \over 2}sin{R \over 2} \\\\ cos{H \over 2}sin{P \over 2}sin{R \over 2}-sin{H \over 2}cos{P \over 2}cos{R \over 2} \\\\ sin{H \over 2}sin{P \over 2}cos{R \over 2}-cos{H \over 2}cos{P \over 2}sin{R \over 2} \end{bmatrix} HPR=⎣⎢⎢⎢⎢⎡cos2H0−sin2H0⎦⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎡cos2P−sin2H00⎦⎥⎥⎥⎥⎤⎣⎢⎢⎢⎢⎡cos2R00−sin2H⎦⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎡cos2Hcos2Pcos2R+sin2Hsin2Psin2R−cos2Hsin2Pcos2R−sin2Hcos2Psin2Rcos2Hsin2Psin2R−sin2Hcos2Pcos2Rsin2Hsin2Pcos2R−cos2Hcos2Psin2R⎦⎥⎥⎥⎥⎥⎥⎥⎥⎤

5. SQT 变换

四元数只能表示旋转，但是 4 X 4 的矩阵却可以表示旋转、平移、缩放

SQT 变换矩阵：四元数、平移向量、缩放向量/缩放统一系数构成的一个 4 X 3 的矩阵

使用 SQT 矩阵的目的：便于对旋转、平移、缩放的插值计算

码农公寓