tb_matrix_quaternion.hpp

 
#ifndef TurtleBrains_MatrixQuaternion_hpp
#define TurtleBrains_MatrixQuaternion_hpp
 
#include <turtle_brains/math/tb_matrix.hpp>
#include <turtle_brains/math/tb_quaternion.hpp>
 
//
// This needs more unit testing to confirm it -really- works, but, I find it odd that I had to transpose the resource
// during FromMatrix, and _NOT_ transpose from the resource during FromQuaternion. Doing the transpose could make sense
// if they used ColumnMajor and TurtleBrains used RowMajor (or opposite), but then I'd expect to have done the same
// transposition in both functions...
//
 
template<typename Type> TurtleBrains::Math::TypedQuaternion<Type> TurtleBrains::Math::TypedQuaternion<Type>::FromMatrix(
    const TurtleBrains::Math::TypedMatrix3<Type>& matrix)
{   //http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
    //https://d3cw3dd2w32x2b.cloudfront.net/wp-content/uploads/2015/01/matrix-to-quat.pdf
    if (matrix(2, 2) < Type(0.0))
    {
        if (matrix(0, 0) > matrix(1, 1))
        {
            const Type t = (Type(1.0) + matrix(0, 0) - matrix(1, 1) - matrix(2, 2));
            TypedQuaternion<Type> q(t, matrix(1, 0) + matrix(0, 1), matrix(0, 2) + matrix(2, 0), matrix(2, 1) - matrix(1, 2));
            q *= (Type(0.5) / std::sqrt(t));
            return q;
        }
        else
        {
            const Type t = (Type(1.0) - matrix(0, 0) + matrix(1, 1) - matrix(2, 2));
            TypedQuaternion<Type> q(matrix(1, 0) + matrix(0, 1), t, matrix(2, 1) + matrix(1, 2), matrix(0, 2) - matrix(2, 0));
            q *= (Type(0.5) / std::sqrt(t));
            return q;
        }
    }
    else
    {
        if (matrix(0, 0) < -matrix(1, 1))
        {
            const Type t = (Type(1.0) - matrix(0, 0) - matrix(1, 1) + matrix(2, 2));
            TypedQuaternion<Type> q(matrix(0, 2) + matrix(2, 0), matrix(2, 1) + matrix(1, 2), t, matrix(1, 0) - matrix(0, 1));
            q *= (Type(0.5) / std::sqrt(t));
            return q;
        }
        else
        {
            const Type t = (Type(1.0) + matrix(0, 0) + matrix(1, 1) + matrix(2, 2));
            TypedQuaternion<Type> q(matrix(2, 1) - matrix(1, 2), matrix(0, 2) - matrix(2, 0), matrix(1, 0) - matrix(0, 1), t);
            q *= (Type(0.5) / std::sqrt(t));
            return q;
        }
    }
}
 
