UnityのQuaternionとVector3を手書きで変換した件について

初めに

仕事でunityで行っていた機能をc++で書いているサーバに引っ越すことになりその際に調べたので備忘録として書いてます。
ちなみにQuaternionとVector3の変換は扱っている座標系などによって異なるためいくつか種類があります。ここではUnityで動いているものを基準に書きます。unityは(pitch,yaw,roll)=(x,y,z)で回転順はZXYです。
理由など詳しいこと調べたい方向けというよりは答えが知りたい人向けです。（理解してないなんていえない・・・）

QuaternionからVector3への変換

Vector3 Quaternion_to_Vector3(Quaternion r)
{
    float x = r.x;
    float y = r.y;
    float z = r.z;
    float w = r.w;

    float x2 = x * x;
    float y2 = y * y;
    float z2 = z * z;

    float xy = x * y;
    float xz = x * z;
    float yz = y * z;
    float wx = w * x;
    float wy = w * y;
    float wz = w * z;

    // 1 - 2y^2 - 2z^2
    float m00 = 1.f - (2.f * y2) - (2.f * z2);

    // 2xy + 2wz
    float m01 = (2.f * xy) + (2.f * wz);

    // 2xy - 2wz
    float m10 = (2.f * xy) - (2.f * wz);

    // 1 - 2x^2 - 2z^2
    float m11 = 1.f - (2.f * x2) - (2.f * z2);

    // 2xz + 2wy
    float m20 = (2.f * xz) + (2.f * wy);

    // 2yz+2wx
    float m21 = (2.f * yz) - (2.f * wx);

    // 1 - 2x^2 - 2y^2
    float m22 = 1.f - (2.f * x2) - (2.f * y2);


    float tx, ty, tz;

    if (abs(m21 - 1.f) < 0.000001f)
    {
        tx = M_PI / 2.f;
        ty = 0;
        tz = atan2(m10, m00);
    }
    else if (abs(m21 + 1.f) < 0.000001f)
    {
        tx = -M_PI / 2.f;
        ty = 0;
        tz = atan2(m10, m00);
    }
    else
    {
        tx = asin(-m21);
        ty = atan2(m20, m22);
        tz = atan2(m01, m11);
    }

    tx = tx / M_PI * 180;
    ty = ty / M_PI * 180;
    tz = tz / M_PI * 180;

    Vector3 res;
    res.x = tx;
    res.y = ty;
    res.z = tz;

    return res;
}

Vector3からQuaternionへの変換

Quaternion Vector3_to_Quaternion(Vector3 rot)
{
    float x = rot.x * M_PI /180.f;
    float y = rot.y * M_PI /180.f;
    float z = rot.z * M_PI /180.f;

    float c1 = cos(x * 0.5f);
    float c2 = cos(y * 0.5f);
    float c3 = cos(z * 0.5f);

    float s1 = sin(x * 0.5f);
    float s2 = sin(y * 0.5f);
    float s3 = sin(z * 0.5f);

    // Unity's order is 'YXZ'
    float qx = s1 * c2 * c3 + c1 * s2 * s3;
    float qy = c1 * s2 * c3 - s1 * c2 * s3;
    float qz = c1 * c2 * s3 - s1 * s2 * c3;
    float qw = c1 * c2 * c3 + s1 * s2 * s3;

    Quaternion res;
    res.x = qx;
    res.y = qy;
    res.z = qz;
    res.w = qw;

    return res;
}