OpenGL学习记录（2）旋转的表示方式与优异性

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

/// <summary>
/// EulerAngle
/// </summary>
public class EulerAngle
{
    //heading - pitch - bank
    //yaw - pitch - roll
    private float heading;
    private float pitch;
    private float bank;

    private float kPi = Mathf.PI;
    private float k2Pi = Mathf.PI * 2.0f;
    private float kPiOver2 = Mathf.PI / 2.0f;
    //private float k1OverPi = 1.0f / Mathf.PI;//π分之一
    private float k1Over2Pi = 1.0f / Mathf.PI * 2.0f; //2π分之一

    public EulerAngle()
    {
    }

    public EulerAngle(float heading, float pitch, float bank)
    {
        this.Heading = heading;
        this.Pitch = pitch;
        this.Bank = bank;
    }

    private float WrapKPI(float theta)
    {
        theta += kPi;
        theta -= Mathf.Floor(theta * k1Over2Pi) * k2Pi;
        theta -= kPi;
        return theta; //-180°~180°
    }

    /// <summary>
    /// 万向锁角度规避处理
    /// heading：-180°~+180°（左右）
    /// bank：-180°~+180°（翻滚）
    /// pitch：-90°~+90°（俯仰）
    /// </summary>
    public void Canonize()
    {
        this.Pitch = this.WrapKPI(this.heading);
        if (this.Pitch < -kPiOver2) //x axis
        {
            Pitch = -kPi - Pitch;
            Heading += kPi;
            bank += kPi;
        }
        else if (this.Pitch > kPiOver2)
        {
            Pitch = kPi - Pitch;
            Heading += kPi;
            Bank += kPi;
        }

        if (Mathf.Abs(Pitch) > kPiOver2 - 1e-4)
        {
            Heading += Bank;
            Bank = 0.0f;
        }
        else
        {
            Bank = this.WrapKPI(Bank);
        }
        Heading = this.WrapKPI(Heading);
    }

    /// <summary>
    /// 将ObjectMatrixToWorldMatrix矩阵(物体转世界)旋转矩阵转换成欧拉角
    /// </summary>
    /// <param name="m"></param>
    public void FromObjectToWorldMatrix(Matrix4x3 m)
    {
        //heading = atan2(m31,m33)
        //pitch = asin(-m32)
        //bank = atan2(m12,m22)

        float sp = -m.M32;
        //check gloab lock?
        if (Mathf.Abs(sp) > 0.99999f) //sin90 = 1.0f，这里表示无限接近90
        {
            Pitch = Mathf.PI / 2 * sp;
            Bank = 0.0f;
            Heading = Mathf.Atan2(-m.M23, m.M33);
        }
        else //没有发生万向锁，则还是按照正常反三角函数公式计算欧拉
        {
            Heading = Mathf.Atan2(m.M31, m.M33);
            Pitch = Mathf.Asin(sp);
            Bank = Mathf.Atan2(m.M21, m.M22);
        }
    }

    /// <summary>
    /// 将WorldMatrixToObjectMatrix矩阵（世界转物体）旋转矩阵转换成欧拉角
    /// </summary>
    /// <param name="m"></param>
    public void FromWorldToObjectMatrix(Matrix4x3 m)
    {
        //**m is dispose.
        float sp = -m.M23;
        //check gloab lock?
        if (Mathf.Abs(sp) > 0.99999f)
        {
            Pitch = Mathf.PI / 2 * sp;
            Bank = 0.0f;
            Heading = Mathf.Atan2(-m.M31, m.M11);
        }
        else
        {
            Heading = Mathf.Atan2(-m.M31, m.M11);
            Pitch = Mathf.Asin(sp);
            Bank = Mathf.Atan2(m.M12, m.M22);
        }
    }

    /// <summary>
    /// 从RotationMatrix（物体-惯性-世界、世界-惯性-物体）旋转矩阵转换成对应的欧拉角
    /// </summary>
    /// <param name="rt"></param>
    public void FromRotationMatrix(RotationMatrix rt)
    {
        float sp = -rt.M23;
        //check gloab lock?
        if (Mathf.Abs(sp) > 0.99999f)
        {
            Pitch = Mathf.PI / 2 * sp;
            Bank = 0.0f;
            Heading = Mathf.Atan2(-rt.M31, rt.M11);
        }
        else
        {
            Heading = Mathf.Atan2(-rt.M31, rt.M11);
            Pitch = Mathf.Asin(sp);
            Bank = Mathf.Atan2(rt.M12, rt.M22);
        }
    }


