不带Y轴的X和Z轴上的Quaternion.Slerp

using System.Numerics;

public class QuaternionSlerpExample
{
    public static Quaternion SlerpWithoutY(Quaternion q1, Quaternion q2, float t)
    {
        // 确保q1和q2的y分量为0
        q1 = new Quaternion(q1.W, 0, q1.Z, q1.X);
        q2 = new Quaternion(q2.W, 0, q2.Z, q2.X);

        // 计算两个四元数的点积
        float dot = Quaternion.Dot(q1, q2);

        // 如果点积小于0，反转一个四元数以确保插值路径最短
        if (dot < 0.0f)
        {
            q2 = -q2;
            dot = -dot;
        }

        // 如果点积接近1，直接线性插值
        if (dot > 0.9995f)
        {
            return Quaternion.Lerp(q1, q2, t);
        }

        // 计算插值参数
        float theta = (float)Math.Acos(dot);
        float sinTheta = (float)Math.Sin(theta);
        float s0 = (float)Math.Sin((1 - t) * theta) / sinTheta;
        float s1 = (float)Math.Sin(t * theta) / sinTheta;

        // 进行Slerp插值
        return s0 * q1 + s1 * q2;
    }

    public static void Main()
    {
        Quaternion q1 = Quaternion.CreateFromAxisAngle(Vector3.UnitX, MathF.PI / 4);
        Quaternion q2 = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, MathF.PI / 4);

        Quaternion result = SlerpWithoutY(q1, q2, 0.5f);
        Console.WriteLine(result);
    }
}