四元数

Desc：

略

理论：

通过矩阵构造一个四元数

推导

Quaternion::fromRotationMatrix(const Matrix3x3& rotation)

void Quaternion::fromRotationMatrix(const Matrix3x3& rotation)
{
    // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
    // article "Quaternion Calculus and Fast Animation".
    float trace = rotation[0][0] + rotation[1][1] + rotation[2][2];
    float root;
    if (trace > 0.0)
    {
        // |w| > 1/2, may as well choose w > 1/2
        root = std::sqrt(trace + 1.0f); // 2w
        w    = 0.5f * root;
        root = 0.5f / root; // 1/(4w)
        x    = (rotation[2][1] - rotation[1][2]) * root;
        y    = (rotation[0][2] - rotation[2][0]) * root;
        z    = (rotation[1][0] - rotation[0][1]) * root;
    }
    else
    {
        // |w| <= 1/2
        size_t s_iNext[3] = {1, 2, 0};
        size_t i          = 0;
        if (rotation[1][1] > rotation[0][0])
            i = 1;
        if (rotation[2][2] > rotation[i][i])
            i = 2;
        size_t j = s_iNext[i];
        size_t k = s_iNext[j];
        root              = std::sqrt(rotation[i][i] - rotation[j][j] - rotation[k][k] + 1.0f);
        float* apkQuat[3] = {&x, &y, &z};
        *apkQuat[i]       = 0.5f * root;
        root              = 0.5f / root;
        w                 = (rotation[k][j] - rotation[j][k]) * root;
        *apkQuat[j]       = (rotation[j][i] + rotation[i][j]) * root;
        *apkQuat[k]       = (rotation[k][i] + rotation[i][k]) * root;
    }
}

通过四元数构造矩阵

推导

void Quaternion::toRotationMatrix(Matrix3x3& kRot) const
{
    float fTx  = x + x;   // 2x
    float fTy  = y + y;   // 2y
    float fTz  = z + z;   // 2z
    float fTwx = fTx * w; // 2xw
    float fTwy = fTy * w; // 2yw
    float fTwz = fTz * w; // 2z2
    float fTxx = fTx * x; // 2x^2
    float fTxy = fTy * x; // 2xy
    float fTxz = fTz * x; // 2xz
    float fTyy = fTy * y; // 2y^2
    float fTyz = fTz * y; // 2yz
    float fTzz = fTz * z; // 2z^2
    kRot[0][0] = 1.0f - (fTyy + fTzz); // 1 - 2y^2 - 2z^2
    kRot[0][1] = fTxy - fTwz;          // 2xy - 2wz
    kRot[0][2] = fTxz + fTwy;          // 2xz + 2wy
    kRot[1][0] = fTxy + fTwz;          // 2xy + 2wz
    kRot[1][1] = 1.0f - (fTxx + fTzz); // 1 - 2x^2 - 2z^2
    kRot[1][2] = fTyz - fTwx;          // 2yz - 2wx
    kRot[2][0] = fTxz - fTwy;          // 2xz - 2wy
    kRot[2][1] = fTyz + fTwx;          // 2yz + 2wx
    kRot[2][2] = 1.0f - (fTxx + fTyy); // 1 - 2x^2 - 2y^2
}

通过角度和轴，构建一个四元数

void Quaternion::fromAngleAxis(const Radian& angle, const Vector3& axis)
{
    // ASSERT:  axis[] is unit length
    //
    // The quaternion representing the rotation is
    //   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)
    Radian half_angle(0.5 * angle);
    float  sin_v = Math::sin(half_angle);
    w            = Math::cos(half_angle);
    x            = sin_v * axis.x;
    y            = sin_v * axis.y;
    z            = sin_v * axis.z;
}

通过轴创建一个四元数

void Quaternion::fromAxes(const Vector3& xaxis, const Vector3& yaxis, const Vector3& zaxis)
{
    Matrix3x3 rot;
    rot[0][0] = xaxis.x;
    rot[1][0] = xaxis.y;
    rot[2][0] = xaxis.z;
    rot[0][1] = yaxis.x;
    rot[1][1] = yaxis.y;
    rot[2][1] = yaxis.z;
    rot[0][2] = zaxis.x;
    rot[1][2] = zaxis.y;
    rot[2][2] = zaxis.z;
    fromRotationMatrix(rot);
}

插值运算

link

