我们知道，四元数是除了欧拉角，旋转矩阵之外，主要用来描述旋转的量。四元数直观的定义就是
$$q=[cos(\frac{\theta }{2}),sin(\frac{\theta }{2})N]$$
其中，N表示旋转轴单位向量，$\theta$表示旋转的角度。那么，给定一个任意向量V，绕N旋转$\theta$角度之后的V’可以用四元数乘法表示。令四元数v = [0, V]，v’ = [0, V’]，那么
$$v^{\prime }=qv{q}^{\ast }$$
我们先来推导一下这个公式。

绕任意轴旋转

首先，四元数的乘法公式为
$$q_1=[s,\text{v}]$$

$$q_2=[t,\text{u}]$$

$$q_1{q}_2=[st-\text{v}\cdot \text{u},s\text{u}+t\text{v}+\text{v}\times \text{u}]$$

那么
$$v{q}^{\ast }=[sin(\frac{\theta }{2})N\cdot V,cos(\frac{\theta }{2})V+sin(\frac{\theta }{2})N\times V]$$

$$\begin{gathered} qv{q}^{\ast }=[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)+si{n}^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)] \end{gathered}$$

我们先对结果的实部进行化简，因为点乘满足分配律，所以有
$$\begin{gathered} 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-si{n}^2(\frac{\theta }{2})N\cdot (N\times V) \\ =-si{n}^2(\frac{\theta }{2})N\cdot (N\times V) \end{gathered}$$

N和V的叉乘显然与N垂直，所以点积为0，因此

$$-si{n}^2(\frac{\theta }{2})N\cdot (N\times V)=0$$

再看虚部，同样叉乘也满足分配律，所以有

$$\begin{gathered} cos(\frac{\theta }{2})(cos(\frac{\theta }{2})V+sin(\frac{\theta }{2})N\times V)+si{n}^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) \\ =co{s}^2(\frac{\theta }{2})V+sin(\frac{\theta }{2})cos(\frac{\theta }{2})N\times V+si{n}^2(\frac{\theta }{2})(N\cdot V)N+sin(\frac{\theta }{2})cos(\frac{\theta }{2})N\times V+si{n}^2(\frac{\theta }{2})N\times (N\times V) \end{gathered}$$

注意，叉乘并不满足结合律，且有
$$\text{a}\times (\text{b}\times \text{c})=(\text{a}\cdot \text{c})\text{b}-(\text{a}\cdot \text{b})\cdot \text{c}$$
所以上式进一步化简得到
$$\begin{gathered} co{s}^2(\frac{\theta }{2})V+2sin(\frac{\theta }{2})cos(\frac{\theta }{2})N\times V+si{n}^2(\frac{\theta }{2})(N\cdot V)N+si{n}^2(\frac{\theta }{2})((N\cdot V)N-V) \\ =(co{s}^2(\frac{\theta }{2})-si{n}^2(\frac{\theta }{2}))V+2si{n}^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) \end{gathered}$$
不难发现，这个就是绕任意轴旋转的公式。

当然，在真正实现中，没必要按部就班地用这种方式计算。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所用的其实就是上述公式的反推。推导如下：
$$\begin{gathered} cos\theta V+(1-cos\theta )(N\cdot V)N+sin\theta (N\times V) \\ =V-2si{n}^2(\frac{\theta }{2})V+2si{n}^2(\frac{\theta }{2})(N\cdot V)N+2sin(\frac{\theta }{2})cos(\frac{\theta }{2})(N\times V) \\ =V-2si{n}^2(\frac{\theta }{2})V+2(Q\cdot V)Q+2w(Q\times V) \\ =V-2si{n}^2(\frac{\theta }{2})V+2(Q\times (Q\times V)+(Q\cdot Q)V)+2w(Q\times V) \\ =V-2si{n}^2(\frac{\theta }{2})V+2Q\times (Q\times V)+2si{n}^2(\frac{\theta }{2})V+2w(Q\times V) \\ =V+2w(Q\times V)+2Q\times (Q\times V) \end{gathered}$$

四元数与矩阵