    /// <summary>
    /// 四元数转换成欧拉角（物体坐标空间到惯性坐标空间）
    /// </summary>
    /// <param name="q">四元数</param>
    public void  FormObjectToInertail(QuaternionX q)
    {
        //prich = arcsin(-m32) = arcsin(-2(yz - wx))
        float sp = -2.0f * (q.Y * q.Z - q.W * q.Z);
        if (Mathf.Abs(sp) > 0.9999f)
        {
            //检测是否发生万向锁
            Pitch = kPiOver2 * sp;
            //atan2( -xz + wy , 0.5 - y(^2) - z(^2))
            Heading = Mathf.Atan2(-q.X * q.Z + q.W * q.Y, 0.5f - q.Y * q.Y - q.Z * q.Z);
            Bank = 0.0f;
        }
        else
        {
            // arcsin(-2(yz - wx))
            Pitch = Mathf.Asin(sp);
            //atan2( xz + wy, 1/2 - x(^2)- y(^2))
            Heading = Mathf.Atan2(q.X * q.Z + q.W * q.Y, 0.5f - q.X * q.X - q.Y * q.Y);
            //atan2( xz + wz, 1/2 - x(^2)- z(^2))
            Bank = Mathf.Atan2(q.X * q.Y + q.W * q.Z, 0.5f - q.X * q.X - q.Z * q.Z);
        }
    }


    /// <summary>
    /// 四元数转换成欧拉角（惯性坐标空间到物体坐标空间）使用四元数共轭
    /// </summary>
    /// <param name="q">四元数</param>
    public void FromInertailToObject(QuaternionX q)
    {        
        //prich = arcsin(-m32) = arcsin(-2(yz - wx))
        float sp = -2.0f * (q.Y * q.Z + q.W * q.Z);
        if (Mathf.Abs(sp) > 0.9999f)
        {
            //检测是否发生万向锁
            Pitch = kPiOver2 * sp;
            //atan2( -xz + wy , 0.5 - y(^2) - z(^2))
            Heading = Mathf.Atan2(-q.X * q.Z - q.W * q.Y, 0.5f - q.Y * q.Y - q.Z * q.Z);
            Bank = 0.0f;
        }
        else
        {
            // arcsin(-2(yz - wx))
            Pitch = Mathf.Asin(sp);
            //atan2( xz + wy, 1/2 - x(^2)- y(^2))
            Heading = Mathf.Atan2(q.X * q.Z - q.W * q.Y, 0.5f - q.X * q.X - q.Y * q.Y);
            //atan2( xz + wz, 1/2 - x(^2)- z(^2))
            Bank = Mathf.Atan2(q.X * q.Y - q.W * q.Z, 0.5f - q.X * q.X - q.Z * q.Z);
        }

    }

    public float Heading { get => heading; set => heading = value; }
    public float Pitch { get => pitch; set => pitch = value; }
    public float Bank { get => bank; set => bank = value; }
}

using UnityEngine;

/// <summary>
/// QuaternionX
/// </summary>
public class QuaternionX
{
    public const float kEpsilonX = 1E-06F;
    //public QuaternionX kQuaternionIndentity = new QuaternionX(1.0f, 0.0f, 0.0f, 0.0f);

    private float x;
    private float y;
    private float z;
    private float w;

    public float X { get => x; set => x = value; }
    public float Y { get => y; set => y = value; }
    public float Z { get => z; set => z = value; }
    public float W { get => w; set => w = value; }

    public QuaternionX(float x, float y, float z, float w)
    {
        X = x;
        Y = y;
        Z = z;
        W = w;
    }
    public QuaternionX()
    {

    }

    /// <summary>
    /// 设置单位四元数
    /// </summary>
    public void Identify()
    {
        W = 1.0f;
        X = Y = Z = 0.0f;
    }

