其他分享
首页 > 其他分享> > 3D 中的方位与角位移(旋转矩阵、欧拉角、四元数)

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

作者：互联网

文章目录

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

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

1. 欧拉角 (Euler angles)

定义：

在这里插入图片描述

优点：

缺点：

万向锁：

万向锁
在这里插入图片描述

发生万向锁后的旋转
在这里插入图片描述

期望的旋转
在这里插入图片描述

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

在这里插入图片描述

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
在极坐标下，复数能够更方便的表示旋转角的变化：

  1. 连续的旋转可以用两个复数的乘积表示
    例： 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} e1​e2​e​=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. 在不知道当前角位移的情况下，可以通过当前方位+角位移计算出之后的方位

2.3 三维空间旋转的拆分

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

在这里插入图片描述

拆分轴角式旋转：

  1. 如下图 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​

  2. 如下图 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θ​

  3. 结合 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 中的一个旋转对应正向和反向旋转的两个四元数，不是一一对应的

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

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)​

优点：

缺点：

3.1 四元数的运算

3.2 四元数默认在极坐标下

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

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

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

四元数 * 向量 = 旋转后的向量，例：
设 用四元数 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 四元数的常用插值方法

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

在这里插入图片描述

线形插值（Lerp：Linear Interpolation）

在这里插入图片描述

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

在这里插入图片描述

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

在这里插入图片描述

Slerp 和 Nlerp 的比较

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 算法构造贝塞尔曲线的过程为：

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

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​

插值步骤为：

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

在这里插入图片描述

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

在这里插入图片描述

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

4.1 欧拉角和旋转矩阵

欧拉角 → \to → 旋转矩阵

在这里插入图片描述

旋转矩阵 → \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

  1. 若 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} ⎣⎡​m11m21m31​m12m22m32​m13m23m33​⎦⎤​m23arcsin(−m23)m13m33m33m13​arctan(m33m13​)m21m22arctan(m22m21​)​=⎣⎡​cosHcosR+sinHsinPsinRsinRcosP−sinHcosR+cosHsinPsinR​−cosHsinR+sinHsinPcosRcosRcosPsinRsinH+cosHsinPcosR​sinHcosP−sinPcosHcosP​⎦⎤​=−sinP=P=sinHcosP=cosHcosP=tanH=H=sinRcosP=cosRcosP=R​

  1. 若 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} ⎣⎡​m11m21m31​m12m22m32​m13m23m33​⎦⎤​m11m13−arctan(m11m13​)​=⎣⎡​cosH0−sinH​sinHsinP0cosHsinP​0−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+2wy​2xy+2wz1−2x2−2z22yz−2wx​2xz−2wy2yz+2wx1−2x2−2y2​⎦⎤​

旋转矩阵 → \to →四元数

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

[ 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} ⎣⎡​m11m21m31​m12m22m32​m13m23m33​⎦⎤​m11+m22+m33w​=⎣⎡​1−2y2−2z22xy−2wz2xz+2wy​2xy+2wz1−2x2−2z22yz−2wx​2xz−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 ​​​

  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 ​​​

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

  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​

  1. 综上可得：

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. 取 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=⎣⎢⎢⎢⎢⎡​cos2H​0−sin2H​0​⎦⎥⎥⎥⎥⎤​⎣⎢⎢⎢⎢⎡​cos2P​−sin2H​00​⎦⎥⎥⎥⎥⎤​⎣⎢⎢⎢⎢⎡​cos2R​00−sin2H​​⎦⎥⎥⎥⎥⎤​=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎡​cos2H​cos2P​cos2R​+sin2H​sin2P​sin2R​−cos2H​sin2P​cos2R​−sin2H​cos2P​sin2R​cos2H​sin2P​sin2R​−sin2H​cos2P​cos2R​sin2H​sin2P​cos2R​−cos2H​cos2P​sin2R​​⎦⎥⎥⎥⎥⎥⎥⎥⎥⎤​

在这里插入图片描述

5. SQT 变换

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

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

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

引用

标签：cos,欧拉角,角位移,over,四元,vec,theta,sin,3D
来源： https://blog.csdn.net/Sun_Raiser/article/details/122764548