我们知道，矩阵也可以用来表示3D的旋转。绕任意轴旋转的矩阵为
$$\left(\begin{array}{cccc}cos\alpha +r_x^2(1-cos\alpha )& r_x{r}_y(1-cos\alpha )+r_{z}sin\alpha & r_x{r}_z(1-cos\alpha )-r_{y}sin\alpha & 0\\ r_x{r}_y(1-cos\alpha )-r_{z}sin\alpha & cos\alpha +r_y^2(1-cos\alpha )& r_y{r}_z(1-cos\alpha )+r_{x}sin\alpha & 0\\ r_x{r}_z(1-cos\alpha )+r_{y}sin\alpha & r_y{r}_z(1-cos\alpha )-r_{x}sin\alpha & cos\alpha +r_z^2(1-cos\alpha )& 0\\ 0& 0& 0& 1\end{array}\right)$$
假设四元数为
$$q=[w,x,y,z]$$
对于矩阵的第一行第一列，有
$$\begin{gathered} cos\alpha +r_x^2(1-cos\alpha ) \\ =2co{s}^2\frac{\alpha }{2}-1+r_x^2(2si{n}^2\frac{\alpha }{2}) \\ =2w^2-1+2(r_{x}sin\frac{\alpha }{2}{)}^2 \\ =2w^2-1+2x^2 \\ =2(1-y^2-z^2)-1 \\ =1-2(y^2+z^2) \end{gathered}$$
对于矩阵的第一行第二列，有
$$\begin{gathered} r_x{r}_y(1-cos\alpha )+r_{z}sin\alpha \\ =r_x{r}_y(2si{n}^2\frac{\alpha }{2})+r_z(2sin\frac{\alpha }{2}cos\frac{\alpha }{2}) \\ =2(r_{x}sin\frac{\alpha }{2})(r_{y}sin\frac{\alpha }{2})+2cos\frac{\alpha }{2}(r_{z}sin\frac{\alpha }{2}) \\ =2xy+2wz \end{gathered}$$
其他部分类似，最终可得到用四元数表示的旋转矩阵为

$$\left(\begin{array}{cccc}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{array}\right)$$
与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 normalizedensure(IsNormalized());
#endifconst 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
$$\begin{gathered} 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 \end{gathered}$$
如果trace大于0，就能解出w的值，进而快速解出x，y和z了，而如果trace小于等于0，则需要换一个思路。对于矩阵对角线上的三个元素，我们取出最大的元素，这里不妨假设
$$M[2][2]>M[1][1]>M[0][0]$$
那么有
$$\begin{gathered} 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\le x^2<y^2<z^2 \end{gathered}$$
可以发现，$z^2$必定大于0，它是一个可以作为分母的值，那么只需构造一个包含$z^2$的式子，把z解出即可。
$$M[2][2]-M[1][1]-M[0][0]+1=4z^2$$
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 negativeint32 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轴依次旋转α，β，γ角度可以得到最终的四元数为
$$\begin{gathered} q=q_{\gamma }{q}_{\beta }{q}_{\alpha } \\ =\left[\begin{array}{c}cos(\frac{\gamma }{2})\\ 0\\ 0\\ sin(\frac{\gamma }{2})\end{array}\right]\left[\begin{array}{c}cos(\frac{\beta }{2})\\ 0\\ -sin(\frac{\beta }{2})\\ 0\end{array}\right]\left[\begin{array}{c}cos(\frac{\alpha }{2})\\ -sin(\frac{\alpha }{2})\\ 0\\ 0\end{array}\right] \\ =\left[\begin{array}{c}cos(\frac{\alpha }{2})cos(\frac{\beta }{2})cos(\frac{\gamma }{2})+sin(\frac{\alpha }{2})sin(\frac{\beta }{2})sin(\frac{\gamma }{2})\\ cos(\frac{\alpha }{2})sin(\frac{\beta }{2})sin(\frac{\gamma }{2})-sin(\frac{\alpha }{2})cos(\frac{\beta }{2})cos(\frac{\gamma }{2})\\ -cos(\frac{\alpha }{2})sin(\frac{\beta }{2})cos(\frac{\gamma }{2})-sin(\frac{\alpha }{2})cos(\frac{\beta }{2})sin(\frac{\gamma }{2})\\ cos(\frac{\alpha }{2})cos(\frac{\beta }{2})sin(\frac{\gamma }{2})-sin(\frac{\alpha }{2})sin(\frac{\beta }{2})cos(\frac{\gamma }{2})\end{array}\right] \end{gathered}$$

根据上述公式，解出roll，pitch，和yaw
$$\alpha =arctan(\frac{-2(wx+yz)}{1-2(x^2+y^2)})$$

$$\beta =arcsin(2(zx-wy))$$

$$\gamma =arctan(\frac{2(wz+xy)}{1-2(y^2+z^2)})$$

不过这里有个特殊情况，我们知道欧拉角是可能导致万向锁的，例如当pitch = 90°时，roll和yaw会变成一个自由度。此时四元数的值为
$$q=\left[\begin{array}{c}\frac{\sqrt{2}}{2}cos(\frac{\alpha -\gamma }{2})\\ -\frac{\sqrt{2}}{2}sin(\frac{\alpha -\gamma }{2})\\ -\frac{\sqrt{2}}{2}cos(\frac{\alpha -\gamma }{2})\\ -\frac{\sqrt{2}}{2}sin(\frac{\alpha -\gamma }{2})\end{array}\right]$$
可以发现此时
$$wx+yz=0$$