    /// <summary>
    /// 求绕着指定坐标轴进行旋转的四元数分量
    /// </summary>
    /// <param name="axis">指定坐标轴</param>
    /// <param name="α">指定的旋转角度</param>
    public void SetToRotationAboutAxis(int axis, float α)
    {
        float cos = Mathf.Cos(α * 0.5f);
        float sin = Mathf.Sin(α * 0.5f);
        switch (axis)
        {
            case 1:
                W = cos;
                X = sin * 1.0f;
                Y = 0.0f;
                Z = 0.0f;
                break;
            case 2:
                W = cos;
                X = 0.0f;
                Y = sin * 1.0f;
                Z = 0.0f;
                break;
            case 3:
                W = cos;
                X = 0.0f;
                Y = 0.0f;
                Z = sin * 1.0f;
                break;
        }
    }

    /// <summary>
    /// 求绕着3d空间中任意指定旋转轴旋转的四元数
    /// </summary>
    /// <param name="vectorAxis">指定的任意旋转轴单位向量</param>
    /// <param name="α">旋转角度</param>
    public void SetToRotateAboutVectorAxis(Vector3D vectorAxis, float α)
    {
        float cos = Mathf.Cos(α * 0.5f);
        float sin = Mathf.Sin(α * 0.5f);
        W = cos;
        X = sin * vectorAxis.X;
        Y = sin * vectorAxis.Y;
        Z = sin * vectorAxis.Z;
    }

    /// <summary>
    /// 从四元数提取旋转角度
    /// </summary>
    /// <returns></returns>
    public float GetRotationAngleFromQuaternion()
    {
        return SafeAcos(W) * 2.0f;
    }

    /// <summary>
    /// 校验三角函数值域
    /// </summary>
    /// <param name="α">角度值</param>
    private float SafeAcos(float α)
    {
        if (α <= -1.0f)
        {
            return Mathf.PI;
        }
        if (α >= 1.0f)
        {
            return 0.0f;
        }
        return Mathf.Acos(α);
    }

    /// <summary>
    /// 从四元数提取旋转轴
    /// </summary>
    /// <returns></returns>
    public Vector3D GetRotationAxisFromQuaternion()
    {
        // q = [w (x y z)]
        //sinα^2 + cosα^2 = 1; //四元数模公式推导
        float sinα1Over2Sq = 1 - W * W; // 因为cosα = w 所以有， sinα^2 = 1- cosα^2  = 1- cosα^2=1- w*w  
        float sinα1Over2 = Mathf.Sqrt(sinα1Over2Sq); //求出sin半角
        float k = 1.0f / sinα1Over2;
        //x / sinα1Over2 = nx
        //y / sinα1Over2 = ny
        //z / sinα1Over2 = nz

        float nx = X * k;
        float ny = Y * k;
        float nz = Z * k;
        return new Vector3D(nx, ny, nz);
    }

    /// <summary>
    /// 四元数和向量乘法运算重载
    /// </summary>
    /// <param name="rotation"></param>
    /// <param name="point"></param>
    /// <returns></returns>
    public static Vector3D operator *(QuaternionX rotation, Vector3 point)
    {
        return new Vector3D(0.0f, 1.0f, 2.0f);
    }

    /// <summary>
    /// 四元数和四元数乘法运算重载（计算四元数叉乘），采用非标准计算公式
    /// </summary>
    /// <param name="lhs"></param>
    /// <param name="rhs"></param>
    /// <returns></returns>
    public static QuaternionX operator *(QuaternionX q1, QuaternionX q2)
    {
        QuaternionX result = new QuaternionX();
        result.W = q1.W * q2.W - q1.X * q2.X - q1.Y * q2.Y - q1.Z * q2.Z;
        result.X = q1.W * q2.X + q1.X * q2.W + q1.Y * q2.Z - q1.Z * q2.Y;
        result.Y = q1.W * q2.Y + q1.Y * q2.W + q1.Z * q2.X - q1.X * q2.Z;
        result.Z = q1.W * q2.Z + q1.Z * q2.W + q1.X * q2.Y - q1.Y * q2.X;
        return result;
    }

