decompose_matrix.py

# SPDX-License-Identifier: MIT

import array

def cast_memoryview(mv, t):
    return mv.cast(t)


def dot_product_vector3(a, b):
    return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]

def mul_vector3_vector3_float(r, a, f):
    r[0] = a[0] * f
    r[1] = a[1] * f
    r[2] = a[2] * f

def zero_vector3(r):
    r[0] = 0.0
    r[1] = 0.0
    r[2] = 0.0

def normalize_v3_v3_length(r, a, unit_length=1.0):
    d = dot_product_vector3(a, a)

    if (d > 1.0e-35):
        d = d ** 0.5
        mul_vector3_vector3_float(r, a, unit_length / d)
    else:
        zero_vector3(r)
        d = 0.0

    return d

def normalize_matrix_44(m):
    norm = cast_memoryview(memoryview(bytearray(cast_memoryview(memoryview(m), "B"))), "f")
    norm[0:15] = m[0:15]
    for i in range(3):
        tmp_v3 = array.array("f")
        tmp_v3.extend(norm[0 + (i * 4):3 + (i * 4)])
        normalize_v3_v3_length(tmp_v3, tmp_v3, 1.0)
        norm[0 + (i * 4):3 + (i * 4)] = tmp_v3
    return norm

def decompose_matrix_44(mat, translation, rotation, scale):
    m00 = mat[ 0]
    m01 = mat[ 1]
    m02 = mat[ 2]
    m03 = mat[ 3]
    m10 = mat[ 4]
    m11 = mat[ 5]
    m12 = mat[ 6]
    m13 = mat[ 7]
    m20 = mat[ 8]
    m21 = mat[ 9]
    m22 = mat[10]
    m23 = mat[11]
    m30 = mat[12]
    m31 = mat[13]
    m32 = mat[14]
    m33 = mat[15]
    translation[0] = m30
    translation[1] = m31
    translation[2] = m32
    scale[0] = ((m00**2) + (m10**2) + (m20**2)) ** 0.5
    scale[1] = ((m01**2) + (m11**2) + (m21**2)) ** 0.5
    scale[2] = ((m02**2) + (m12**2) + (m22**2)) ** 0.5
    mat = normalize_matrix_44(mat)
    m00 = mat[ 0]
    m01 = mat[ 1]
    m02 = mat[ 2]
    m03 = mat[ 3]
    m10 = mat[ 4]
    m11 = mat[ 5]
    m12 = mat[ 6]
    m13 = mat[ 7]
    m20 = mat[ 8]
    m21 = mat[ 9]
    m22 = mat[10]
    m23 = mat[11]
    m30 = mat[12]
    m31 = mat[13]
    m32 = mat[14]
    m33 = mat[15]
    tr = 0.25 * (1.0 + m00 + m11 + m22)
    if (tr > 1e-4):
        s = ((tr) ** 0.5)
        rotation[3] = s
        s = 1.0 / (4.0 * s)
        rotation[0] = ((m12 - m21) * s)
        rotation[1] = ((m20 - m02) * s)
        rotation[2] = ((m01 - m10) * s)
    else:
        if (m00 > m11 and m00 > m22):
            s = 2.0 * ((1.0 + m00 - m11 - m22) ** 0.5)
            rotation[0] = (0.25 * s)

            s = 1.0 / s
            rotation[3] = ((m12 - m21) * s)
            rotation[1] = ((m10 + m01) * s)
            rotation[2] = ((m20 + m02) * s)
        elif (m11 > m22):
            s = 2.0 * ((1.0 + m11 - m00 - m22) ** 0.5)
            rotation[1] = (0.25 * s)

            s = 1.0 / s
            rotation[3] = ((m20 - m02) * s)
            rotation[0] = ((m10 + m01) * s)
            rotation[2] = ((m21 + m12) * s)
        else:
            s = 2.0 * ((1.0 + m22 - m00 - m11) ** 0.5)
            rotation[2] = (0.25 * s)

            s = 1.0 / s
            rotation[3] = ((m01 - m10) * s)
            rotation[0] = ((m20 + m02) * s)
            rotation[1] = ((m21 + m12) * s)
    rot_len = (rotation[0] * rotation[0] + rotation[1] * rotation[1] + rotation[2] * rotation[2] + rotation[3] * rotation[3]) ** 0.5
    if rot_len != 0.0:
        f = 1.0 / rot_len
        for i in range(len(rotation)):
            rotation[i] *= f
    else:
        rotation[0] = 0.0
        rotation[1] = 0.0
        rotation[2] = 0.0
        rotation[3] = 1.0