【Overload游戏引擎细节分析】四元数公式到代码

四元数的公式

本文只记录一下四元数相关的公式，不讲其数学原理，因为我也不懂。只介绍Overload中这些数学公式到代码。

1. 四元数定义

威廉·汉密尔顿于1843年发明了四元数，作为一种允许对向量进行乘法和除法、旋转和拉伸的方法。四元数是有四个数，由一个标量和一个向量组成的复数。[w,v]，w为标量，v为3D向量，展开后为——[w，(x,y,z)]：
$$\mathbf{q}=w+x\ast i+y\ast j+z\ast k$$
其中：
$i^2=j^2=k^2=-1$，$ij=-ji=k$，$ki=-ik=j$，$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. 相加
$$\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)$$
对应的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. 相乘
$$\begin{array}{rl}\hat{\mathbf{q}}\times \hat{\mathbf{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)\end{array}$$
对应的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. 规则化
$$\hat{\mathbf{q}}=\hat{\mathbf{q}}/\sqrt{w^2+x^2+y^2+z^2}$$

5. 点积
$$\hat{\mathbf{q}}\cdot \hat{\mathbf{r}}=q_x\ast r_x+q_y\ast r_y+q_z\ast r_z+q_w\ast r_w$$
代码：

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的点积是$cos(\theta )$, 进而用acos函数可以求出$\theta$
$$\theta =acos(\hat{\mathbf{q}}\cdot \hat{\mathbf{r}})$$
给出插值量t，0 $$Q_m=\frac{\hat{\mathbf{q}}\ast sin((1-t)\ast \theta )+\hat{\mathbf{r}}sin(t\ast \theta )}{sin(\theta )}$$

对应的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);
	}
}

当两个四元数角度较小时， $sin(\theta )$存在除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
计算出的四元数也是单位化的，所以：
$$cos(angle/2{)}^2+ax\ast ax\ast sin(angle/2{)}^2+ay\ast ay\ast sin(angle/2{)}^2+az\ast az\ast sin(angle/2{)}^2=1$$

4.四元数转轴角

$$x=qx/sqrt(1-qw\ast qw)$$
$$y=qy/sqrt(1-qw\ast qw)$$
$$z=qz/sqrt(1-qw\ast qw)$$
$$angle=2\ast 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)0​2(xy−zw)1−2(x2+z2)2(yz+xw)0​2(xz+yw)2(yz−xw)1−2(x2+y2)0​0001​ ​

4. 绕点旋转

四元数表示的是绕坐标系原点的旋转，绕点旋转顾名思义就是绕特定点的旋转。其本质上是一种平移+旋转的复合运动。
假定我们有个矩阵[R]定义了绕原点的旋转，现在我们想围绕任意一点P做一样的旋转。
我们可以看到其朝向与围绕原点的旋转是一样的，只是被平移到了另外的位置。

其运动等价为：

1. 将物体平移到原点(减去P，使用矢量-Px,-Py,-Pz平移)
2. 绕原点进行旋转
3. 将物体平移回去(加上P，使用+Px,+Py,+Pz平移)

[变换结果] = [+Px,+Py,+Pz] * [绕原点旋转] * [-Px,-Py,-Pz]

用矩阵形式：
$$[R]=[T{]}^{-1}\ast [R]\ast [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= ​1000​0100​0010​Px​Py​Pz​1​ ​

[ 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]= ​1000​0100​0010​−Px​−Py​−Pz​1​ ​

三者相乘得到：
[ 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} ​r00​r10​r20​0​r01​r11​r21​0​r02​r12​r22​0​Px​−r00​∗Px​−r01​∗Py​−r02​∗Pz​Py​−r10​∗Px​−r11​∗Py​−r12​∗Pz​Pz​−r20​∗Px​−r21​∗Py​−r22​∗Pz​1​ ​
可以与绕原点相比，矩阵旋转部分分量是一样的，只是增加了平移部分。

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);
}

其使用了另外的一个公式，这个公式的出处我没用找到，应该等价于上面的公式，具体我没有做推导验证。这个函数没有看太明白，希望大佬们能留言指点，多谢！