    /// <summary>
    /// 规范化四元数模，解决浮点计算的累计误差
    /// </summary>
    public void Normalize()
    {
        float mag = (float)Mathf.Sqrt(W * W + X * X + Y * Y + Z * Z);
        if (mag > 0.0f)
        {
            float oneOverMag = 1.0f / mag;
            W = W * oneOverMag;
            X = X * oneOverMag;
            Y = Y * oneOverMag;
            Z = Z * oneOverMag;
        }
        else
        {
            Identify();
        }
    }

    /// <summary>
    /// 计算两个四元数点乘
    /// </summary>
    /// <param name="q1"></param>
    /// <param name="q2"></param>
    /// <returns></returns>
    public float Dot(QuaternionX q1, QuaternionX q2)
    {
        return q1.W * q2.W + q1.X + q2.X + q1.Y + q1.Y + q1.Z + q2.Z;
    }

    /// <summary>
    /// 计算四元数共轭
    /// </summary>
    /// <param name="q1"></param>
    /// <returns></returns>
    public QuaternionX Conjugate(QuaternionX q1)
    {
        QuaternionX q2 = new QuaternionX();
        q2.W = q1.W;
        q2.X = -q1.X;
        q2.Y = -q1.Y;
        q2.Z = -q1.Z;
        return q2;
    }

    /// <summary>
    /// 计算四元数幂次
    /// </summary>
    /// <param name="q">四元数</param>
    /// <param name="exponent">幂次</param>
    /// <returns></returns>
    public QuaternionX Pow(QuaternionX q, float exponent)
    {
        //q^t = exp (t log q) = exp(t [0,αn]) = exp([0,tαn]),其中t为指数exponent = exp p = [cosα , nsinα],其中α为newAlpha
        if (Mathf.Abs(q.W) > 0.9999f) //w 必须 <= 1,因为w是旋转角，当旋转角大于1时，计算的cos和sin就为0，导致后面计算会出现除以0无意义的计算
        {
            return q;
        }

        float alpha = Mathf.Acos(q.W);//半角
        float newAlpha = exponent * alpha;

        QuaternionX result = new QuaternionX();
        result.W = Mathf.Cos(newAlpha);

        float mult = Mathf.Sin(newAlpha) / Mathf.Sin(alpha);
        result.X = q.X * mult;
        result.Y = q.Y * mult;
        result.Z = q.Z * mult;

        return result;
    }

    /// <summary>
    /// 四元数转换成矩阵（从惯性空间到物体空间）
    /// </summary>
    /// <param name="q"></param>
    /// <returns></returns>
    public Matrix3x3 InnetialQuaternionXToMatrix(QuaternionX q)
    {
        //  | 1 - 2y(^2) - 2z(^2)        2xy + 2wz        2xz - 2wy |
        //  | 2xy - 2wz       1 - 2x(^2) - 2z(^2)     2yz + 2wx |
        //  | 2xz + 2wy        2yz - 2wx     1 - 2x(^2) - 2y(^2) |
        Matrix3x3 mt = new Matrix3x3();
        mt.M11 = 1.0f - 2.0f * (q.Y * q.Y) - 2.0f * (q.Z * q.Z);
        mt.M12 = 2.0f * q.X * q.Y + 2.0f * q.W * q.Z;
        mt.M13 = 2.0f * q.X * q.Z - 2.0f * q.W * q.Y;

        mt.M21 = 2.0f * q.X * q.Y - 2.0f * q.W * q.Z;
        mt.M22 = 1.0f - 2.0f * (q.X * q.X) - 2.0f * (q.Z * q.Z);
        mt.M23 = 2.0f * q.Y * q.Z + 2.0f * q.W * q.Z;

        mt.M31 = 2.0f * q.X * q.Z + 2.0f * q.W * q.Y;
        mt.M32 = 2.0f * q.Y * q.Z - 2.0f * q.W * q.X;
        mt.M33 = 1.0f - 2.0f * (q.X * q.X) - 2.0f * (q.Y * q.Y);

        return mt;
    }

