Quaternion formula
This article only records the formulas related to quaternions, and does not talk about its mathematical principles, because I don’t understand them either. Only introduce these mathematical formulas in Overload to the code.
1. Definition of quaternion
William Hamilton invented quaternions in 1843 as a method that allowed vectors to be multiplied and divided, rotated and stretched. A quaternion is a complex number consisting of four numbers, a scalar and a vector. [w,v], w is a scalar, v is a 3D vector, after expansion it is——[w, (x,y,z)]: q = w
+ x ∗ i + y ∗ j + z ∗ k \mathbf{ q} = w + x*i+y*j+z*kq=w+x∗i+y∗j+z∗k
in:
i 2 = j 2 = k 2 = − 1 i^2=j^2=k^2=-1i2=j2=k2=−1， i j = − j i = k ij=-ji=k ij=−ji=k，ki = − ik = j ki=-ik=jto=− i=j， j k = − k j = i jk=-kj=i jk=−kj=i，
For any given quaternion, a rotation in 3D space can be represented. In 3D space, any rotation can be expressed as a rotation angle around an axis. As the name suggests, the axis angle is an axis A (ax, ay, az) plus a rotation angle angle. Convert quaternions to axis angles. So any given quaternion can represent a rotation in 3D space.
Take a look at the definition of quaternions in Overload. It just encapsulates 4 numbers and defines a lot of functions.
struct FQuaternion
{
public:
float x; // 矢量部分
float y;
float z;
float w; // 标量部分
}
2. Common calculations of quaternions
1. 相加
q ^ + r ^ = ( qw , qv ) + ( rw , rv ) = ( qw + rw , qv + rv ) \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)
corresponding Overload code:
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) ( and rx+jry+krz+rw)=qwrw−qxrx−qyry−qzrz+i(qyrz−qzry+rwqx+qwrx)+j(qzrx−qxrz+rwqy+qwry)+k(qxry−qyrx+rwqz+qwrz)
Corresponding Overload code:
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. Invert the conjugate vector part and directly enter the code:
OvMaths::FQuaternion OvMaths::FQuaternion::Conjugate(const FQuaternion & p_target)
{
return {
-p_target.x, -p_target.y, -p_target.z, p_target.w };
}
4. Determine
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
Code:
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. Interpolation
The interpolation of two quaternions q and r can be used to animate the rotation process.
The interpolation calculation must first calculate half of the angle between q and r θ \thetaθ , qfrequencycos ( θ ) cos(\theta)cos ( θ ) , and then use the acos function to findθ \thetaθ
θ = acos ( q ^ ⋅ r ^ ) \theta = acos(\hat{\mathbf{q}} \cdot \that{\mathbf{r}})i=acos(q^⋅r^ )
gives the interpolation quantity t, 0<t<1.0, the interpolation quantity can be obtained:
Q m = q ^ ∗ sin ( ( 1 − t ) ∗ θ ) + r ^ sin ( t ∗ θ ) sin ( θ ) Q_m= \frac{\hat{\mathbf{q}}*sin((1-t)*\theta) + \hat{\mathbf{r}}sin(t*\theta)}{sin(\theta) }Qm=s in ( i )q^∗sin((1−t)∗i )+r^sin(t∗i ).
Corresponding Overload code:
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);
}
}
When the angle between the two quaternions is small, sin ( θ ) sin(\theta)s in ( θ ) has a division by 0 problem, so special treatment is performed.
3. Axis angle to quaternion
Input: rotation axis A (ax, ay, az), rotation angle angle
Output:
qx = ax * sin(angle/2)
qy = ay * sin(angle/2)
qz = az * sin(angle/2)
qw = cos(angle/2)
Among them:
the rotation axis is a unit vector, so: ax ax + ay ay + az*az = 1
The calculated quaternion is also unitized, so:
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 = 1cos(angle/2)2+ax∗ax∗sin(angle/2)2+and y∗and y∗sin(angle/2)2+the z∗the z∗sin(angle/2)2=1
4. Quaternion axis angle
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=q z / s q r t ( 1−qw∗qw)
a n g l e = 2 ∗ a c o s ( q w ) angle = 2 * acos(qw) angle=2∗acos(qw)
- Singularity
Axis calculation is a value with singularity. When angle = 0, there is a division by 0 problem.
q = cos(angle/2) + i ( x * sin(angle/2)) + j (y * sin(angle/2)) + k ( z * sin(angle/2)) when angle is 0
degrees When, the quaternion is q = 1 + i 0 + j 0 + k 0, so when calculating the axis,
angle = 2 * acos(qw) = 0
x = qx / sqrt(1-qw qw) = divide by zero = infinity
y = qy / sqrt(1-qw qw) = divide by zero = infinity
z = qz / sqrt(1-qw*qw) = divide by zero = infinity
but turning 0 degrees is equivalent to no rotation, so the axis given Can be any value.
The corresponding code in 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. Convert quaternion to matrix
Quaternions are mainly used to represent rotations in graphics. For the qw + i qx + j qy + k qz quaternion, the corresponding 3x3 and 4x4 rotation matrices are as follows:
R 3 , 3 = [ 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 ) ] 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−the w ).2(xy−zw)1−2(x2+z2)2(yz+xw)2(xz+the w )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−the w )02(xy−zw)1−2(x2+z2)2(yz+xw)02(xz+the w )2(yz−xw)1−2(x2+y2)00001
4. Rotate around a point
Quaternions represent rotation around the origin of the coordinate system. Rotation around a point, as the name implies, is rotation around a specific point. It is essentially a compound movement of translation + rotation.
Suppose we have a matrix [R] that defines a rotation around the origin, and now we want to do the same rotation around any point P.
We can see that its orientation is the same as the rotation around the origin, but it has been translated to a different position.
Its kinematic equivalent is:
- Translate the object to the origin (minus P, translate using vectors -Px, -Py, -Pz)
- Rotate around origin
- Translate the object back (add P, use +Px, +Py, +Pz to translate)
[Transformation result] = [+Px,+Py,+Pz] * [Rotate around the origin] * [-Px,-Py,-Pz]
In matrix form:
[ R ] = [ T ] − 1 ∗ [ R ] ∗ [ T ] [R] = [T]^{-1} * [R] * [T][R]=[T]−1∗[R]∗[T]
Among them:
[R] = rotation matrix, which can be a quaternion
[ 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 & &0&-P_y\\0&0&1&-P_z\\0&0&0&1\end{bmatrix}[T]= 100001000010−Px−Py−Pz1
Determine the following:
[ r 00 r 01 r 02 P x − r 00 ∗ P x − r 01 ∗ P y − r 02 ∗ P zr 10 r 11 r 12 P y − r 10 ∗ P x − r 11 ∗ P y − r 12 ∗ P zr 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
It can be compared with around the origin, the rotation component of the matrix is the same, only the translation component is added.
The winding point formula in Overload is as follows:
// 点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);
}
It uses another formula. I have not found the source of this formula. It should be equivalent to the above formula. I have not done any derivation and verification. I don’t understand this function very well. I hope you guys can leave a message and give me some advice. Thank you!