向量转四元数

原文：https://krasjet.github.io/quaternion/

Function1：

private Quaternion LookAt(Vector3 dir)
{
    Quaternion cal = new Quaternion();
    //Vector3 euler = Quaternion.LookRotation(dir).eulerAngles;
 
    //欧拉角Y: cosY = z/sqrt(x^2+z^2)
    float CosY = dir.z / Mathf.Sqrt(dir.x * dir.x + dir.z * dir.z);
    float CosYDiv2 = Mathf.Sqrt((CosY + 1) / 2);
    if (dir.x < 0) CosYDiv2 = -CosYDiv2;
    float SinYDiv2 = Mathf.Sqrt((1 - CosY) / 2);
 
    //欧拉角X: cosX = sqrt((x^2+z^2)/(x^2+y^2+z^2)
    float CosX = Mathf.Sqrt((dir.x * dir.x + dir.z * dir.z) / (dir.x * dir.x + dir.y * dir.y + dir.z * dir.z));
    //if (dir.z < 0) CosX = -CosX;
    float CosXDiv2 = Mathf.Sqrt((CosX + 1) / 2);
    if (dir.y > 0) CosXDiv2 = -CosXDiv2;
    float SinXDiv2 = Mathf.Sqrt((1 - CosX) / 2);
 
    //四元数w = cos(x/2)cos(y/2)
    cal.w = CosXDiv2 * CosYDiv2;
    //四元数x = sin(x/2)cos(y/2)
    cal.x = SinXDiv2 * CosYDiv2;
    //四元数y = cos(x/2)sin(y/2)
    cal.y = CosXDiv2 * SinYDiv2;
    //四元数z = sin(x/2)sin(y/2)
    cal.z = -SinXDiv2 * SinYDiv2;
 
    /*
    CalCosX = CosX;
    CalCosY = CosY;
    RightCosX = Mathf.Cos(Mathf.Deg2Rad * (Quaternion.LookRotation(dir).eulerAngles.x));
    RightCosY = Mathf.Cos(Mathf.Deg2Rad * (Quaternion.LookRotation(dir).eulerAngles.y));
    RightEulers = Quaternion.LookRotation(dir).eulerAngles;
    */
 
    return cal;
}

Function2：

public Quaternion DirToQua(Vector3 dir)
{
    //z==y  x==z y==x
    float yawRad = Mathf.Atan2(dir.y, dir.x);
    float pitchRad = Mathf.Atan2(dir.z, Mathf.Sqrt(dir.x * dir.x + dir.y * dir.y));
    float divide_by_2 = 0.5f;
    float sp, sy, cp, cy;
    SinCos(out sp, out cp, pitchRad * divide_by_2);
    SinCos(out sy, out cy, yawRad * divide_by_2);
    Quaternion rotationQuat;
    rotationQuat.x = sp * sy;
    rotationQuat.y = -sp * cy;
    rotationQuat.z = cp * sy;
    rotationQuat.w = cp * cy;
    return rotationQuat;
}
 
public void SinCos(out float scalarSin, out float scalarCos, float value)
{
    float inv_pi = 0.31830988618f;
    float half_pi = 1.57079632679f;
    float quotient = (inv_pi * 0.5f) * value;
    if (value >= 0)
    {
        quotient = (float)((int)(quotient + 0.5f));
    }
    else
    {
        quotient = (float)((int)(quotient - 0.5f));
    }
    float y = value - (2.0f * Mathf.PI) * quotient;
    float sign;
    if (y > half_pi)
    {
        y = Mathf.PI - y;
        sign = -1.0f;
    }
    else if (y < -half_pi)
    {
        y = -Mathf.PI - y;
        sign = -1.0f;
    }
    else
    {
        sign = +1.0f;
    }
    float y2 = y * y;
    // 11-degree minimax approximation
    scalarSin = (((((-2.3889859e-08f * y2 + 2.7525562e-06f) * y2 - 0.00019840874f) * y2 + 0.0083333310f) * y2 - 0.16666667f) * y2 + 1.0f) * y;
    // 10-degree minimax approximation
    float p = ((((-2.6051615e-07f * y2 + 2.4760495e-05f) * y2 - 0.0013888378f) * y2 + 0.041666638f) * y2 - 0.5f) * y2 + 1.0f;
    scalarCos = sign * p;
}