Luce packages
LuceLuce BaseLuciaOS

dymokomi/luce-usd

Source of release 0.9.3

0.9.3 / src / usd_geom / matrix.lucb

## UsdGeom's transform math, after Gf (pxr/base/gf/matrix4d.cpp, quatd.h,
## rotation.cpp) and UsdGeomXformOp (pxr/usd/usdGeom/xformOp.cpp): 4x4 f64
## matrices stored row-major and applied to row vectors (p' = p * M, the
## translation in the last row), as USD authors them, and numeric reads of
## the values xform ops and primvars hold.
##
## The usd_geom module reads UsdGeom schemas from a stage: this fragment has
## the math, then xform (xformOp stacks), prims (traversal with world
## transforms, purposes and visibility).
from luce_std import math
import memory
import errors
from sdf.names import Names, absolute_root, empty_path
from sdf.layer_data import Spec, SpecType
from sdf.values import Value, ListPart, specifier_over
from sdf.value_types import ValueKind, scalar_of, components, element_bytes
from stage import Stage, scalar_number
from geom_tokens import GeomTokens
from luce_geocore.core.growing import Growing

pub struct Matrix4d:
    pub var m: f64[16]

    pub static func identity() -> Matrix4d:
        return Matrix4d([1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0])

    pub static func translation(x: f64, y: f64, z: f64) -> Matrix4d:
        return Matrix4d([1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, x, y, z, 1.0])

    pub static func scaling(x: f64, y: f64, z: f64) -> Matrix4d:
        return Matrix4d([x, 0.0, 0.0, 0.0, 0.0, y, 0.0, 0.0, 0.0, 0.0, z, 0.0, 0.0, 0.0, 0.0, 1.0])

    ## A rotation of `degrees` about axis 0 (X), 1 (Y) or 2 (Z), right-handed.
    pub static func rotation(axis: usize, degrees: f64) -> Matrix4d:
        let radians = degrees * math.pi / 180.0
        let s = math.sin(radians)
        let c = math.cos(radians)
        match axis:
            0: return Matrix4d([1.0, 0.0, 0.0, 0.0, 0.0, c, s, 0.0, 0.0, -s, c, 0.0, 0.0, 0.0, 0.0, 1.0])
            1: return Matrix4d([c, 0.0, -s, 0.0, 0.0, 1.0, 0.0, 0.0, s, 0.0, c, 0.0, 0.0, 0.0, 0.0, 1.0])
            _: return Matrix4d([c, s, 0.0, 0.0, -s, c, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0])

    ## The rotation of the quaternion (real, i, j, k), normalized first.
    pub static func quaternion(real: f64, i: f64, j: f64, k: f64) -> Matrix4d:
        let length = math.sqrt(real * real + i * i + j * j + k * k)
        if length == 0.0: return Matrix4d.identity()
        let w = real / length
        let x = i / length
        let y = j / length
        let z = k / length
        return Matrix4d([
            1.0 - 2.0 * (y * y + z * z), 2.0 * (x * y + z * w), 2.0 * (z * x - y * w), 0.0,
            2.0 * (x * y - z * w), 1.0 - 2.0 * (z * z + x * x), 2.0 * (y * z + x * w), 0.0,
            2.0 * (z * x + y * w), 2.0 * (y * z - x * w), 1.0 - 2.0 * (y * y + x * x), 0.0,
            0.0, 0.0, 0.0, 1.0])

    ## self * other: `self` applies first.
    pub func times(other: const Matrix4d*) -> Matrix4d:
        var out: f64[16] = ---
        for row in 0..<(usize)4:
            for column in 0..<(usize)4:
                var sum: f64 = 0.0
                for k in 0..<(usize)4: sum += self.m[row * 4 + k] * other.m[k * 4 + column]
                out[row * 4 + column] = sum
        return Matrix4d(out)

    pub func is_identity() -> bool:
        for at in 0..<(usize)16:
            if self.m[at] != (1.0 if at % 5 == 0 else 0.0): return false
        return true

    ## The determinant of the upper 3x3 (the linear part's).
    pub func determinant3() -> f64:
        let m = self.m
        return m[0] * (m[5] * m[10] - m[6] * m[9]) - m[1] * (m[4] * m[10] - m[6] * m[8]) + m[2] * (m[4] * m[9] - m[5] * m[8])

    ## The inverse (GfMatrix4d::GetInverse), or none when singular.
    pub func inverse() -> Matrix4d?:
        let a = self.m
        var inv: f64[16] = ---
        inv[0] = a[5] * a[10] * a[15] - a[5] * a[11] * a[14] - a[9] * a[6] * a[15] + a[9] * a[7] * a[14] + a[13] * a[6] * a[11] - a[13] * a[7] * a[10]
        inv[4] = -a[4] * a[10] * a[15] + a[4] * a[11] * a[14] + a[8] * a[6] * a[15] - a[8] * a[7] * a[14] - a[12] * a[6] * a[11] + a[12] * a[7] * a[10]
        inv[8] = a[4] * a[9] * a[15] - a[4] * a[11] * a[13] - a[8] * a[5] * a[15] + a[8] * a[7] * a[13] + a[12] * a[5] * a[11] - a[12] * a[7] * a[9]
        inv[12] = -a[4] * a[9] * a[14] + a[4] * a[10] * a[13] + a[8] * a[5] * a[14] - a[8] * a[6] * a[13] - a[12] * a[5] * a[10] + a[12] * a[6] * a[9]
        inv[1] = -a[1] * a[10] * a[15] + a[1] * a[11] * a[14] + a[9] * a[2] * a[15] - a[9] * a[3] * a[14] - a[13] * a[2] * a[11] + a[13] * a[3] * a[10]
        inv[5] = a[0] * a[10] * a[15] - a[0] * a[11] * a[14] - a[8] * a[2] * a[15] + a[8] * a[3] * a[14] + a[12] * a[2] * a[11] - a[12] * a[3] * a[10]
        inv[9] = -a[0] * a[9] * a[15] + a[0] * a[11] * a[13] + a[8] * a[1] * a[15] - a[8] * a[3] * a[13] - a[12] * a[1] * a[11] + a[12] * a[3] * a[9]
        inv[13] = a[0] * a[9] * a[14] - a[0] * a[10] * a[13] - a[8] * a[1] * a[14] + a[8] * a[2] * a[13] + a[12] * a[1] * a[10] - a[12] * a[2] * a[9]
        inv[2] = a[1] * a[6] * a[15] - a[1] * a[7] * a[14] - a[5] * a[2] * a[15] + a[5] * a[3] * a[14] + a[13] * a[2] * a[7] - a[13] * a[3] * a[6]
        inv[6] = -a[0] * a[6] * a[15] + a[0] * a[7] * a[14] + a[4] * a[2] * a[15] - a[4] * a[3] * a[14] - a[12] * a[2] * a[7] + a[12] * a[3] * a[6]
        inv[10] = a[0] * a[5] * a[15] - a[0] * a[7] * a[13] - a[4] * a[1] * a[15] + a[4] * a[3] * a[13] + a[12] * a[1] * a[7] - a[12] * a[3] * a[5]
        inv[14] = -a[0] * a[5] * a[14] + a[0] * a[6] * a[13] + a[4] * a[1] * a[14] - a[4] * a[2] * a[13] - a[12] * a[1] * a[6] + a[12] * a[2] * a[5]
        inv[3] = -a[1] * a[6] * a[11] + a[1] * a[7] * a[10] + a[5] * a[2] * a[11] - a[5] * a[3] * a[10] - a[9] * a[2] * a[7] + a[9] * a[3] * a[6]
        inv[7] = a[0] * a[6] * a[11] - a[0] * a[7] * a[10] - a[4] * a[2] * a[11] + a[4] * a[3] * a[10] + a[8] * a[2] * a[7] - a[8] * a[3] * a[6]
        inv[11] = -a[0] * a[5] * a[11] + a[0] * a[7] * a[9] + a[4] * a[1] * a[11] - a[4] * a[3] * a[9] - a[8] * a[1] * a[7] + a[8] * a[3] * a[5]
        inv[15] = a[0] * a[5] * a[10] - a[0] * a[6] * a[9] - a[4] * a[1] * a[10] + a[4] * a[2] * a[9] + a[8] * a[1] * a[6] - a[8] * a[2] * a[5]
        let determinant = a[0] * inv[0] + a[1] * inv[4] + a[2] * inv[8] + a[3] * inv[12]
        if determinant == 0.0 or not math.is_finite(determinant): return none
        for at in 0..<(usize)16: inv[at] = inv[at] / determinant
        return Matrix4d(inv)

    ## A point placed by the matrix.
    pub func point(x: f64, y: f64, z: f64) -> (f64, f64, f64):
        let m = self.m
        return (x * m[0] + y * m[4] + z * m[8] + m[12], x * m[1] + y * m[5] + z * m[9] + m[13], x * m[2] + y * m[6] + z * m[10] + m[14])

    ## A direction placed by the linear part.
    pub func direction(x: f64, y: f64, z: f64) -> (f64, f64, f64):
        let m = self.m
        return (x * m[0] + y * m[4] + z * m[8], x * m[1] + y * m[5] + z * m[9], x * m[2] + y * m[6] + z * m[10])

## Up to `out.length` components of a numeric scalar, vector, quaternion
## or matrix value as f64, in stored order (quaternions imaginary first);
## how many there were, 0 for other values.
pub func value_components(value: const Value*, out: f64[]) -> usize:
    if value.array or value.kind == ValueKind.empty: return 0
    let scalar = scalar_of(value.kind)
    if scalar == value.kind:
        let number = scalar_number(value) else return 0
        if out.length > 0: out[0] = number
        return 1
    let count = components(value.kind)
    let bytes = value.bytes()
    if count == 0 or bytes.length < element_bytes(value.kind): return 0
    let take = count if count < out.length else out.length
    for at in 0..<take: out[at] = component_at(bytes, scalar, at)
    return count

## Component `at` of packed numeric bytes whose scalar kind is `scalar`.
pub func component_at(bytes: const u8[], scalar: ValueKind, at: usize) -> f64:
    match scalar:
        .double: return memory.read[f64]((void*)&bytes[at * 8])
        .float: return (f64)memory.read[f32]((void*)&bytes[at * 4])
        .half: return (f64)f16.bits(memory.read[u16]((void*)&bytes[at * 2]))
        .int: return (f64)memory.read[i32]((void*)&bytes[at * 4])
        _: return 0.0