template<typename Type> TurtleBrains::Math::TypedQuaternion<Type> TurtleBrains::Math::TypedQuaternion<Type>::FromMatrix(
    const TurtleBrains::Math::TypedMatrix4<Type>& realMatrix)
{
    TurtleBrains::Math::TypedMatrix4<Type> matrix = realMatrix.GetTransposed();
 
    Type fourXSquaredMinus1 = matrix(0, 0) - matrix(1, 1) - matrix(2, 2);
    Type fourYSquaredMinus1 = matrix(1, 1) - matrix(0, 0) - matrix(2, 2);
    Type fourZSquaredMinus1 = matrix(2, 2) - matrix(0, 0) - matrix(1, 1);
    Type fourWSquaredMinus1 = matrix(0, 0) + matrix(1, 1) + matrix(2, 2);
 
    int biggestIndex = 0;
    Type fourBiggestSquaredMinus1 = fourWSquaredMinus1;
    if (fourXSquaredMinus1 > fourBiggestSquaredMinus1)
    {
        fourBiggestSquaredMinus1 = fourXSquaredMinus1;
        biggestIndex = 1;
    }
    if (fourYSquaredMinus1 > fourBiggestSquaredMinus1)
    {
        fourBiggestSquaredMinus1 = fourYSquaredMinus1;
        biggestIndex = 2;
    }
    if (fourZSquaredMinus1 > fourBiggestSquaredMinus1)
    {
        fourBiggestSquaredMinus1 = fourZSquaredMinus1;
        biggestIndex = 3;
    }
 
    Type biggestVal = std::sqrt(fourBiggestSquaredMinus1 + static_cast<Type>(1)) * static_cast<Type>(0.5);
    Type mult = static_cast<Type>(0.25) / biggestVal;
 
    switch (biggestIndex)
    {
    case 0:
        return TypedQuaternion<Type>((matrix(1, 2) - matrix(2, 1)) * mult, (matrix(2, 0) - matrix(0, 2)) * mult, (matrix(0, 1) - matrix(1, 0)) * mult, biggestVal);
    case 1:
        return TypedQuaternion<Type>(biggestVal, (matrix(0, 1) + matrix(1, 0)) * mult, (matrix(2, 0) + matrix(0, 2)) * mult, (matrix(1, 2) - matrix(2, 1)) * mult);
    case 2:
        return TypedQuaternion<Type>((matrix(0, 1) + matrix(1, 0)) * mult, biggestVal, (matrix(1, 2) + matrix(2, 1)) * mult, (matrix(2, 0) - matrix(0, 2)) * mult);
    case 3:
        return TypedQuaternion<Type>((matrix(2, 0) + matrix(0, 2)) * mult, (matrix(1, 2) + matrix(2, 1)) * mult, biggestVal, (matrix(0, 1) - matrix(1, 0)) * mult);
    default: // Silence a -Wswitch-default warning in GCC. Should never actually get here. Assert is just for sanity.
        tb_error("Something went bad with quat from matrix.");
        return TypedQuaternion<Type>(Type(0.0), Type(0.0), Type(0.0), Type(1.0));
    }
 
 
 
 
 
 
 
    //if (matrix(2, 2) < 0.0f)
    //{
    //  if (matrix(0, 0) > matrix(1, 1))
    //  {
    //      const Type trace = (Type(1.0) + matrix(0, 0) - matrix(1, 1) - matrix(2, 2));
    //      TypedQuaternion<Type> q(trace, matrix(1, 0) + matrix(0, 1), matrix(0, 2) + matrix(2, 0), matrix(2, 1) - matrix(1, 2));
    //      q *= Type(0.5) / std::sqrt(trace);
    //      return q;
    //  }
    //  else
    //  {
    //      const Type trace = (Type(1.0) - matrix(0, 0) + matrix(1, 1) - matrix(2, 2));
    //      TypedQuaternion<Type> q(matrix(1, 0) + matrix(0, 1), trace, matrix(2, 1) + matrix(1, 2), matrix(0, 2) - matrix(2, 0));
    //      q *= Type(0.5) / std::sqrt(trace);
    //      return q;
    //  }
    //}
    //else
    //{
    //  if (matrix(0, 0) < -matrix(1, 1))
    //  {
    //      const Type trace = (Type(1.0) - matrix(0, 0) - matrix(1, 1) + matrix(2, 2));
    //      TypedQuaternion<Type> q(matrix(0, 2) + matrix(2, 0), matrix(2, 1) + matrix(1, 2), trace, matrix(1, 0) - matrix(0, 1));
    //      q *= Type(0.5) / std::sqrt(trace);
    //      return q;
    //  }
    //  else
    //  {
    //      const Type trace = (Type(1.0) + matrix(0, 0) + matrix(1, 1) + matrix(2, 2));
    //      TypedQuaternion q(matrix(2, 1) - matrix(1, 2), matrix(0, 2) - matrix(2, 0), matrix(1, 0) - matrix(0, 1), trace);
    //      q *= Type(0.5) / std::sqrt(trace);
    //      return q;
    //  }
    //}
}
 
