UE5中的四元数
UE5中的四元数
- 绕任意轴旋转
- 四元数与矩阵
- 四元数与欧拉角
- 将一个向量旋转到另一个向量
- 插值
- Reference
我们知道，四元数是除了欧拉角，旋转矩阵之外，主要用来描述旋转的量。四元数直观的定义就是
q
=
[
c
o
s
(
θ
2
)
,
s
i
n
(
θ
2
)
N
]
q = [cos(\frac{\theta}{2}), sin(\frac{\theta}{2})N]
q=[cos(2θ),sin(2θ)N]
其中，N表示旋转轴单位向量，
θ
\theta
θ表示旋转的角度。那么，给定一个任意向量V，绕N旋转
θ
\theta
θ角度之后的V’可以用四元数乘法表示。令四元数v = [0, V]，v’ = [0, V’]，那么
v
′
=
q
v
q
∗
v' = qvq^*
v′=qvq∗
我们先来推导一下这个公式。
绕任意轴旋转
首先，四元数的乘法公式为
q
1
=
[
s
,
v
]
q_1 = [s, \textbf{v}]
q1=[s,v]
q 2 = [ t , u ] q_2 = [t, \textbf{u}] q2=[t,u]
q 1 q 2 = [ s t − v ⋅ u , s u + t v + v × u ] q_1q_2 = [st - \textbf{v} \cdot \textbf{u}, s\textbf{u} + t\textbf{v} + \textbf{v} \times \textbf{u}] q1q2=[st−v⋅u,su+tv+v×u]
那么
v
q
∗
=
[
s
i
n
(
θ
2
)
N
⋅
V
,
c
o
s
(
θ
2
)
V
+
s
i
n
(
θ
2
)
N
×
V
]
vq^* = [sin(\frac{\theta}{2})N \cdot V, cos(\frac{\theta}{2}) V + sin(\frac{\theta}{2}) N \times V]
vq∗=[sin(2θ)N⋅V,cos(2θ)V+sin(2θ)N×V]
q v q ∗ = [ s i n ( θ 2 ) c o s ( θ 2 ) N ⋅ V − s i n ( θ 2 ) N ⋅ ( c o s ( θ 2 ) V + s i n ( θ 2 ) N × V ) , c o s ( θ 2 ) ( c o s ( θ 2 ) V + s i n ( θ 2 ) N × V ) + s i n 2 ( θ 2 ) ( N ⋅ V ) N + s i n ( θ 2 ) N × ( c o s ( θ 2 ) V + s i n ( θ 2 ) N × V ) ] qvq^* = [ sin(\frac{\theta}{2})cos(\frac{\theta}{2})N \cdot V - sin(\frac{\theta}{2})N \cdot (cos(\frac{\theta}{2}) V + sin(\frac{\theta}{2}) N \times V), \\ cos(\frac{\theta}{2})(cos(\frac{\theta}{2}) V + sin(\frac{\theta}{2}) N \times V) + sin^2(\frac{\theta}{2})(N \cdot V)N + sin(\frac{\theta}{2}) N \times (cos(\frac{\theta}{2}) V + sin(\frac{\theta}{2}) N \times V) ] qvq∗=[sin(2θ)cos(2θ)N⋅V−sin(2θ)N⋅(cos(2θ)V+sin(2θ)N×V),cos(2θ)(cos(2θ)V+sin(2θ)N×V)+sin2(2θ)(N⋅V)N+sin(2θ)N×(cos(2θ)V+sin(2θ)N×V)]
我们先对结果的实部进行化简，因为点乘满足分配律，所以有
s
i
n
(
θ
2
)
c
o
s
(
θ
2
)
N
⋅
V
−
s
i
n
(
θ
2
)
N
⋅
(
c
o
s
(
θ
2
)
V
+
s
i
n
(
θ
2
)
N
×
V
)
=
s
i
n
(
θ
2
)
c
o
s
(
θ
2
)
N
⋅
V
−
s
i
n
(
θ
2
)
c
o
s
(
θ
2
)
N
⋅
V
−
s
i
n
2
(
θ
2
)
N
⋅
(
N
×
V
)
=
−
s
i
n
2
(
θ
2
)
N
⋅
(
N
×
V
)
sin(\frac{\theta}{2})cos(\frac{\theta}{2})N \cdot V - sin(\frac{\theta}{2})N \cdot (cos(\frac{\theta}{2}) V + sin(\frac{\theta}{2}) N \times V) \\ = sin(\frac{\theta}{2})cos(\frac{\theta}{2})N \cdot V - sin(\frac{\theta}{2})cos(\frac{\theta}{2})N \cdot V - sin^2(\frac{\theta}{2})N \cdot (N \times V) \\ = - sin^2(\frac{\theta}{2})N \cdot (N \times V)
sin(2θ)cos(2θ)N⋅V−sin(2θ)N⋅(cos(2θ)V+sin(2θ)N×V)=sin(2θ)cos(2θ)N⋅V−sin(2θ)cos(2θ)N⋅V−sin2(2θ)N⋅(N×V)=−sin2(2θ)N⋅(N×V)
N和V的叉乘显然与N垂直，所以点积为0，因此
− s i n 2 ( θ 2 ) N ⋅ ( N × V ) = 0 -sin^2(\frac{\theta}{2})N \cdot (N \times V) = 0 −sin2(2θ)N⋅(N×V)=0
再看虚部，同样叉乘也满足分配律，所以有
c o s ( θ 2 ) ( c o s ( θ 2 ) V + s i n ( θ 2 ) N × V ) + s i n 2 ( θ 2 ) ( N ⋅ V ) N + s i n ( θ 2 ) N × ( c o s ( θ 2 ) V + s i n ( θ 2 ) N × V ) = c o s 2 ( θ 2 ) V + s i n ( θ 2 ) c o s ( θ 2 ) N × V + s i n 2 ( θ 2 ) ( N ⋅ V ) N + s i n ( θ 2 ) c o s ( θ 2 ) N × V + s i n 2 ( θ 2 ) N × ( N × V ) cos(\frac{\theta}{2})(cos(\frac{\theta}{2}) V + sin(\frac{\theta}{2}) N \times V) + sin^2(\frac{\theta}{2})(N \cdot V)N + sin(\frac{\theta}{2}) N \times (cos(\frac{\theta}{2}) V + sin(\frac{\theta}{2}) N \times V) \\ = cos^2(\frac{\theta}{2})V + sin(\frac{\theta}{2})cos(\frac{\theta}{2})N \times V + sin^2(\frac{\theta}{2})(N \cdot V)N + sin(\frac{\theta}{2})cos(\frac{\theta}{2}) N \times V + sin^2(\frac{\theta}{2})N \times (N \times V) cos(2θ)(cos(2θ)V+sin(2θ)N×V)+sin2(2θ)(N⋅V)N+sin(2θ)N×(cos(2θ)V+sin(2θ)N×V)=cos2(2θ)V+sin(2θ)cos(2θ)N×V+sin2(2θ)(N⋅V)N+sin(2θ)cos(2θ)N×V+sin2(2θ)N×(N×V)
注意，叉乘并不满足结合律，且有
a
×
(
b
×
c
)
=
(
a
⋅
c
)
b
−
(
a
⋅
b
)
⋅
c
\textbf{a} \times (\textbf{b} \times \textbf{c}) = (\textbf{a} \cdot \textbf{c})\textbf{b} - (\textbf{a} \cdot \textbf{b}) \cdot \textbf{c}
a×(b×c)=(a⋅c)b−(a⋅b)⋅c
所以上式进一步化简得到
c
o
s
2
(
θ
2
)
V
+
2
s
i
n
(
θ
2
)
c
o
s
(
θ
2
)
N
×
V
+
s
i
n
2
(
θ
2
)
(
N
⋅
V
)
N
+
s
i
n
2
(
θ
2
)
(
(
N
⋅
V
)
N
−
V
)
=
(
c
o
s
2
(
θ
2
)
−
s
i
n
2
(
θ
2
)
)
V
+
2
s
i
n
2
(
θ
2
)
(
(
N
⋅
V
)
N
+
2
s
i
n
(
θ
2
)
c
o
s
(
θ
2
)
N
×
V
=
c
o
s
θ
V
+
(
1
−
c
o
s
θ
)
(
N
⋅
V
)
N
+
s
i
n
θ
(
N
×
V
)
cos^2(\frac{\theta}{2})V + 2sin(\frac{\theta}{2})cos(\frac{\theta}{2})N \times V + sin^2(\frac{\theta}{2})(N \cdot V)N + sin^2(\frac{\theta}{2})((N \cdot V)N - V) \\ = (cos^2(\frac{\theta}{2}) - sin^2(\frac{\theta}{2}))V + 2sin^2(\frac{\theta}{2})((N \cdot V)N + 2sin(\frac{\theta}{2})cos(\frac{\theta}{2})N \times V \\ = cos\theta V + (1- cos\theta)(N \cdot V)N + sin\theta(N \times V)
cos2(2θ)V+2sin(2θ)cos(2θ)N×V+sin2(2θ)(N⋅V)N+sin2(2θ)((N⋅V)N−V)=(cos2(2θ)−sin2(2θ))V+2sin2(2θ)((N⋅V)N+2sin(2θ)cos(2θ)N×V=cosθV+(1−cosθ)(N⋅V)N+sinθ(N×V)
不难发现，这个就是绕任意轴旋转的公式。
当然，在真正实现中，没必要按部就班地用这种方式计算。UE实现用四元数旋转任意向量的代码如下：
template<typename T>
FORCEINLINE TVector<T> TQuat<T>::RotateVector(TVector<T> V) const
{
// http://people.csail.mit.edu/bkph/articles/Quaternions.pdf
// V' = V + 2w(Q x V) + (2Q x (Q x V))
// refactor:
// V' = V + w(2(Q x V)) + (Q x (2(Q x V)))
// T = 2(Q x V);
// V' = V + w*(T) + (Q x T)
const TVector<T> Q(X, Y, Z);
const TVector<T> TT = 2.f * TVector<T>::CrossProduct(Q, V);
const TVector<T> Result = V + (W * TT) + TVector<T>::CrossProduct(Q, TT);
return Result;
}
UE所用的其实就是上述公式的反推。推导如下：
c
o
s
θ
V
+
(
1
−
c
o
s
θ
)
(
N
⋅
V
)
N
+
s
i
n
θ
(
N
×
V
)
=
V
−
2
s
i
n
2
(
θ
2
)
V
+
2
s
i
n
2
(
θ
2
)
(
N
⋅
V
)
N
+
2
s
i
n
(
θ
2
)
c
o
s
(
θ
2
)
(
N
×
V
)
=
V
−
2
s
i
n
2
(
θ
2
)
V
+
2
(
Q
⋅
V
)
Q
+
2
w
(
Q
×
V
)
=
V
−
2
s
i
n
2
(
θ
2
)
V
+
2
(
Q
×
(
Q
×
V
)
+
(
Q
⋅
Q
)
V
)
+
2
w
(
Q
×
V
)
=
V
−
2
s
i
n
2
(
θ
2
)
V
+
2
Q
×
(
Q
×
V
)
+
2
s
i
n
2
(
θ
2
)
V
+
2
w
(
Q
×
V
)
=
V
+
2
w
(
Q
×
V
)
+
2
Q
×
(
Q
×
V
)
cos\theta V + (1- cos\theta)(N \cdot V)N + sin\theta(N \times V) \\ = V -2sin^2(\frac{\theta}{2})V + 2sin^2(\frac{\theta}{2})(N \cdot V)N + 2sin(\frac{\theta}{2})cos(\frac{\theta}{2})(N \times V) \\ = V -2sin^2(\frac{\theta}{2})V + 2(Q \cdot V)Q + 2w(Q \times V) \\ = V -2sin^2(\frac{\theta}{2})V + 2(Q \times (Q \times V) + (Q \cdot Q)V) + 2w(Q \times V) \\ = V -2sin^2(\frac{\theta}{2})V + 2Q \times (Q \times V) + 2sin^2(\frac{\theta}{2})V + 2w(Q \times V) \\ = V + 2w(Q \times V) + 2Q \times (Q \times V)
cosθV+(1−cosθ)(N⋅V)N+sinθ(N×V)=V−2sin2(2θ)V+2sin2(2θ)(N⋅V)N+2sin(2θ)cos(2θ)(N×V)=V−2sin2(2θ)V+2(Q⋅V)Q+2w(Q×V)=V−2sin2(2θ)V+2(Q×(Q×V)+(Q⋅Q)V)+2w(Q×V)=V−2sin2(2θ)V+2Q×(Q×V)+2sin2(2θ)V+2w(Q×V)=V+2w(Q×V)+2Q×(Q×V)
四元数与矩阵
我们知道，矩阵也可以用来表示3D的旋转。绕任意轴旋转的矩阵为
(
c
o
s
α
+
r
x
2
(
1
−
c
o
s
α
)
r
x
r
y
(
1
−
c
o
s
α
)
+
r
z
s
i
n
α
r
x
r
z
(
1
−
c
o
s
α
)
−
r
y
s
i
n
α
0
r
x
r
y
(
1
−
c
o
s
α
)
−
r
z
s
i
n
α
c
o
s
α
+
r
y
2
(
1
−
c
o
s
α
)
r
y
r
z
(
1
−
c
o
s
α
)
+
r
x
s
i
n
α
0
r
x
r
z
(
1
−
c
o
s
α
)
+
r
y
s
i
n
α
r
y
r
z
(
1
−
c
o
s
α
)
−
r
x
s
i
n
α
c
o
s
α
+
r
z
2
(
1
−
c
o
s
α
)
0
0
0
0
1
)
\begin{pmatrix} cos\alpha + r_x^2(1 - cos\alpha) & r_xr_y(1 - cos\alpha) + r_zsin\alpha & r_xr_z(1 - cos\alpha) - r_ysin\alpha & 0 \\ r_xr_y(1 - cos\alpha) - r_zsin\alpha & cos\alpha + r_y^2(1 - cos\alpha) & r_yr_z(1 - cos\alpha) + r_xsin\alpha & 0 \\ r_xr_z(1 - cos\alpha) + r_ysin\alpha & r_yr_z(1 - cos\alpha) - r_xsin\alpha & cos\alpha + r_z^2(1 - cos\alpha) & 0 \\ 0 & 0 & 0 & 1 \end{pmatrix}
cosα+rx2(1−cosα)rxry(1−cosα)−rzsinαrxrz(1−cosα)+rysinα0rxry(1−cosα)+rzsinαcosα+ry2(1−cosα)ryrz(1−cosα)−rxsinα0rxrz(1−cosα)−rysinαryrz(1−cosα)+rxsinαcosα+rz2(1−cosα)00001
假设四元数为
q
=
[
w
,
x
,
y
,
z
]
q = [w, x, y, z]
q=[w,x,y,z]
对于矩阵的第一行第一列，有
c
o
s
α
+
r
x
2
(
1
−
c
o
s
α
)
=
2
c
o
s
2
α
2
−
1
+
r
x
2
(
2
s
i
n
2
α
2
)
=
2
w
2
−
1
+
2
(
r
x
s
i
n
α
2
)
2
=
2
w
2
−
1
+
2
x
2
=
2
(
1
−
y
2
−
z
2
)
−
1
=
1
−
2
(
y
2
+
z
2
)
cos\alpha + r_x^2(1 - cos\alpha) \\ = 2cos^2\dfrac{\alpha}{2} - 1 + r_x^2(2sin^2\dfrac{\alpha}{2}) \\ = 2w^2 - 1 + 2(r_x sin\dfrac{\alpha}{2})^2 \\ = 2w^2 - 1 + 2x^2 \\ = 2(1 - y^2 - z^2) - 1 \\ = 1 - 2(y^2 + z^2)
cosα+rx2(1−cosα)=2cos22α−1+rx2(2sin22α)=2w2−1+2(rxsin2α)2=2w2−1+2x2=2(1−y2−z2)−1=1−2(y2+z2)
对于矩阵的第一行第二列，有
r
x
r
y
(
1
−
c
o
s
α
)
+
r
z
s
i
n
α
=
r
x
r
y
(
2
s
i
n
2
α
2
)
+
r
z
(
2
s
i
n
α
2
c
o
s
α
2
)
=
2
(
r
x
s
i
n
α
2
)
(
r
y
s
i
n
α
2
)
+
2
c
o
s
α
2
(
r
z
s
i
n
α
2
)
=
2
x
y
+
2
w
z
r_xr_y(1 - cos\alpha) + r_zsin\alpha \\ = r_xr_y(2sin^2\dfrac{\alpha}{2}) + r_z(2sin\dfrac{\alpha}{2}cos\dfrac{\alpha}{2}) \\ = 2(r_xsin\dfrac{\alpha}{2})(r_ysin\dfrac{\alpha}{2}) + 2cos\dfrac{\alpha}{2}(r_zsin\dfrac{\alpha}{2}) \\ = 2xy + 2wz
rxry(1−cosα)+rzsinα=rxry(2sin22α)+rz(2sin2αcos2α)=2(rxsin2α)(rysin2α)+2cos2α(rzsin2α)=2xy+2wz
其他部分类似，最终可得到用四元数表示的旋转矩阵为
(
1
−
2
(
y
2
+
z
2
)
2
x
y
+
2
w
z
2
x
z
−
2
w
y
0
2
x
y
−
2
w
z
1
−
2
(
x
2
+
z
2
)
2
y
z
+
2
w
x
0
2
x
z
+
2
w
y
2
y
z
−
2
w
z
1
−
2
(
x
2
+
y
2
)
0
0
0
0
1
)
\begin{pmatrix} 1 - 2(y^2 + z^2) & 2xy + 2wz & 2xz - 2wy & 0 \\ 2xy - 2wz & 1 - 2(x^2 + z^2) & 2yz + 2wx & 0 \\ 2xz + 2wy & 2yz - 2wz & 1 - 2(x^2 + y^2) & 0 \\ 0 & 0 & 0 & 1 \end{pmatrix}
1−2(y2+z2)2xy−2wz2xz+2wy02xy+2wz1−2(x2+z2)2yz−2wz02xz−2wy2yz+2wx1−2(x2+y2)00001
与UE中四元数转矩阵的函数完全一致。
template<typename T>
void TQuat<T>::ToMatrix(TMatrix<T>& R) const
{
// Note: copied and modified from TQuatRotationTranslationMatrix<> mainly to avoid circular dependency.
#if !(UE_BUILD_SHIPPING || UE_BUILD_TEST) && WITH_EDITORONLY_DATA
// Make sure Quaternion is normalized
ensure(IsNormalized());
#endif
const T x2 = X + X; const T y2 = Y + Y; const T z2 = Z + Z;
const T xx = X * x2; const T xy = X * y2; const T xz = X * z2;
const T yy = Y * y2; const T yz = Y * z2; const T zz = Z * z2;
const T wx = W * x2; const T wy = W * y2; const T wz = W * z2;
R.M[0][0] = 1.0f - (yy + zz); R.M[1][0] = xy - wz; R.M[2][0] = xz + wy; R.M[3][0] = 0.0f;
R.M[0][1] = xy + wz; R.M[1][1] = 1.0f - (xx + zz); R.M[2][1] = yz - wx; R.M[3][1] = 0.0f;
R.M[0][2] = xz - wy; R.M[1][2] = yz + wx; R.M[2][2] = 1.0f - (xx + yy); R.M[3][2] = 0.0f;
R.M[0][3] = 0.0f; R.M[1][3] = 0.0f; R.M[2][3] = 0.0f; R.M[3][3] = 1.0f;
}
那么，假如已知的是旋转矩阵，又如何解出四元数呢？我们取出矩阵中的3x3部分，计算3x3矩阵的trace
t
r
=
(
1
−
2
(
y
2
+
z
2
)
)
+
(
1
−
2
(
x
2
+
z
2
)
)
+
(
1
−
2
(
x
2
+
y
2
)
)
=
3
−
4
(
x
2
+
y
2
+
z
2
)
=
4
w
2
−
1
tr = (1 - 2(y^2 + z^2)) + (1 - 2(x^2 + z^2)) + (1 - 2(x^2 + y^2)) \\ = 3 - 4(x^2 + y^2 + z^2) \\ = 4w^2 - 1
tr=(1−2(y2+z2))+(1−2(x2+z2))+(1−2(x2+y2))=3−4(x2+y2+z2)=4w2−1
如果trace大于0，就能解出w的值，进而快速解出x，y和z了，而如果trace小于等于0，则需要换一个思路。对于矩阵对角线上的三个元素，我们取出最大的元素，这里不妨假设
M
[
2
]
[
2
]
>
M
[
1
]
[
1
]
>
M
[
0
]
[
0
]
M[2][2] > M[1][1] > M[0][0]
M[2][2]>M[1][1]>M[0][0]
那么有
1
−
2
(
x
2
+
y
2
)
>
1
−
2
(
x
2
+
z
2
)
>
1
−
2
(
y
2
+
z
2
)
x
2
+
y
2
<
x
2
+
z
2
<
y
2
+
z
2
0
≤
x
2
<
y
2
<
z
2
1 - 2(x^2 + y^2) > 1 - 2(x^2 + z^2) > 1 - 2(y^2 + z^2) \\ x^2 + y^2 < x^2 + z^2 < y^2 + z^2 \\ 0 \leq x^2 < y^2 < z^2
1−2(x2+y2)>1−2(x2+z2)>1−2(y2+z2)x2+y2<x2+z2<y2+z20≤x2<y2<z2
可以发现，
z
2
z^2
z2必定大于0，它是一个可以作为分母的值，那么只需构造一个包含
z
2
z^2
z2的式子，把z解出即可。
M
[
2
]
[
2
]
−
M
[
1
]
[
1
]
−
M
[
0
]
[
0
]
+
1
=
4
z
2
M[2][2] - M[1][1] - M[0][0] + 1 = 4z^2
M[2][2]−M[1][1]−M[0][0]+1=4z2
UE中的相关代码如下：
template<typename T>
inline TQuat<T>::TQuat(const UE::Math::TMatrix<T>& M)
{
//const MeReal *const t = (MeReal *) tm;
T s;
// Check diagonal (trace)
const T tr = M.M[0][0] + M.M[1][1] + M.M[2][2];
if (tr > 0.0f)
{
T InvS = FMath::InvSqrt(tr + T(1.f));
this->W = T(T(0.5f) * (T(1.f) / InvS));
s = T(0.5f) * InvS;
this->X = ((M.M[1][2] - M.M[2][1]) * s);
this->Y = ((M.M[2][0] - M.M[0][2]) * s);
this->Z = ((M.M[0][1] - M.M[1][0]) * s);
}
else
{
// diagonal is negative
int32 i = 0;
if (M.M[1][1] > M.M[0][0])
i = 1;
if (M.M[2][2] > M.M[i][i])
i = 2;
static constexpr int32 nxt[3] = { 1, 2, 0 };
const int32 j = nxt[i];
const int32 k = nxt[j];
s = M.M[i][i] - M.M[j][j] - M.M[k][k] + T(1.0f);
T InvS = FMath::InvSqrt(s);
T qt[4];
qt[i] = T(0.5f) * (T(1.f) / InvS);
s = T(0.5f) * InvS;
qt[3] = (M.M[j][k] - M.M[k][j]) * s;
qt[j] = (M.M[i][j] + M.M[j][i]) * s;
qt[k] = (M.M[i][k] + M.M[k][i]) * s;
this->X = qt[0];
this->Y = qt[1];
this->Z = qt[2];
this->W = qt[3];
DiagnosticCheckNaN();
}
}
四元数与欧拉角
UE中的旋转顺序为先roll，再pitch，最后yaw的外旋旋转顺序。绕固定坐标系的X-Y-Z轴依次旋转α，β，γ角度可以得到最终的四元数为
q
=
q
γ
q
β
q
α
=
[
c
o
s
(
γ
2
)
0
0
s
i
n
(
γ
2
)
]
[
c
o
s
(
β
2
)
0
−
s
i
n
(
β
2
)
0
]
[
c
o
s
(
α
2
)
−
s
i
n
(
α
2
)
0
0
]
=
[
c
o
s
(
α
2
)
c
o
s
(
β
2
)
c
o
s
(
γ
2
)
+
s
i
n
(
α
2
)
s
i
n
(
β
2
)
s
i
n
(
γ
2
)
c
o
s
(
α
2
)
s
i
n
(
β
2
)
s
i
n
(
γ
2
)
−
s
i
n
(
α
2
)
c
o
s
(
β
2
)
c
o
s
(
γ
2
)
−
c
o
s
(
α
2
)
s
i
n
(
β
2
)
c
o
s
(
γ
2
)
−
s
i
n
(
α
2
)
c
o
s
(
β
2
)
s
i
n
(
γ
2
)
c
o
s
(
α
2
)
c
o
s
(
β
2
)
s
i
n
(
γ
2
)
−
s
i
n
(
α
2
)
s
i
n
(
β
2
)
c
o
s
(
γ
2
)
]
q = q_{\gamma}q_{\beta}q_{\alpha} \\ = \begin{bmatrix} cos(\dfrac{\gamma}{2}) \\ 0 \\ 0 \\ sin(\dfrac{\gamma}{2}) \end{bmatrix} \begin{bmatrix} cos(\dfrac{\beta}{2}) \\ 0 \\ -sin(\dfrac{\beta}{2}) \\ 0 \end{bmatrix} \begin{bmatrix} cos(\dfrac{\alpha}{2}) \\ -sin(\dfrac{\alpha}{2}) \\ 0 \\ 0 \end{bmatrix} \\ = \begin{bmatrix} cos(\dfrac{\alpha}{2})cos(\dfrac{\beta}{2})cos(\dfrac{\gamma}{2}) + sin(\dfrac{\alpha}{2})sin(\dfrac{\beta}{2})sin(\dfrac{\gamma}{2}) \\ cos(\dfrac{\alpha}{2})sin(\dfrac{\beta}{2})sin(\dfrac{\gamma}{2}) - sin(\dfrac{\alpha}{2})cos(\dfrac{\beta}{2})cos(\dfrac{\gamma}{2}) \\ -cos(\dfrac{\alpha}{2})sin(\dfrac{\beta}{2})cos(\dfrac{\gamma}{2}) - sin(\dfrac{\alpha}{2})cos(\dfrac{\beta}{2})sin(\dfrac{\gamma}{2}) \\ cos(\dfrac{\alpha}{2})cos(\dfrac{\beta}{2})sin(\dfrac{\gamma}{2}) - sin(\dfrac{\alpha}{2})sin(\dfrac{\beta}{2})cos(\dfrac{\gamma}{2}) \end{bmatrix}
q=qγqβqα=
cos(2γ)00sin(2γ)
cos(2β)0−sin(2β)0
cos(2α)−sin(2α)00
=
cos(2α)cos(2β)cos(2γ)+sin(2α)sin(2β)sin(2γ)cos(2α)sin(2β)sin(2γ)−sin(2α)cos(2β)cos(2γ)−cos(2α)sin(2β)cos(2γ)−sin(2α)cos(2β)sin(2γ)cos(2α)cos(2β)sin(2γ)−sin(2α)sin(2β)cos(2γ)
根据上述公式，解出roll，pitch，和yaw
α
=
a
r
c
t
a
n
(
−
2
(
w
x
+
y
z
)
1
−
2
(
x
2
+
y
2
)
)
\alpha = arctan(\dfrac{-2(wx + yz)}{1 - 2(x^2 + y^2)})
α=arctan(1−2(x2+y2)−2(wx+yz))
β = a r c s i n ( 2 ( z x − w y ) ) \beta = arcsin(2(zx - wy)) β=arcsin(2(zx−wy))
γ = a r c t a n ( 2 ( w z + x y ) 1 − 2 ( y 2 + z 2 ) ) \gamma = arctan(\dfrac{2(wz + xy)}{1 - 2(y^2 + z^2)}) γ=arctan(1−2(y2+z2)2(wz+xy))
不过这里有个特殊情况，我们知道欧拉角是可能导致万向锁的，例如当pitch = 90°时，roll和yaw会变成一个自由度。此时四元数的值为
q
=
[
2
2
c
o
s
(
α
−
γ
2
)
−
2
2
s
i
n
(
α
−
γ
2
)
−
2
2
c
o
s
(
α
−
γ
2
)
−
2
2
s
i
n
(
α
−
γ
2
)
]
q = \begin{bmatrix} \dfrac{\sqrt 2}{2} cos(\dfrac{\alpha - \gamma}{2}) \\ -\dfrac{\sqrt 2}{2} sin(\dfrac{\alpha - \gamma}{2}) \\ -\dfrac{\sqrt 2}{2} cos(\dfrac{\alpha - \gamma}{2}) \\ -\dfrac{\sqrt 2}{2} sin(\dfrac{\alpha - \gamma}{2}) \end{bmatrix}
q=
22cos(2α−γ)−22sin(2α−γ)−22cos(2α−γ)−22sin(2α−γ)
可以发现此时
w
x
+
y
z
=
0
wx + yz = 0
wx+yz=0
w z + x y = 0 wz + xy = 0 wz+xy=0
y 2 + z 2 = 1 2 y^2 + z^2 = \dfrac{1}{2} y2+z2=21
x 2 + y 2 = 1 2 x^2 + y^2 = \dfrac{1}{2} x2+y2=21
这会导致arctan里出现0除0，因此并不能采用上述推导的公式进行计算。我们知道，pitch = 90°时当且仅当
z
x
−
w
y
=
1
2
zx - wy = \dfrac{1}{2}
zx−wy=21
并且
t
a
n
α
−
γ
2
=
−
x
w
tan \dfrac{\alpha - \gamma}{2} = -\dfrac{x}{w}
tan2α−γ=−wx
即
α
=
γ
−
2
a
r
c
t
a
n
(
x
w
)
\alpha = \gamma - 2arctan(\dfrac{x}{w})
α=γ−2arctan(wx)
通常令yaw=0°，得到
α
=
−
2
a
r
c
t
a
n
(
x
w
)
\alpha = -2arctan(\dfrac{x}{w})
α=−2arctan(wx)
UE相关的代码如下，这是四元数转欧拉角的部分：
template<>
FRotator3f FQuat4f::Rotator() const
{
DiagnosticCheckNaN();
const float SingularityTest = Z * X - W * Y;
const float YawY = 2.f * (W * Z + X * Y);
const float YawX = (1.f - 2.f * (FMath::Square(Y) + FMath::Square(Z)));
// reference
// http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
// http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
// this value was found from experience, the above websites recommend different values
// but that isn't the case for us, so I went through different testing, and finally found the case
// where both of world lives happily.
const float SINGULARITY_THRESHOLD = 0.4999995f;
const float RAD_TO_DEG = (180.f / UE_PI);
float Pitch, Yaw, Roll;
if (SingularityTest < -SINGULARITY_THRESHOLD)
{
Pitch = -90.f;
Yaw = (FMath::Atan2(YawY, YawX) * RAD_TO_DEG);
Roll = FRotator3f::NormalizeAxis(-Yaw - (2.f * FMath::Atan2(X, W) * RAD_TO_DEG));
}
else if (SingularityTest > SINGULARITY_THRESHOLD)
{
Pitch = 90.f;
Yaw = (FMath::Atan2(YawY, YawX) * RAD_TO_DEG);
Roll = FRotator3f::NormalizeAxis(Yaw - (2.f * FMath::Atan2(X, W) * RAD_TO_DEG));
}
else
{
Pitch = (FMath::FastAsin(2.f * SingularityTest) * RAD_TO_DEG);
Yaw = (FMath::Atan2(YawY, YawX) * RAD_TO_DEG);
Roll = (FMath::Atan2(-2.f * (W*X + Y*Z), (1.f - 2.f * (FMath::Square(X) + FMath::Square(Y)))) * RAD_TO_DEG);
}
FRotator3f RotatorFromQuat = FRotator3f(Pitch, Yaw, Roll);
return RotatorFromQuat;
}
这是欧拉角转四元数的部分：
template<>
FQuat4f FRotator3f::Quaternion() const
{
const float DEG_TO_RAD = UE_PI/(180.f);
const float RADS_DIVIDED_BY_2 = DEG_TO_RAD/2.f;
float SP, SY, SR;
float CP, CY, CR;
const float PitchNoWinding = FMath::Fmod(Pitch, 360.0f);
const float YawNoWinding = FMath::Fmod(Yaw, 360.0f);
const float RollNoWinding = FMath::Fmod(Roll, 360.0f);
FMath::SinCos(&SP, &CP, PitchNoWinding * RADS_DIVIDED_BY_2);
FMath::SinCos(&SY, &CY, YawNoWinding * RADS_DIVIDED_BY_2);
FMath::SinCos(&SR, &CR, RollNoWinding * RADS_DIVIDED_BY_2);
FQuat4f RotationQuat;
RotationQuat.X = CR*SP*SY - SR*CP*CY;
RotationQuat.Y = -CR*SP*CY - SR*CP*SY;
RotationQuat.Z = CR*CP*SY - SR*SP*CY;
RotationQuat.W = CR*CP*CY + SR*SP*SY;
return RotationQuat;
}
将一个向量旋转到另一个向量
实际应用中经常会遇到已知一个向量A，如何将其旋转到另外一个向量B上。想要构造表示旋转的四元数，只需计算出旋转轴N和旋转的角度θ即可。
q
=
[
c
o
s
(
θ
2
)
,
s
i
n
(
θ
2
)
N
∣
∣
N
∣
∣
]
q = [cos(\frac{\theta}{2}), sin(\frac{\theta}{2})\dfrac{N}{||N||}]
q=[cos(2θ),sin(2θ)∣∣N∣∣N]
旋转轴可由两个向量的叉乘得到，假设两个向量均为单位向量，那么
q
=
[
c
o
s
(
θ
2
)
,
s
i
n
(
θ
2
)
A
×
B
s
i
n
θ
]
=
[
c
o
s
(
θ
2
)
,
A
×
B
2
c
o
s
(
θ
2
)
]
q = [cos(\frac{\theta}{2}), sin(\frac{\theta}{2})\dfrac{A \times B}{sin\theta}] \\ = [cos(\frac{\theta}{2}), \dfrac{A \times B}{2cos(\frac{\theta}{2})}]
q=[cos(2θ),sin(2θ)sinθA×B]=[cos(2θ),2cos(2θ)A×B]
再对q进行数乘
2
c
o
s
(
θ
2
)
q
=
[
2
c
o
s
2
(
θ
2
)
,
A
×
B
]
=
[
1
+
c
o
s
θ
,
A
×
B
]
=
[
1
+
A
⋅
B
,
A
×
B
]
2cos(\frac{\theta}{2})q = [2cos^2(\frac{\theta}{2}), A \times B] \\ = [1 + cos\theta, A \times B] \\ = [1 + A \cdot B, A \times B]
2cos(2θ)q=[2cos2(2θ),A×B]=[1+cosθ,A×B]=[1+A⋅B,A×B]
这样，我们可以仅用两个向量的点乘和叉乘，就能得到表示旋转的四元数，最后将其归一化即可。不过，这里还需要考虑两个向量共线的特殊情况。当两个向量共线时，叉乘为0向量，点乘为±1。可以看到，如果两个向量方向相同，得到的四元数为[1, 0, 0, 0]，即为表示旋转角度为0的四元数；而如果两个向量方向相反，得到四元数为[0, 0, 0, 0]，显然是不正确的。此时只需要再指定一个向量，一般取x，y，z值当中较小的单位轴，然后在此基础上再构建出一个旋转轴即可。
UE中相关代码如下：
template<typename T>
FORCEINLINE_DEBUGGABLE TQuat<T> FindBetween_Helper(const TVector<T>& A, const TVector<T>& B, T NormAB)
{
T W = NormAB + TVector<T>::DotProduct(A, B);
TQuat<T> Result;
if (W >= 1e-6f * NormAB)
{
//Result = FVector::CrossProduct(A, B);
Result = TQuat<T>(
A.Y * B.Z - A.Z * B.Y,
A.Z * B.X - A.X * B.Z,
A.X * B.Y - A.Y * B.X,
W);
}
else
{
// A and B point in opposite directions
W = 0.f;
const T X = FMath::Abs(A.X);
const T Y = FMath::Abs(A.Y);
const T Z = FMath::Abs(A.Z);
// Find orthogonal basis.
const TVector<T> Basis = (X > Y && X > Z) ? TVector<T>::YAxisVector : -TVector<T>::XAxisVector;
//Result = FVector::CrossProduct(A, Basis);
Result = TQuat<T>(
A.Y * Basis.Z - A.Z * Basis.Y,
A.Z * Basis.X - A.X * Basis.Z,
A.X * Basis.Y - A.Y * Basis.X,
W);
}
Result.Normalize();
return Result;
}
插值
假设有四元数 q 0 q_0 q0和 q 1 q_1 q1，如何从 q 0 q_0 q0变换到 q 1 q_1 q1呢？我们知道四元数本身可以表示旋转，所以如果有向量v，那么
经过
q
0
q_0
q0变换有
v
0
=
q
0
v
q
0
∗
v_0 = q_0 v q_0^*
v0=q0vq0∗
经过
q
1
q_1
q1变换有
v
1
=
q
1
v
q
1
∗
v_1 = q_1 v q_1^*
v1=q1vq1∗
换言之，从
q
0
q_0
q0变换到
q
1
q_1
q1，可以认为等同于将
v
0
v_0
v0变换到
v
1
v_1
v1，假设变换的四元数为
Δ
q
\Delta q
Δq，有
v
1
=
Δ
q
v
0
Δ
q
∗
v_1 = \Delta q v_0 \Delta q^*
v1=Δqv0Δq∗
q 1 v q 1 ∗ = ( Δ q q 0 ) v ( Δ q q 0 ) ∗ q_1 v q_1^* = (\Delta q q_0) v (\Delta q q_0)^* q1vq1∗=(Δqq0)v(Δqq0)∗
所以
q
1
=
Δ
q
q
0
q_1 = \Delta q q_0
q1=Δqq0
Δ q = q 1 q 0 ∗ Δ q = [ c o s ( θ 1 2 ) , s i n ( θ 1 2 ) N 1 ] [ c o s ( θ 0 2 ) , − s i n ( θ 0 2 ) N 0 ] \Delta q = q_1 q_0^* \\ \Delta q = [cos(\frac{\theta_1}{2}), sin(\frac{\theta_1}{2})N_1] [cos(\frac{\theta_0}{2}), -sin(\frac{\theta_0}{2})N_0] \\ Δq=q1q0∗Δq=[cos(2θ1),sin(2θ1)N1][cos(2θ0),−sin(2θ0)N0]
Δ
q
\Delta q
Δq的实部部分，刚好是把
q
0
q_0
q0，
q
1
q_1
q1视为四维向量的点乘。而
Δ
q
\Delta q
Δq本身也可以表示旋转，它的实部也可写成
c
o
s
ϕ
2
=
q
0
⋅
q
1
=
∣
∣
q
0
∣
∣
∣
∣
q
1
∣
∣
c
o
s
θ
=
c
o
s
θ
cos \dfrac{\phi}{2} = q_0 \cdot q_1 = ||q_0|| ||q_1|| cos \theta = cos \theta
cos2ϕ=q0⋅q1=∣∣q0∣∣∣∣q1∣∣cosθ=cosθ
也就是说，
q
0
q_0
q0和
q
1
q_1
q1作为四维向量的夹角，刚好是它们之间的旋转变化量
Δ
q
\Delta q
Δq所表示的角度的一半。
所以，对四元数进行插值，可以理解为对四维向量进行插值。如果做Slerp，假设四维向量间的夹角为θ，要插值的角度为tθ，得到的四维向量为
q
t
=
a
q
0
+
b
q
1
q_t = aq_0 + bq_1
qt=aq0+bq1
式子两边都点乘
q
0
q_0
q0，得到
q
0
q
t
=
a
q
0
2
+
b
q
0
q
1
q_0 q_t = a q_0^2 + b q_0 q_1
q0qt=aq02+bq0q1
c o s ( t θ ) = a + b c o s θ cos(t\theta) = a + b cos\theta cos(tθ)=a+bcosθ
式子两边都点乘
q
1
q_1
q1，得到
q
1
q
t
=
a
q
0
q
1
+
b
q
1
2
q_1 q_t = a q_0 q_1 + b q_1^2
q1qt=aq0q1+bq12
c o s ( ( 1 − t ) θ ) = a c o s θ + b cos((1 - t)\theta) = a cos \theta + b cos((1−t)θ)=acosθ+b
解方程得到
a
=
s
i
n
(
(
1
−
t
)
θ
)
s
i
n
θ
a = \dfrac{sin((1 - t)\theta)}{sin \theta}
a=sinθsin((1−t)θ)
b = s i n ( t θ ) s i n θ b = \dfrac{sin(t\theta)}{sin\theta} b=sinθsin(tθ)
所以
q
t
=
s
i
n
(
(
1
−
t
)
θ
)
s
i
n
θ
q
0
+
s
i
n
(
t
θ
)
s
i
n
θ
q
1
q_t = \dfrac{sin((1 - t)\theta)}{sin \theta} q_0 + \dfrac{sin(t\theta)}{sin\theta} q_1
qt=sinθsin((1−t)θ)q0+sinθsin(tθ)q1
值得注意的是，四元数和3D旋转并不是一一对应的，一个旋转既可以表示为绕旋转轴正方向旋转θ角度，也可以表示为绕旋转轴负方向旋转
2
π
−
θ
2\pi - \theta
2π−θ角度。
q
1
=
[
c
o
s
(
θ
2
)
,
s
i
n
(
θ
2
)
N
]
q_1 = [cos(\frac{\theta}{2}), sin(\frac{\theta}{2})N]
q1=[cos(2θ),sin(2θ)N]
q 2 = [ c o s ( 2 π − θ 2 ) , s i n ( 2 π − θ 2 ) ( − N ) ] = [ − c o s ( θ 2 ) , − s i n ( θ 2 ) N ] = − q 1 q_2 = [cos(\frac{2\pi - \theta}{2}), sin(\frac{2\pi - \theta}{2})(-N)] = [-cos(\frac{\theta}{2}), -sin(\frac{\theta}{2})N] = -q_1 q2=[cos(22π−θ),sin(22π−θ)(−N)]=[−cos(2θ),−sin(2θ)N]=−q1
可以看到表示同一旋转的两个四元数相互为负。所以在四元数插值时，要先选择一个最短的旋转路径。检测方式就是计算两个四元数的夹角，如果为负数将某一个取反即可。
另外，如果四元数的夹角很小，那么夹角的正弦值可能趋近于0，这会导致上述式子a，b出现除0的后果。为了避免这种情况发生，此时需要将Slerp换成普通的线性插值计算。
UE中相关代码如下：
template<typename T>
TQuat<T> TQuat<T>::Slerp_NotNormalized(const TQuat<T>& Quat1, const TQuat<T>& Quat2, T Slerp)
{
// Get cosine of angle between quats.
T RawCosom =
Quat1.X * Quat2.X +
Quat1.Y * Quat2.Y +
Quat1.Z * Quat2.Z +
Quat1.W * Quat2.W;
// Unaligned quats - compensate, results in taking shorter route.
const T Sign = FMath::FloatSelect(RawCosom, static_cast<T>(1.0), static_cast<T>(-1.0));
RawCosom *= Sign;
T Scale0 = static_cast<T>(1.0) - Slerp;
T Scale1 = Slerp * Sign;
if (RawCosom < static_cast<T>(0.9999))
{
const T Omega = FMath::Acos(RawCosom);
const T InvSin = static_cast<T>(1.0) / FMath::Sin(Omega);
Scale0 = FMath::Sin(Scale0 * Omega) * InvSin;
Scale1 = FMath::Sin(Scale1 * Omega) * InvSin;
}
return TQuat<T>(
Scale0 * Quat1.X + Scale1 * Quat2.X,
Scale0 * Quat1.Y + Scale1 * Quat2.Y,
Scale0 * Quat1.Z + Scale1 * Quat2.Z,
Scale0 * Quat1.W + Scale1 * Quat2.W);
}
Reference
[1] 四元数与三维旋转
[2] 四元数与欧拉角（RPY角）的相互转换
[3] Efficiently Building a Matrix to Rotate One Vector to Another
[4] Householder变换
[5] CloudCompare/CloudCompare