    /// <summary>
    /// 四元数转换矩阵（从物体坐标系空间到惯性坐标系空间）
    /// </summary>
    /// <param name="q">四元数</param>
    /// <returns></returns>
    public Matrix3x3 ObjectQuaternionXToMatrix(QuaternionX q)
    {
        //  |1 - 2y(^2) - 2z(^2)   2xy - 2wz                2xz + 2wy  |
        //  |2xy + 2wz           1 - 2x(^2) - 2z(^2)         2yz - 2wx |
        //  |2xz - 2wy           2yz + 2wx                      1 - 2x(^2) - 2y(^2) |

        Matrix3x3 mt = new Matrix3x3();
        mt.M11 = 1.0f - 2.0f * (q.Y * q.Y) - 2.0f * (q.Z * q.Z);
        mt.M12 = 2.0f * q.X * q.Y - 2.0f * q.W * q.Z;
        mt.M13 = 2.0f * q.X * q.Z + 2.0f * q.W * q.Y;

        mt.M21 = 2.0f * q.X * q.Y + 2.0f * q.W * q.Z;
        mt.M22 = 1.0f - 2.0f * (q.X * q.X) - 2.0f * (q.Z * q.Z);
        mt.M23 = 2.0f * q.Y * q.Z - 2.0f * q.W * q.X;

        mt.M31 = 2.0f * q.X * q.Z - 2.0f * q.W * q.Y;
        mt.M32 = 2.0f * q.Y * q.Z + 2.0f * q.W * q.Z;
        mt.M33 = 1.0f - 2.0f * (q.X * q.X) - 2.0f * (q.Y * q.Y);

        return mt;
    }

    /// <summary>
    /// 从旋转矩阵计算四元数，返回四元数
    /// </summary>
    /// <param name="rt"></param>
    public void FromRotationMatrix(RotationMatrix rt)
    {
        float m11 = rt.M11;
        float m12 = rt.M12;
        float m13 = rt.M13;

        float m21 = rt.M21;
        float m22 = rt.M22;
        float m23 = rt.M23;

        float m31 = rt.M31;
        float m32 = rt.M32;
        float m33 = rt.M33;

        float tr_w = m11 + m22 + m33; //4w(^2) -1
        float tr_x = m11 - m22 - m33;//4x(^2) -1
        float tr_y = m22 - m11 - m33;//4y(^2) -1
        float tr_z = m33 - m11 - m22;//4z(^2) -1

        //比较4个量中的最大值
        int index = 0;
        float markonBigValue = tr_w; //假定w是最大
        //检测x
        if (tr_x > markonBigValue)
        {
            markonBigValue = tr_x;
            index = 1;
        }
        //检测y
        if (tr_y > markonBigValue)
        {
            markonBigValue = tr_y;
            index = 2;
        }
        //检测z
        if (tr_z > markonBigValue)
        {
            markonBigValue = tr_z;
            index = 3;
        }

        //**注意 markonBigValue 就是上面式子中的tr_w、tr_x等
        //sqrt(m11+m22+m33+1)/2  w
        //sqrt(m11+m22-m33+1)/2  x 
        //.....
        float bigValue = Mathf.Sqrt(markonBigValue + 1.0f) * 0.5f;
        float k = 0.25f / bigValue;
        switch (index)
        {
            case 0: //w最大，取对应公式
                W = bigValue;
                X = (m23 - m32) * k;
                Y = (m31 - m13) * k;
                Z = (m12 - m21) * k;
                break;
            case 1:
                X = bigValue;
                W = (m23 - m32) * k;
                Y = (m12 + m21) * k;
                Z = (m31 + m13) * k;
                break;
            case 2:
                Y = bigValue;
                W = (m31 - m13) * k;
                X = (m12 + m21) * k;
                Z = (m23 + m32) * k;
                break;
            case 3:
                Z = bigValue;
                W = (m12 - m21) * k;
                X = (m31 + m13) * k;
                Y = (m23 + m32) * k;
                break;
        }
    }


