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

四元数的公式

本文只记录一下四元数相关的公式,不讲其数学原理,因为我也不懂。只介绍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+xi+yj+zk
其中:
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)=qwrwqxrxqyryqzrz+i(qyrzqzry+rwqx+qwrx)+j(qzrxqxrz+rwqy+qwry)+k(qxryqyrx+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^=qxrx+qyry+qzrz+qwrw
代码:

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 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((1t)θ)+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+axaxsin(angle/2)2+ayaysin(angle/2)2+azazsin(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(1qwqw)
y = q y / s q r t ( 1 − q w ∗ q w ) y = qy / sqrt(1-qw*qw) y=qy/sqrt(1qwqw)
z = q z / s q r t ( 1 − q w ∗ q w ) z = qz / sqrt(1-qw*qw) z=qz/sqrt(1qwqw)
a n g l e = 2 ∗ a c o s ( q w ) angle = 2 * acos(qw) angle=2acos(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-qw
    qw) = 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= 12(y2+z2)2(xy+zw)2(xzyw)2(xyzw)12(x2+z2)2(yz+xw)2(xz+yw)2(yzxw)12(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= 12(y2+z2)2(xy+zw)2(xzyw)02(xyzw)12(x2+z2)2(yz+xw)02(xz+yw)2(yzxw)12(x2+y2)00001

4. 绕点旋转

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

其运动等价为:

  1. 将物体平移到原点(减去P,使用矢量-Px,-Py,-Pz平移)
  2. 绕原点进行旋转
  3. 将物体平移回去(加上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]= 100001000010PxPyPz1

三者相乘得到:
[ 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} r00r10r200r01r11r210r02r12r220Pxr00Pxr01Pyr02PzPyr10Pxr11Pyr12PzPzr20Pxr21Pyr22Pz1
可以与绕原点相比,矩阵旋转部分分量是一样的,只是增加了平移部分。

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

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

你可能感兴趣的:(游戏引擎)