/// <summary>
/// Returns a quaternion constructed by first performing 3 rotations around the principal axes in a given order.
/// All rotation angles are in radians and clockwise when looking along the rotation axis towards the origin.
/// When the rotation order is known at compile time, it is recommended for performance reasons to use specific
/// Euler rotation constructors such as EulerZXY(...).
/// </summary>
/// <param name="x">The rotation angle around the x-axis in radians.</param>
/// <param name="y">The rotation angle around the y-axis in radians.</param>
/// <param name="z">The rotation angle around the z-axis in radians.</param>
/// <param name="order">The order in which the rotations are applied.</param>
/// <returns>The quaternion representing the Euler angle rotation in the specified order.</returns>
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static quaternion Euler(float x, float y, float z, RotationOrder order = RotationOrder.Default)
{
    return Euler(float3(x, y, z), order);
}

/// <summary>
/// Returns a quaternion constructed by first performing 3 rotations around the principal axes in a given order.
/// All rotation angles are in radians and clockwise when looking along the rotation axis towards the origin.
/// When the rotation order is known at compile time, it is recommended for performance reasons to use specific
/// Euler rotation constructors such as EulerZXY(...).
/// </summary>
/// <param name="xyz">A float3 vector containing the rotation angles around the x-, y- and z-axis measures in radians.</param>
/// <param name="order">The order in which the rotations are applied.</param>
/// <returns>The quaternion representing the Euler angle rotation in the specified order.</returns>
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static quaternion Euler(float3 xyz, RotationOrder order = RotationOrder.ZXY)
{
    switch (order)
    {
        case RotationOrder.XYZ:
            return EulerXYZ(xyz);
        case RotationOrder.XZY:
            return EulerXZY(xyz);
        case RotationOrder.YXZ:
            return EulerYXZ(xyz);
        case RotationOrder.YZX:
            return EulerYZX(xyz);
        case RotationOrder.ZXY:
            return EulerZXY(xyz);
        case RotationOrder.ZYX:
            return EulerZYX(xyz);
        default:
            return quaternion.identity;
    }
}

/// <summary>
/// Returns a quaternion representing a rotation around a unit axis by an angle in radians.
/// The rotation direction is clockwise when looking along the rotation axis towards the origin.
/// </summary>
/// <param name="axis">The axis of rotation.</param>
/// <param name="angle">The angle of rotation in radians.</param>
/// <returns>The quaternion representing a rotation around an axis.</returns>
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static quaternion AxisAngle(float3 axis, float angle)
{
    float sina, cosa;
    math.sincos(0.5f * angle, out sina, out cosa);
    return quaternion(float4(axis * sina, cosa));
}

/// <summary>
/// Returns a quaternion constructed by first performing a rotation around the x-axis, then the y-axis and finally the z-axis.
/// All rotation angles are in radians and clockwise when looking along the rotation axis towards the origin.
/// </summary>
/// <param name="xyz">A float3 vector containing the rotation angles around the x-, y- and z-axis measures in radians.</param>
/// <returns>The quaternion representing the Euler angle rotation in x-y-z order.</returns>
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static quaternion EulerXYZ(float3 xyz)
{
    // return mul(rotateZ(xyz.z), mul(rotateY(xyz.y), rotateX(xyz.x)));
    float3 s, c;
    sincos(0.5f * xyz, out s, out c);
    return quaternion(
        // s.x * c.y * c.z - s.y * s.z * c.x,
        // s.y * c.x * c.z + s.x * s.z * c.y,
        // s.z * c.x * c.y - s.x * s.y * c.z,
        // c.x * c.y * c.z + s.y * s.z * s.x
        float4(s.xyz, c.x) * c.yxxy * c.zzyz + s.yxxy * s.zzyz * float4(c.xyz, s.x) * float4(-1.0f, 1.0f, -1.0f, 1.0f)
        );
}

/// <summary>
/// Returns a quaternion constructed by first performing a rotation around the x-axis, then the z-axis and finally the y-axis.
/// All rotation angles are in radians and clockwise when looking along the rotation axis towards the origin.
/// </summary>
/// <param name="xyz">A float3 vector containing the rotation angles around the x-, y- and z-axis measures in radians.</param>
/// <returns>The quaternion representing the Euler angle rotation in x-z-y order.</returns>
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static quaternion EulerXZY(float3 xyz)
{
    // return mul(rotateY(xyz.y), mul(rotateZ(xyz.z), rotateX(xyz.x)));
    float3 s, c;
    sincos(0.5f * xyz, out s, out c);
    return quaternion(
        // s.x * c.y * c.z + s.y * s.z * c.x,
        // s.y * c.x * c.z + s.x * s.z * c.y,
        // s.z * c.x * c.y - s.x * s.y * c.z,
        // c.x * c.y * c.z - s.y * s.z * s.x
        float4(s.xyz, c.x) * c.yxxy * c.zzyz + s.yxxy * s.zzyz * float4(c.xyz, s.x) * float4(1.0f, 1.0f, -1.0f, -1.0f)
        );
}

