matrix_recompose.hpp

Go to the documentation of this file.

#pragma once
 
#include "detail/glm_includes.h"
 
namespace math
{
using namespace glm;
 
namespace detail
{
namespace
{
template<typename genType>
GLM_FUNC_QUALIFIER GLM_CONSTEXPR genType lowest_scale()
{
    GLM_STATIC_ASSERT(std::numeric_limits<genType>::is_iec559 || GLM_CONFIG_UNRESTRICTED_FLOAT,
                      "'pi' only accepts floating-point inputs");
    return static_cast<genType>(0.0001);
}
 
} // namespace
 
template<length_t L, typename T, qualifier Q>
GLM_FUNC_QUALIFIER T length_impl(vec<L, T, Q> const& v)
{
    return std::max(lowest_scale<T>(), length(v));
}
 
// result = (a * ascl) + (b * bscl)
template<typename T, qualifier Q>
GLM_FUNC_QUALIFIER vec<3, T, Q> combine_impl(vec<3, T, Q> const& a, vec<3, T, Q> const& b, T ascl, T bscl)
{
    return (a * ascl) + (b * bscl);
}
 
template<typename T, qualifier Q>
GLM_FUNC_QUALIFIER vec<3, T, Q> scale_impl(vec<3, T, Q> const& v, T desiredLength)
{
    return v * desiredLength / length_impl(v);
}
 
template<typename T>
GLM_FUNC_QUALIFIER T scale_fix(T& in)
{
    T el = in;
    if(math::epsilonEqual<T>(el, T(0), lowest_scale<T>()))
    {
        el = lowest_scale<T>() * (std::signbit(el) ? T(1.0) : T(1.0));
    }
 
    return el;
}
 
template<typename T, qualifier Q>
GLM_FUNC_QUALIFIER vec<3, T, Q> scale_fix(vec<3, T, Q> const& scale)
{
    auto result = scale;
    for(math::length_t i = 0; i < result.length(); ++i)
    {
        auto& el = result[i];
        el = scale_fix(el);
    }
 
    return result;
}
 
} // namespace detail
// Recomposes a model matrix from a previously-decomposed matrix
// http://www.opensource.apple.com/source/WebCore/WebCore-514/platform/graphics/transforms/TransformationMatrix.cpp
// https://stackoverflow.com/a/75573092/1047040
template<typename T, qualifier Q>
GLM_FUNC_QUALIFIER mat<4, 4, T, Q> recompose_impl(vec<3, T, Q> const& scale,
                                                  qua<T, Q> const& orientation,
                                                  vec<3, T, Q> const& translation,
                                                  vec<3, T, Q> const& skew,
                                                  vec<4, T, Q> const& perspective)
{
    mat<4, 4, T, Q> m(1);
 
    m[0][3] = perspective.x;
    m[1][3] = perspective.y;
    m[2][3] = perspective.z;
    m[3][3] = perspective.w;
 
    m *= glm::translate(translation);
    m *= glm::mat4_cast(orientation);
 
    if(abs(skew.x) > static_cast<T>(0))
    {
        mat<4, 4, T, Q> tmp(static_cast<T>(1));
        tmp[2][1] = skew.x;
        m *= tmp;
    }
 
    if(abs(skew.y) > static_cast<T>(0))
    {
        mat<4, 4, T, Q> tmp(static_cast<T>(1));
        tmp[2][0] = skew.y;
        m *= tmp;
    }
 
    if(abs(skew.z) > static_cast<T>(0))
    {
        mat<4, 4, T, Q> tmp(static_cast<T>(1));
        tmp[1][0] = skew.z;
        m *= tmp;
    }
 
    m *= glm::scale(scale);
 
    return m;
}
 
template<typename T, qualifier Q>
GLM_FUNC_QUALIFIER void glm_recompose(mat<4, 4, T, Q>& model_matrix,
                                      vec<3, T, Q> const& in_scale,
                                      qua<T, Q> const& in_orientation,
                                      vec<3, T, Q> const& in_translation,
                                      vec<3, T, Q> const& in_skew,
                                      vec<4, T, Q> const& in_perspective)
{
    model_matrix = recompose_impl(detail::scale_fix(in_scale), in_orientation, in_translation, in_skew, in_perspective);
}
 
// Matrix decompose
// http://www.opensource.apple.com/source/WebCore/WebCore-514/platform/graphics/transforms/TransformationMatrix.cpp
// Decomposes the mode matrix to translations,rotation scale components
template<typename T, qualifier Q>
GLM_FUNC_QUALIFIER bool glm_decompose(mat<4, 4, T, Q> const& ModelMatrix,
                                      vec<3, T, Q>& Scale,
                                      qua<T, Q>& Orientation,
                                      vec<3, T, Q>& Translation,
                                      vec<3, T, Q>& Skew,
                                      vec<4, T, Q>& Perspective)
{
    mat<4, 4, T, Q> LocalMatrix(ModelMatrix);
 
    // Normalize the matrix.
    if(epsilonEqual(LocalMatrix[3][3], static_cast<T>(0), epsilon<T>()))
    {
        LocalMatrix[3][3] = T(1);
 
        // DO not return here, but tread perspective as 1
        //return false;
    }
 
    for(length_t i = 0; i < 4; ++i)
        for(length_t j = 0; j < 4; ++j)
            LocalMatrix[i][j] /= LocalMatrix[3][3];
 
    // perspectiveMatrix is used to solve for perspective, but it also provides
    // an easy way to test for singularity of the upper 3x3 component.
    mat<4, 4, T, Q> PerspectiveMatrix(LocalMatrix);
 
    for(length_t i = 0; i < 3; i++)
        PerspectiveMatrix[i][3] = static_cast<T>(0);
    PerspectiveMatrix[3][3] = static_cast<T>(1);
 
    // if(epsilonEqual(determinant(PerspectiveMatrix), static_cast<T>(0), epsilon<T>()))
    // {
    //     return false;
    // }
    // return false;
 
    // First, isolate perspective.  This is the messiest.
    if(epsilonNotEqual(LocalMatrix[0][3], static_cast<T>(0), epsilon<T>()) ||
       epsilonNotEqual(LocalMatrix[1][3], static_cast<T>(0), epsilon<T>()) ||
       epsilonNotEqual(LocalMatrix[2][3], static_cast<T>(0), epsilon<T>()))
    {
        // rightHandSide is the right hand side of the equation.
        vec<4, T, Q> RightHandSide;
        RightHandSide[0] = LocalMatrix[0][3];
        RightHandSide[1] = LocalMatrix[1][3];
        RightHandSide[2] = LocalMatrix[2][3];
        RightHandSide[3] = LocalMatrix[3][3];
 
        // Solve the equation by inverting PerspectiveMatrix and multiplying
        // rightHandSide by the inverse.  (This is the easiest way, not
        // necessarily the best.)
        mat<4, 4, T, Q> InversePerspectiveMatrix =
            glm::inverse(PerspectiveMatrix); //   inverse(PerspectiveMatrix, inversePerspectiveMatrix);
        mat<4, 4, T, Q> TransposedInversePerspectiveMatrix =
            glm::transpose(InversePerspectiveMatrix); //   transposeMatrix4(inversePerspectiveMatrix,
                                                      //   transposedInversePerspectiveMatrix);
 
        Perspective = TransposedInversePerspectiveMatrix * RightHandSide;
        //  v4MulPointByMatrix(rightHandSide, transposedInversePerspectiveMatrix, perspectivePoint);
 
        // Clear the perspective partition
        LocalMatrix[0][3] = LocalMatrix[1][3] = LocalMatrix[2][3] = static_cast<T>(0);
        LocalMatrix[3][3] = static_cast<T>(1);
    }
    else
    {
        // No perspective.
        Perspective = vec<4, T, Q>(0, 0, 0, 1);
    }
 
    // Next take care of translation (easy).
    Translation = vec<3, T, Q>(LocalMatrix[3]);
    LocalMatrix[3] = vec<4, T, Q>(0, 0, 0, LocalMatrix[3].w);
 
    vec<3, T, Q> Row[3], Pdum3;
 
    // Now get scale and shear.
    for(length_t i = 0; i < 3; ++i)
        for(length_t j = 0; j < 3; ++j)
            Row[i][j] = LocalMatrix[i][j];
 
    // Compute X scale factor and normalize first row.
    Scale.x = detail::length_impl(Row[0]); // v3Length(Row[0]);
 
    Row[0] = detail::scale_impl(Row[0], static_cast<T>(1));
 
    // Compute XY shear factor and make 2nd row orthogonal to 1st.
    Skew.z = dot(Row[0], Row[1]);
    Row[1] = detail::combine_impl(Row[1], Row[0], static_cast<T>(1), -Skew.z);
 
    // Now, compute Y scale and normalize 2nd row.
    Scale.y = detail::length_impl(Row[1]);
    Row[1] = detail::scale_impl(Row[1], static_cast<T>(1));
    Skew.z /= Scale.y;
 
    // Compute XZ and YZ shears, orthogonalize 3rd row.
    Skew.y = glm::dot(Row[0], Row[2]);
    Row[2] = detail::combine_impl(Row[2], Row[0], static_cast<T>(1), -Skew.y);
    Skew.x = glm::dot(Row[1], Row[2]);
    Row[2] = detail::combine_impl(Row[2], Row[1], static_cast<T>(1), -Skew.x);
 
    // Next, get Z scale and normalize 3rd row.
    Scale.z = detail::length_impl(Row[2]);
    Row[2] = detail::scale_impl(Row[2], static_cast<T>(1));
    Skew.y /= Scale.z;
    Skew.x /= Scale.z;
 
    // At this point, the matrix (in rows[]) is orthonormal.
    // Check for a coordinate system flip.  If the determinant
    // is -1, then negate the matrix and the scaling factors.
    Pdum3 = cross(Row[1], Row[2]); // v3Cross(row[1], row[2], Pdum3);
    if(dot(Row[0], Pdum3) < 0)
    {
        for(length_t i = 0; i < 3; i++)
        {
            Scale[i] *= static_cast<T>(-1);
            Row[i] *= static_cast<T>(-1);
        }
    }
 
    // Now, get the rotations out, as described in the gem.
 
    // FIXME - Add the ability to return either quaternions (which are
    // easier to recompose with) or Euler angles (rx, ry, rz), which
    // are easier for authors to deal with. The latter will only be useful
    // when we fix https://bugs.webkit.org/show_bug.cgi?id=23799, so I
    // will leave the Euler angle code here for now.
 
    // ret.rotateY = asin(-Row[0][2]);
    // if (cos(ret.rotateY) != 0) {
    //     ret.rotateX = atan2(Row[1][2], Row[2][2]);
    //     ret.rotateZ = atan2(Row[0][1], Row[0][0]);
    // } else {
    //     ret.rotateX = atan2(-Row[2][0], Row[1][1]);
    //     ret.rotateZ = 0;
    // }
 
    int i, j, k = 0;
    T root, trace = Row[0].x + Row[1].y + Row[2].z;
    if(trace > static_cast<T>(0))
    {
        root = sqrt(trace + static_cast<T>(1.0));
        Orientation.w = static_cast<T>(0.5) * root;
        root = static_cast<T>(0.5) / root;
        Orientation.x = root * (Row[1].z - Row[2].y);
        Orientation.y = root * (Row[2].x - Row[0].z);
        Orientation.z = root * (Row[0].y - Row[1].x);
    } // End if > 0
    else
    {
        static int Next[3] = {1, 2, 0};
        i = 0;
        if(Row[1].y > Row[0].x)
            i = 1;
        if(Row[2].z > Row[i][i])
            i = 2;
        j = Next[i];
        k = Next[j];
 
#ifdef GLM_FORCE_QUAT_DATA_WXYZ
        int off = 1;
#else
        int off = 0;
#endif
 
        root = sqrt(Row[i][i] - Row[j][j] - Row[k][k] + static_cast<T>(1.0));
 
        Orientation[i + off] = static_cast<T>(0.5) * root;
        root = static_cast<T>(0.5) / root;
        Orientation[j + off] = root * (Row[i][j] + Row[j][i]);
        Orientation[k + off] = root * (Row[i][k] + Row[k][i]);
        Orientation.w = root * (Row[j][k] - Row[k][j]);
    } // End if <= 0
 
    return true;
}
} // namespace math