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