四元数的公式
本文只记录一下四元数相关的公式，不讲其数学原理，因为我也不懂。只介绍Overload中这些数学公式到代码。
1. 四元数定义
威廉·汉密尔顿于1843年发明了四元数，作为一种允许对向量进行乘法和除法、旋转和拉伸的方法。四元数是有四个数，由一个标量和一个向量组成的复数。[w,v]，w为标量，v为3D向量，展开后为——[w，(x,y,z)]：
q
=
w
+
x
∗
i
+
y
∗
j
+
z
∗
k
\mathbf{q} = w + x*i+y*j+z*k
q=w+x∗i+y∗j+z∗k
其中：
i
2
=
j
2
=
k
2
=
−
1
i^2=j^2=k^2=-1
i2=j2=k2=−1，
i
j
=
−
j
i
=
k
ij=-ji=k
ij=−ji=k，
k
i
=
−
i
k
=
j
ki=-ik=j
ki=−ik=j，
j
k
=
−
k
j
=
i
jk=-kj=i
jk=−kj=i，
对于给定的任意一个四元数可以表示3D空间中的一个旋转。在3D空间中，任意旋转都可以表示为绕着某个轴旋转一个旋转角得到。轴角度顾名思义就是一个轴A(ax, ay, az)加上一个旋转角度angle。四元数与轴角相互转换。所以对于给定的任意一个四元数可以表示3D空间中的一个旋转。
看看Overload中四元数的定义，只是封装4个数，定义一大堆函数。
struct FQuaternion
{
public:
float x; // 矢量部分
float y;
float z;
float w; // 标量部分
}
2. 四元数常用计算
1. 相加
q
^
+
r
^
=
(
q
w
,
q
v
)
+
(
r
w
,
r
v
)
=
(
q
w
+
r
w
,
q
v
+
r
v
)
\hat{\mathbf{q}} + \hat{\mathbf{r}}=(\mathbf{q}_w, q_v)+(\mathbf{r}_w, r_v)=(\mathbf{q}_w+\mathbf{r}_w, q_v+r_v)
q^+r^=(qw,qv)+(rw,rv)=(qw+rw,qv+rv)
对应的Overload代码：
OvMaths::FQuaternion OvMaths::FQuaternion::operator+(const FQuaternion& p_otherQuat) const
{
return FQuaternion(x + p_otherQuat.x, y + p_otherQuat.x,
z + p_otherQuat.z, w + p_otherQuat.w);
}
2. 相乘
q
^
×
r
^
=
(
i
q
x
+
j
q
y
+
k
q
z
+
q
w
)
(
i
r
x
+
j
r
y
+
k
r
z
+
r
w
)
=
q
w
r
w
−
q
x
r
x
−
q
y
r
y
−
q
z
r
z
+
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
)
\begin{aligned} \hat{\mathbf{q}}\times \hat{\mathbf{r}} &= (iq_x+jq_y+kq_z+q_w)(ir_x+jr_y+kr_z+r_w)\\&=q_wr_w-q_xr_x-q_yr_y-q_zr_z \\ &+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)\end{aligned}
q^×r^=(iqx+jqy+kqz+qw)(irx+jry+krz+rw)=qwrw−qxrx−qyry−qzrz+i(qyrz−qzry+rwqx+qwrx)+j(qzrx−qxrz+rwqy+qwry)+k(qxry−qyrx+rwqz+qwrz)
对应的Overload代码：
OvMaths::FQuaternion OvMaths::FQuaternion::operator*(const FQuaternion& p_otherQuat) const
{
return FQuaternion
(
x * p_otherQuat.w + y * p_otherQuat.z - z * p_otherQuat.y + w * p_otherQuat.x,
-x * p_otherQuat.z + y * p_otherQuat.w + z * p_otherQuat.x + w * p_otherQuat.y,
x * p_otherQuat.y - y * p_otherQuat.x + z * p_otherQuat.w + w * p_otherQuat.z,
-x * p_otherQuat.x - y * p_otherQuat.y - z * p_otherQuat.z + w * p_otherQuat.w
);
}
3. 共轭
向量部分取反，直接上代码：
OvMaths::FQuaternion OvMaths::FQuaternion::Conjugate(const FQuaternion & p_target)
{
return { -p_target.x, -p_target.y, -p_target.z, p_target.w };
}
4. 规则化
q
^
=
q
^
/
w
2
+
x
2
+
y
2
+
z
2
\hat{\mathbf{q}} = \hat{\mathbf{q}} / \sqrt{w^2+x^2+y^2+z^2}
q^=q^/w2+x2+y2+z2
5. 点积
q
^
⋅
r
^
=
q
x
∗
r
x
+
q
y
∗
r
y
+
q
z
∗
r
z
+
q
w
∗
r
w
\hat{\mathbf{q}} \cdot \hat{\mathbf{r}} = q_x*r_x + q_y*r_y + q_z*r_z + q_w*r_w
q^⋅r^=qx∗rx+qy∗ry+qz∗rz+qw∗rw
代码：
float OvMaths::FQuaternion::DotProduct(const FQuaternion & p_left, const FQuaternion & p_right)
{
return
p_left.x * p_right.x +
p_left.y * p_right.y +
p_left.z * p_right.z +
p_left.w * p_right.w;
}
6. 插值
两个四元数q与r的插值可用于制作旋转过程的动画。
插值计算要先计算q与r夹角的一半
θ
\theta
θ, q与r的点积是
c
o
s
(
θ
)
cos(\theta)
cos(θ), 进而用acos函数可以求出
θ
\theta
θ
θ
=
a
c
o
s
(
q
^
⋅
r
^
)
\theta = acos(\hat{\mathbf{q}} \cdot \hat{\mathbf{r}})
θ=acos(q^⋅r^)
给出插值量t，0<t<1.0, 即可求出插值量：
Q
m
=
q
^
∗
s
i
n
(
(
1
−
t
)
∗
θ
)
+
r
^
s
i
n
(
t
∗
θ
)
s
i
n
(
θ
)
Q_m= \frac{\hat{\mathbf{q}}*sin((1-t)*\theta) + \hat{\mathbf{r}}sin(t*\theta)}{sin(\theta)}
Qm=sin(θ)q^∗sin((1−t)∗θ)+r^sin(t∗θ)
对应的Overload代码：
OvMaths::FQuaternion OvMaths::FQuaternion::Slerp(const FQuaternion& p_start, const FQuaternion& p_end, float p_alpha)
{
FQuaternion from = p_start; // 第一个四元数
FQuaternion to = p_end; // 第二个四元数
p_alpha = std::clamp(p_alpha, 0.f, 1.f); // 插值量t
float cosAngle = FQuaternion::DotProduct(from, to); // 计算四元数的点积
if (cosAngle < 0.f) // 小于0说明两个四元数角度大于180度，to为什么要翻转呢？
{
cosAngle = -cosAngle;
to = FQuaternion(-to.x, -to.y, -to.z, -to.w);
}
if (cosAngle < 0.95f) // 两个四元数角度较大时
{
float angle = std::acos(cosAngle); // 计算角度theta
float sinAngle = std::sin(angle); // 计算sin(theta)
float invSinAngle = 1.f / sinAngle;
float t1 = std::sin((1 - p_alpha) * angle) * invSinAngle;
float t2 = std::sin(p_alpha * angle) * invSinAngle;
return FQuaternion(from.x * t1 + to.x * t2, from.y * t1 + to.y * t2, from.z * t1 + to.z * t2, from.w * t1 + to.w * t2);
}
else
{
// 两个四元数角度较小时
return FQuaternion::Lerp(from, to, p_alpha);
}
}
当两个四元数角度较小时， s i n ( θ ) sin(\theta) sin(θ)存在除0问题，所以进行特殊处理。
3. 轴角转四元数
输入：旋转轴A（ax， ay，az）、旋转角度angle
输出：
qx = ax * sin(angle/2)
qy = ay * sin(angle/2)
qz = az * sin(angle/2)
qw = cos(angle/2)
其中：
旋转轴是单位矢量，所以: axax + ayay + az*az = 1
计算出的四元数也是单位化的，所以：
c
o
s
(
a
n
g
l
e
/
2
)
2
+
a
x
∗
a
x
∗
s
i
n
(
a
n
g
l
e
/
2
)
2
+
a
y
∗
a
y
∗
s
i
n
(
a
n
g
l
e
/
2
)
2
+
a
z
∗
a
z
∗
s
i
n
(
a
n
g
l
e
/
2
)
2
=
1
cos(angle/2)^2 + ax*ax * sin(angle/2)^2 + ay*ay * sin(angle/2)^2+ az*az * sin(angle/2)^2 = 1
cos(angle/2)2+ax∗ax∗sin(angle/2)2+ay∗ay∗sin(angle/2)2+az∗az∗sin(angle/2)2=1
4.四元数转轴角
x
=
q
x
/
s
q
r
t
(
1
−
q
w
∗
q
w
)
x = qx / sqrt(1-qw*qw)
x=qx/sqrt(1−qw∗qw)
y
=
q
y
/
s
q
r
t
(
1
−
q
w
∗
q
w
)
y = qy / sqrt(1-qw*qw)
y=qy/sqrt(1−qw∗qw)
z
=
q
z
/
s
q
r
t
(
1
−
q
w
∗
q
w
)
z = qz / sqrt(1-qw*qw)
z=qz/sqrt(1−qw∗qw)
a
n
g
l
e
=
2
∗
a
c
o
s
(
q
w
)
angle = 2 * acos(qw)
angle=2∗acos(qw)
- 奇异性
轴计算是存在奇异性的值，angle = 0的时候存在除0问题。
q = cos(angle/2) + i ( x * sin(angle/2)) + j (y * sin(angle/2)) + k ( z * sin(angle/2))
当angle是0度的时候，四元数是q = 1 + i 0 + j 0 + k 0，所以计算轴的时候
angle = 2 * acos(qw) = 0
x = qx / sqrt(1-qwqw) = divide by zero = infinity
y = qy / sqrt(1-qwqw) = divide by zero = infinity
z = qz / sqrt(1-qw*qw) = divide by zero = infinity
但转动0度相当于不旋转，所以给出的轴可以是任意值。
Overload中对应代码：
OvMaths::FVector3 OvMaths::FQuaternion::GetRotationAxis(const FQuaternion & p_target)
{
const float S = sqrt(std::max(1.f - (p_target.w * p_target.w), 0.f));
if (S >= 0.0001f)
{
return FVector3(p_target.x / S, p_target.y / S, p_target.z / S);
}
return FVector3(1.f, 0.f, 0.f);
}
5. 四元数转矩阵
四元数在图形学中主要用于表示旋转。对于qw + i qx + j qy + k qz 四元数，其对应的3x3、4x4旋转矩阵如下：
R
3
,
3
=
[
1
−
2
(
y
2
+
z
2
)
2
(
x
y
−
z
w
)
2
(
x
z
+
y
w
)
2
(
x
y
+
z
w
)
1
−
2
(
x
2
+
z
2
)
2
(
y
z
−
x
w
)
2
(
x
z
−
y
w
)
2
(
y
z
+
x
w
)
1
−
2
(
x
2
+
y
2
)
]
R_{3,3}=\begin{bmatrix} 1-2(y^2+z^2) & 2(xy-zw) & 2(xz+yw)\\ 2(xy+zw) & 1-2(x^2+z^2) & 2(yz-xw)\\ 2(xz-yw) & 2(yz+xw) & 1-2(x^2+y^2) \end{bmatrix}
R3,3=
1−2(y2+z2)2(xy+zw)2(xz−yw)2(xy−zw)1−2(x2+z2)2(yz+xw)2(xz+yw)2(yz−xw)1−2(x2+y2)
R 4 , 4 = [ 1 − 2 ( y 2 + z 2 ) 2 ( x y − z w ) 2 ( x z + y w ) 0 2 ( x y + z w ) 1 − 2 ( x 2 + z 2 ) 2 ( y z − x w ) 0 2 ( x z − y w ) 2 ( y z + x w ) 1 − 2 ( x 2 + y 2 ) 0 0 0 0 1 ] R_{4,4}=\begin{bmatrix} 1-2(y^2+z^2) & 2(xy-zw) & 2(xz+yw) & 0\\ 2(xy+zw) & 1-2(x^2+z^2) & 2(yz-xw) & 0\\ 2(xz-yw) & 2(yz+xw) & 1-2(x^2+y^2) & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} R4,4= 1−2(y2+z2)2(xy+zw)2(xz−yw)02(xy−zw)1−2(x2+z2)2(yz+xw)02(xz+yw)2(yz−xw)1−2(x2+y2)00001
4. 绕点旋转
四元数表示的是绕坐标系原点的旋转，绕点旋转顾名思义就是绕特定点的旋转。其本质上是一种平移+旋转的复合运动。
假定我们有个矩阵[R]定义了绕原点的旋转，现在我们想围绕任意一点P做一样的旋转。
我们可以看到其朝向与围绕原点的旋转是一样的，只是被平移到了另外的位置。
其运动等价为：
- 将物体平移到原点(减去P，使用矢量-Px,-Py,-Pz平移)
- 绕原点进行旋转
- 将物体平移回去(加上P，使用+Px,+Py,+Pz平移)
[变换结果] = [+Px,+Py,+Pz] * [绕原点旋转] * [-Px,-Py,-Pz]
用矩阵形式：
[
R
]
=
[
T
]
−
1
∗
[
R
]
∗
[
T
]
[R] = [T]^{-1} * [R] * [T]
[R]=[T]−1∗[R]∗[T]
其中:
[R] = 旋转矩阵，可以是个四元数
[
T
]
−
1
=
[
1
0
0
P
x
0
1
0
P
y
0
0
1
P
z
0
0
0
1
]
[T]^{-1}=\begin{bmatrix} 1 & 0 & 0 & P_x\\ 0 & 1 & 0 & P_y\\ 0 & 0 & 1 & P_z\\ 0 & 0 & 0 & 1 \end{bmatrix}
[T]−1=
100001000010PxPyPz1
[ T ] = [ 1 0 0 − P x 0 1 0 − P y 0 0 1 − P z 0 0 0 1 ] [T]=\begin{bmatrix} 1 & 0 & 0 & -P_x\\ 0 & 1 & 0 & -P_y\\ 0 & 0 & 1 & -P_z\\ 0 & 0 & 0 & 1 \end{bmatrix} [T]= 100001000010−Px−Py−Pz1
三者相乘得到：
[
r
00
r
01
r
02
P
x
−
r
00
∗
P
x
−
r
01
∗
P
y
−
r
02
∗
P
z
r
10
r
11
r
12
P
y
−
r
10
∗
P
x
−
r
11
∗
P
y
−
r
12
∗
P
z
r
20
r
21
r
22
P
z
−
r
20
∗
P
x
−
r
21
∗
P
y
−
r
22
∗
P
z
0
0
0
1
]
\begin{bmatrix} r_{00} & r_{01} & r_{02} & P_x - r_{00}*P_x - r_{01}*P_y - r_{02}*P_z\\ r_{10} & r_{11} & r_{12} & P_y - r_{10}*P_x - r_{11}*P_y - r_{12}*P_z\\ r_{20} & r_{21} & r_{22} & P_z - r_{20}*P_x - r_{21}*P_y - r_{22}*P_z\\ 0 & 0 & 0 & 1 \end{bmatrix}
r00r10r200r01r11r210r02r12r220Px−r00∗Px−r01∗Py−r02∗PzPy−r10∗Px−r11∗Py−r12∗PzPz−r20∗Px−r21∗Py−r22∗Pz1
可以与绕原点相比，矩阵旋转部分分量是一样的，只是增加了平移部分。
Overload中的绕点公式如下：
// 点p_point绕原点旋转的坐标
OvMaths::FVector3 OvMaths::FQuaternion::RotatePoint(const FVector3& p_point, const FQuaternion& p_quaternion)
{
FVector3 Q(p_quaternion.x, p_quaternion.y, p_quaternion.z);
FVector3 T = FVector3::Cross(Q, p_point) * 2.0f;
return p_point + (T * p_quaternion.w) + FVector3::Cross(Q, T);
}
// 点p_point绕p_pivot旋转后的坐标（这个地方我理解可能有误，转动之后不应该再加回来吗？）
OvMaths::FVector3 OvMaths::FQuaternion::RotatePoint(const FVector3 & p_point, const FQuaternion & p_quaternion, const FVector3 & p_pivot)
{
FVector3 toRotate = p_point - p_pivot;
return RotatePoint(toRotate, p_quaternion);
}
其使用了另外的一个公式，这个公式的出处我没用找到，应该等价于上面的公式，具体我没有做推导验证。这个函数没有看太明白，希望大佬们能留言指点，多谢！
1796
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