$$wz+xy=0$$

$$y^2+z^2=\frac{1}{2}$$

$$x^2+y^2=\frac{1}{2}$$

这会导致arctan里出现0除0，因此并不能采用上述推导的公式进行计算。我们知道，pitch = 90°时当且仅当
$$zx-wy=\frac{1}{2}$$
并且
$$tan\frac{\alpha -\gamma }{2}=-\frac{x}{w}$$
即
$$\alpha =\gamma -2arctan(\frac{x}{w})$$
通常令yaw=0°，得到
$$\alpha =-2arctan(\frac{x}{w})$$
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=[cos(\frac{\theta }{2}),sin(\frac{\theta }{2})\frac{N}{||N||}]$$
旋转轴可由两个向量的叉乘得到，假设两个向量均为单位向量，那么
$$\begin{gathered} q=[cos(\frac{\theta }{2}),sin(\frac{\theta }{2})\frac{A\times B}{sin\theta }] \\ =[cos(\frac{\theta }{2}),\frac{A\times B}{2cos(\frac{\theta }{2})}] \end{gathered}$$
再对q进行数乘
$$\begin{gathered} 2cos(\frac{\theta }{2})q=[2co{s}^2(\frac{\theta }{2}),A\times B] \\ =[1+cos\theta ,A\times B] \\ =[1+A\cdot B,A\times B] \end{gathered}$$
这样，我们可以仅用两个向量的点乘和叉乘，就能得到表示旋转的四元数，最后将其归一化即可。不过，这里还需要考虑两个向量共线的特殊情况。当两个向量共线时，叉乘为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 directionsW = 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_1$，如何从$q_0$变换到$q_1$呢？我们知道四元数本身可以表示旋转，所以如果有向量v，那么

经过$q_0$变换有
$$v_0=q_{0}v{q}_0^{\ast }$$
经过$q_1$变换有
$$v_1=q_{1}v{q}_1^{\ast }$$
换言之，从$q_0$变换到$q_1$，可以认为等同于将$v_0$变换到$v_1$，假设变换的四元数为$\Delta q$，有
$$v_1=\Delta q{v}_0\Delta q^{\ast }$$

$$q_{1}v{q}_1^{\ast }=(\Delta q{q}_0)v(\Delta q{q}_0{)}^{\ast }$$

所以
$$q_1=\Delta q{q}_0$$

$$\begin{gathered} \Delta q=q_1{q}_0^{\ast } \\ \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] \end{gathered}$$

$\Delta q$的实部部分，刚好是把$q_0$，$q_1$视为四维向量的点乘。而$\Delta q$本身也可以表示旋转，它的实部也可写成
$$cos\frac{\varphi }{2}=q_0\cdot q_1=||q_0||||q_1||cos\theta =cos\theta$$
也就是说，$q_0$和$q_1$作为四维向量的夹角，刚好是它们之间的旋转变化量$\Delta q$所表示的角度的一半。

所以，对四元数进行插值，可以理解为对四维向量进行插值。如果做Slerp，假设四维向量间的夹角为θ，要插值的角度为tθ，得到的四维向量为
$$q_t=a{q}_0+b{q}_1$$
式子两边都点乘$q_0$，得到
$$q_0{q}_t=a{q}_0^2+b{q}_0{q}_1$$

$$cos(t\theta )=a+bcos\theta$$

式子两边都点乘$q_1$，得到
$$q_1{q}_t=a{q}_0{q}_1+b{q}_1^2$$

$$cos((1-t)\theta )=acos\theta +b$$

解方程得到
$$a=\frac{sin((1-t)\theta )}{sin\theta }$$

$$b=\frac{sin(t\theta )}{sin\theta }$$

所以
$$q_t=\frac{sin((1-t)\theta )}{sin\theta }{q}_0+\frac{sin(t\theta )}{sin\theta }{q}_1$$
值得注意的是，四元数和3D旋转并不是一一对应的，一个旋转既可以表示为绕旋转轴正方向旋转θ角度，也可以表示为绕旋转轴负方向旋转$2\pi -\theta$角度。

$$q_1=[cos(\frac{\theta }{2}),sin(\frac{\theta }{2})N]$$

$$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$$

可以看到表示同一旋转的两个四元数相互为负。所以在四元数插值时，要先选择一个最短的旋转路径。检测方式就是计算两个四元数的夹角，如果为负数将某一个取反即可。

另外，如果四元数的夹角很小，那么夹角的正弦值可能趋近于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