/// <summary>
/// Returns a quaternion constructed by first performing a rotation around the y-axis, then the x-axis and finally the z-axis.
/// All rotation angles are in radians and clockwise when looking along the rotation axis towards the origin.
/// </summary>
/// <param name="xyz">A float3 vector containing the rotation angles around the x-, y- and z-axis measures in radians.</param>
/// <returns>The quaternion representing the Euler angle rotation in y-x-z order.</returns>
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static quaternion EulerYXZ(float3 xyz)
{
    // return mul(rotateZ(xyz.z), mul(rotateX(xyz.x), rotateY(xyz.y)));
    float3 s, c;
    sincos(0.5f * xyz, out s, out c);
    return quaternion(
        // s.x * c.y * c.z - s.y * s.z * c.x,
        // s.y * c.x * c.z + s.x * s.z * c.y,
        // s.z * c.x * c.y + s.x * s.y * c.z,
        // c.x * c.y * c.z - s.y * s.z * s.x
        float4(s.xyz, c.x) * c.yxxy * c.zzyz + s.yxxy * s.zzyz * float4(c.xyz, s.x) * float4(-1.0f, 1.0f, 1.0f, -1.0f)
        );
}

/// <summary>
/// Returns a quaternion constructed by first performing a rotation around the y-axis, then the z-axis and finally the x-axis.
/// All rotation angles are in radians and clockwise when looking along the rotation axis towards the origin.
/// </summary>
/// <param name="xyz">A float3 vector containing the rotation angles around the x-, y- and z-axis measures in radians.</param>
/// <returns>The quaternion representing the Euler angle rotation in y-z-x order.</returns>
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static quaternion EulerYZX(float3 xyz)
{
    // return mul(rotateX(xyz.x), mul(rotateZ(xyz.z), rotateY(xyz.y)));
    float3 s, c;
    sincos(0.5f * xyz, out s, out c);
    return quaternion(
        // s.x * c.y * c.z - s.y * s.z * c.x,
        // s.y * c.x * c.z - s.x * s.z * c.y,
        // s.z * c.x * c.y + s.x * s.y * c.z,
        // c.x * c.y * c.z + s.y * s.z * s.x
        float4(s.xyz, c.x) * c.yxxy * c.zzyz + s.yxxy * s.zzyz * float4(c.xyz, s.x) * float4(-1.0f, -1.0f, 1.0f, 1.0f)
        );
}

/// <summary>
/// Returns a quaternion constructed by first performing a rotation around the z-axis, then the x-axis and finally the y-axis.
/// All rotation angles are in radians and clockwise when looking along the rotation axis towards the origin.
/// This is the default order rotation order in Unity.
/// </summary>
/// <param name="xyz">A float3 vector containing the rotation angles around the x-, y- and z-axis measures in radians.</param>
/// <returns>The quaternion representing the Euler angle rotation in z-x-y order.</returns>
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static quaternion EulerZXY(float3 xyz)
{
    // return mul(rotateY(xyz.y), mul(rotateX(xyz.x), rotateZ(xyz.z)));
    float3 s, c;
    sincos(0.5f * xyz, out s, out c);
    return quaternion(
        // s.x * c.y * c.z + s.y * s.z * c.x,
        // s.y * c.x * c.z - s.x * s.z * c.y,
        // s.z * c.x * c.y - s.x * s.y * c.z,
        // c.x * c.y * c.z + s.y * s.z * s.x
        float4(s.xyz, c.x) * c.yxxy * c.zzyz + s.yxxy * s.zzyz * float4(c.xyz, s.x) * float4(1.0f, -1.0f, -1.0f, 1.0f)
        );
}