template<typename Type> TurtleBrains::Math::TypedMatrix3<Type> TurtleBrains::Math::TypedMatrix3<Type>::FromQuaternion(
    const TurtleBrains::Math::TypedQuaternion<Type>& quaternion)
{   //http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/index.htm
    const Type& qx(quaternion.x);
    const Type& qy(quaternion.y);
    const Type& qz(quaternion.z);
    const Type& qw(quaternion.w);
 
    const Type qxSq(quaternion.x * quaternion.x);
    const Type qySq(quaternion.y * quaternion.y);
    const Type qzSq(quaternion.z * quaternion.z);
 
    //The resource mentioned this expected the quaternion to be normalized; thought if it is not normalized
    //the sqrt can be thrown away by performing the normalization after calculating the products..
    return TypedMatrix3<Type>(
        Type(1.0) - Type(2.0) * qySq - Type(2.0) * qzSq,    Type(2.0) * qx * qy - Type(2.0) * qz * qw,          Type(2.0) * qx * qz + Type(2.0) * qy * qw,
        Type(2.0) * qx * qy + Type(2.0) * qz * qw,          Type(1.0) - Type(2.0) * qxSq - Type(2.0) * qzSq,    Type(2.0) * qy * qz - Type(2.0) * qx * qw,
        Type(2.0) * qx * qz - Type(2.0) * qy * qw,          Type(2.0) * qy * qz + Type(2.0) * qx * qw,          Type(1.0) - Type(2.0) * qxSq - Type(2.0) * qySq);
 
    //This is actually the expected values, but it fails the tests and the above passes. Keeping this block around
    //until such a time comes that Matrix3 gets some attention. Note: This IS the correct block, 2019-01-01
    //return Matrix3(
    //  1.0f - 2.0f * qySq - 2.0f * qzSq,    2.0f * qx * qy + 2.0f * qz * qw,     2.0f * qx * qz - 2.0f * qy * qw,
    //  2.0f * qx * qy - 2.0f * qz * qw,     1.0f - 2.0f * qxSq - 2.0f * qzSq,    2.0f * qy * qz + 2.0f * qx * qw,
    //  2.0f * qx * qz + 2.0f * qy * qw,     2.0f * qy * qz - 2.0f * qx * qw,     1.0f - 2.0f * qxSq - 2.0f * qySq);
}
 
template<typename Type> TurtleBrains::Math::TypedMatrix4<Type> TurtleBrains::Math::TypedMatrix4<Type>::FromQuaternion(
    const TurtleBrains::Math::TypedQuaternion<Type>& quaternion, const TurtleBrains::Math::TypedVector3<Type>& translation)
{
    const Type& qx(quaternion.x);
    const Type& qy(quaternion.y);
    const Type& qz(quaternion.z);
    const Type& qw(quaternion.w);
 
    const Type qxSq(quaternion.x * quaternion.x);
    const Type qySq(quaternion.y * quaternion.y);
    const Type qzSq(quaternion.z * quaternion.z);
 
    //The resource mentioned this expected the quaternion to be normalized; thought if it is not normalized
    //the sqrt can be thrown away by performing the normalization after calculating the products...
    return TypedMatrix4<Type>(
        Type(1.0) - Type(2.0) * qySq - Type(2.0) * qzSq,   Type(2.0) * qx * qy + Type(2.0) * qz * qw,         Type(2.0) * qx * qz - Type(2.0) * qy * qw,       Type(0.0),
        Type(2.0) * qx * qy - Type(2.0) * qz * qw,         Type(1.0) - Type(2.0) * qxSq - Type(2.0) * qzSq,   Type(2.0) * qy * qz + Type(2.0) * qx * qw,       Type(0.0),
        Type(2.0) * qx * qz + Type(2.0) * qy * qw,         Type(2.0) * qy * qz - Type(2.0) * qx * qw,         Type(1.0) - Type(2.0) * qxSq - Type(2.0) * qySq, Type(0.0),
        translation.x,                                     translation.y,                                     translation.z,                                   Type(1.0));
}
 
#endif /* TurtleBrains_MatrixQuaternion_hpp */