    /// <summary>
    /// 欧拉角转四元数（物体坐标空间-惯性坐标空间）
    /// </summary>
    /// <param name="orientation"></param>
    public void SetToRotationObjectToInertail(EulerAngle orientation)
    {
        float sp, sb, sh;
        float cp, cb, ch;

        sp = Mathf.Sin(orientation.Pitch * 0.5f);
        sb = Mathf.Sin(orientation.Bank * 0.5f);
        sh = Mathf.Sin(orientation.Heading * 0.5f);

        cp = Mathf.Cos(orientation.Pitch * 0.5f);
        cb = Mathf.Cos(orientation.Bank * 0.5f);
        ch = Mathf.Cos(orientation.Heading * 0.5f);

        W = ch * cp * cb + sh * sp * sb;
        X = ch * sp * cb + sh * cp * sb;
        Y = -ch * sp * sb + sh * cp * cb;
        Z = -sh * sp * cb + ch * sp * sb;
    }

    /// <summary>
    /// 欧拉角转四元数（惯性坐标空间-物体坐标空间）
    /// </summary>
    /// <param name="orientation"></param>
    public void SetToRotationInertailToObject(EulerAngle orientation)
    {
        float sp, sb, sh;
        float cp, cb, ch;

        //cos(h / 2)cos(p / 2)cos(b / 2) + sin(h / 2)sin(p / 2)sin(b / 2)[w]
        //- cos(h / 2)sin(p / 2)cos(b / 2) - sin(h / 2)cos(p / 2)sin(b / 2)[x]
        //cos(h / 2)sin(p / 2)sin(b / 2) - sin(h / 2)cos(p / 2)cos(b / 2)[y]
        //sin(b / 2)sin(p / 2)cos(b / 2) - cos(h / 2)cos(p / 2)sin(b / 2)[z]


        sp = Mathf.Sin(orientation.Pitch * 0.5f);
        sb = Mathf.Sin(orientation.Bank * 0.5f);
        sh = Mathf.Sin(orientation.Heading * 0.5f);

        cp = Mathf.Cos(orientation.Pitch * 0.5f);
        cb = Mathf.Cos(orientation.Bank * 0.5f);
        ch = Mathf.Cos(orientation.Heading * 0.5f);

        W = ch * cp * cb + sh * sp * sb;
        X = -ch * sp * cb - sh * cp * sb;
        Y = ch * sp * sb - sh * cp * cb;
        Z = sh * sp * cb - ch * sp * sb;
    }


    /// <summary>
    /// 对两个四元数进行插值操作
    /// </summary>
    /// <param name="q0">四元数q0</param>
    /// <param name="q1">四元数q1</param>
    /// <param name="t">插值系数t</param>
    /// <returns></returns>
    public QuaternionX slerp(QuaternionX q0, QuaternionX q1, float t)
    {
        if (t <= 0.0f)
        {
            return q0;
        }
        if (t >= 1.0f)
        {
            return q1;
        }

        float cosOmega = Dot(q0, q1); // 夹角的cos
        float qlw = q1.W;
        float qlx = q1.X;
        float qly = q1.Y;
        float qlz = q1.Z;

        if (cosOmega < 0.0f)
        {
            qlw = -qlw;
            qlx = -qlx;
            qly = -qly;
            qlz = -qlz;
            cosOmega = -cosOmega;
        }

        float k0, k1;
        //如果v0 v1非常接近
        if (cosOmega > 0.9999f)
        {
            //那就使用简单线性插值
            k0 = 1.0f - t;
            k1 = t;
        }
        else
        {
            //cosa(^2) + sin(^2) = 1;
            float sinOmega = Mathf.Sqrt(1.0f - cosOmega * cosOmega);
            float omega = Mathf.Atan2(sinOmega, cosOmega);

            float k = 1.0f / sinOmega;
            k0 = Mathf.Sin((1.0f - t) * omega) * k;
            k1 = Mathf.Sin(t * omega) * k;

        }

        QuaternionX qN = new QuaternionX();
        qN.X = k0 * q0.X + k1 * qlx;
        qN.Y = k0 * q0.Y + k1 * qly;
        qN.Z = k0 * q0.Z + k1 * qlz;
        qN.W = k0 * q0.W + k1 * qlw;

        return qN;
    }


    public override string ToString()
    {
        return "[" + W + "     (" + X + ", " + Y + ", " + Z + ")" + "]";
    }

}