/// <summary>
/// Returns a quaternion constructed by first performing a rotation around the z-axis, then the y-axis and finally the x-axis.
/// All rotation angles are in radians and clockwise when looking along the rotation axis towards the origin.
/// </summary>
/// <param name="xyz">A float3 vector containing the rotation angles around the x-, y- and z-axis measures in radians.</param>
/// <returns>The quaternion representing the Euler angle rotation in z-y-x order.</returns>
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static quaternion EulerZYX(float3 xyz)
{
    // return mul(rotateX(xyz.x), mul(rotateY(xyz.y), rotateZ(xyz.z)));
    float3 s, c;
    sincos(0.5f * xyz, out s, out c);
    return quaternion(
        // s.x * c.y * c.z + s.y * s.z * c.x,
        // s.y * c.x * c.z - s.x * s.z * c.y,
        // s.z * c.x * c.y + s.x * s.y * c.z,
        // c.x * c.y * c.z - s.y * s.x * s.z
        float4(s.xyz, c.x) * c.yxxy * c.zzyz + s.yxxy * s.zzyz * float4(c.xyz, s.x) * float4(1.0f, -1.0f, 1.0f, -1.0f)
        );
}

Rotation

/// <summary>Returns a quaternion that rotates around the x-axis by a given number of radians.</summary>
/// <param name="angle">The clockwise rotation angle when looking along the x-axis towards the origin in radians.</param>
/// <returns>The quaternion representing a rotation around the x-axis.</returns>
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static quaternion RotateX(float angle)
{
    float sina, cosa;
    math.sincos(0.5f * angle, out sina, out cosa);
    return quaternion(sina, 0.0f, 0.0f, cosa);
}

/// <summary>Returns a quaternion that rotates around the y-axis by a given number of radians.</summary>
/// <param name="angle">The clockwise rotation angle when looking along the y-axis towards the origin in radians.</param>
/// <returns>The quaternion representing a rotation around the y-axis.</returns>
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static quaternion RotateY(float angle)
{
    float sina, cosa;
    math.sincos(0.5f * angle, out sina, out cosa);
    return quaternion(0.0f, sina, 0.0f, cosa);
}

/// <summary>Returns a quaternion that rotates around the z-axis by a given number of radians.</summary>
/// <param name="angle">The clockwise rotation angle when looking along the z-axis towards the origin in radians.</param>
/// <returns>The quaternion representing a rotation around the z-axis.</returns>
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static quaternion RotateZ(float angle)
{
    float sina, cosa;
    math.sincos(0.5f * angle, out sina, out cosa);
    return quaternion(0.0f, 0.0f, sina, cosa);
}

/// <summary>
/// Returns a quaternion view rotation given a unit length forward vector and a unit length up vector.
/// The two input vectors are assumed to be unit length and not collinear.
/// If these assumptions are not met use float3x3.LookRotationSafe instead.
/// </summary>
/// <param name="forward">The view forward direction.</param>
/// <param name="up">The view up direction.</param>
/// <returns>The quaternion view rotation.</returns>
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public static quaternion LookRotation(float3 forward, float3 up)
{
    float3 t = normalize(cross(up, forward));
    return quaternion(float3x3(t, cross(forward, t), forward));
}

/// <summary>
/// Returns a quaternion view rotation given a forward vector and an up vector.
/// The two input vectors are not assumed to be unit length.
/// If the magnitude of either of the vectors is so extreme that the calculation cannot be carried out reliably or the vectors are collinear,
/// the identity will be returned instead.
/// </summary>
/// <param name="forward">The view forward direction.</param>
/// <param name="up">The view up direction.</param>
/// <returns>The quaternion view rotation or the identity quaternion.</returns>
public static quaternion LookRotationSafe(float3 forward, float3 up)
{
    float forwardLengthSq = dot(forward, forward);
    float upLengthSq = dot(up, up);

    forward *= rsqrt(forwardLengthSq);
    up *= rsqrt(upLengthSq);

    float3 t = cross(up, forward);
    float tLengthSq = dot(t, t);
    t *= rsqrt(tLengthSq);

    float mn = min(min(forwardLengthSq, upLengthSq), tLengthSq);
    float mx = max(max(forwardLengthSq, upLengthSq), tLengthSq);

    bool accept = mn > 1e-35f && mx < 1e35f && isfinite(forwardLengthSq) && isfinite(upLengthSq) && isfinite(tLengthSq);
    return quaternion(select(float4(0.0f, 0.0f, 0.0f, 1.0f), quaternion(float3x3(t, cross(forward, t),forward)).value, accept));
}