136 lines
No EOL
4.9 KiB
Python
136 lines
No EOL
4.9 KiB
Python
import numpy as np
|
|
import zlib
|
|
import struct
|
|
from typing import Any, Callable
|
|
|
|
def crc32_array(a: np.ndarray) -> int:
|
|
if not a.size: return 0
|
|
ac = np.ascontiguousarray(a)
|
|
mv = memoryview(ac.data).cast("B")
|
|
return zlib.adler32(mv)
|
|
|
|
def crc32_list(l: list[np.ndarray]) -> int:
|
|
if not l: return 0
|
|
h = 0
|
|
for a in l:
|
|
h = zlib.adler32(struct.pack(">I", crc32_array(a)), h)
|
|
return h
|
|
|
|
def euler_rotation_matrix(rx: float, ry: float, rz: float) -> np.ndarray:
|
|
"""Computes a 3x3 rotation matrix (Rz @ Ry @ Rx) from Euler angles in radians."""
|
|
cx, sx = np.cos(rx), np.sin(rx)
|
|
cy, sy = np.cos(ry), np.sin(ry)
|
|
cz, sz = np.cos(rz), np.sin(rz)
|
|
Rx = np.array([[1.0, 0.0, 0.0], [0.0, cx, -sx], [0.0, sx, cx]], dtype=np.float64)
|
|
Ry = np.array([[cy, 0.0, sy], [0.0, 1.0, 0.0], [-sy, 0.0, cy]], dtype=np.float64)
|
|
Rz = np.array([[cz, -sz, 0.0], [sz, cz, 0.0], [0.0, 0.0, 1.0]], dtype=np.float64)
|
|
return Rz @ (Ry @ Rx)
|
|
|
|
from collections import OrderedDict
|
|
|
|
def _make_hashable(val: Any) -> Any:
|
|
if isinstance(val, np.ndarray):
|
|
return crc32_array(val)
|
|
elif isinstance(val, (list, tuple)):
|
|
if not val:
|
|
return ()
|
|
if isinstance(val[0], np.ndarray):
|
|
return crc32_list(list(val))
|
|
return tuple(_make_hashable(v) for v in val)
|
|
elif isinstance(val, dict):
|
|
return tuple(sorted((k, _make_hashable(v)) for k, v in val.items()))
|
|
elif isinstance(val, set):
|
|
return tuple(sorted(_make_hashable(v) for v in val))
|
|
return val
|
|
|
|
def ndarray_lru_cache(maxsize: int = 128) -> Callable[[Callable[..., Any]], Callable[..., Any]]:
|
|
def decorator(func: Callable[..., Any]) -> Callable[..., Any]:
|
|
cache: OrderedDict[tuple[Any, ...], Any] = OrderedDict()
|
|
def wrapper(*args, **kwargs):
|
|
key_parts = [_make_hashable(arg) for arg in args]
|
|
for k, v in sorted(kwargs.items()):
|
|
key_parts.append((k, _make_hashable(v)))
|
|
|
|
key = tuple(key_parts)
|
|
if key in cache:
|
|
cache.move_to_end(key)
|
|
return cache[key]
|
|
|
|
result = func(*args, **kwargs)
|
|
cache[key] = result
|
|
if len(cache) > maxsize:
|
|
cache.popitem(last=False)
|
|
return result
|
|
return wrapper
|
|
return decorator
|
|
|
|
|
|
def update_transform(
|
|
existing_transforms: list[dict[str, Any]] | dict[str, Any] | None = None,
|
|
t_arr: np.ndarray | list[float] | tuple[float, float, float] | None = None,
|
|
r_arr: np.ndarray | list[float] | tuple[float, float, float] | None = None,
|
|
s_arr: np.ndarray | list[float] | tuple[float, float, float] | None = None,
|
|
) -> list[dict[str, Any]]:
|
|
"""
|
|
Maintains a single consolidated transform record {type: 'transform', translation: (x,y,z), rotation: (rx,ry,rz), scale: (sx,sy,sz)}.
|
|
Combines newly applied translation, Euler rotation (radians), and scale with any pre-existing transform.
|
|
"""
|
|
cur_t = [0.0, 0.0, 0.0]
|
|
cur_r = [0.0, 0.0, 0.0]
|
|
cur_s = [1.0, 1.0, 1.0]
|
|
has_any = False
|
|
|
|
if existing_transforms:
|
|
t_list = [existing_transforms] if isinstance(existing_transforms, dict) else existing_transforms
|
|
for t in t_list:
|
|
if isinstance(t, dict):
|
|
tr = t.get("translation", (0.0, 0.0, 0.0))
|
|
rot = t.get("rotation", (0.0, 0.0, 0.0))
|
|
sc = t.get("scale", (1.0, 1.0, 1.0))
|
|
if "offset" in t:
|
|
tr = t["offset"]
|
|
if any(v != 0.0 for v in tr) or any(v != 0.0 for v in rot) or any(v != 1.0 for v in sc):
|
|
has_any = True
|
|
cur_t[0] += float(tr[0])
|
|
cur_t[1] += float(tr[1])
|
|
cur_t[2] += float(tr[2])
|
|
cur_r[0] += float(rot[0])
|
|
cur_r[1] += float(rot[1])
|
|
cur_r[2] += float(rot[2])
|
|
cur_s[0] *= float(sc[0])
|
|
cur_s[1] *= float(sc[1])
|
|
cur_s[2] *= float(sc[2])
|
|
|
|
if t_arr is not None:
|
|
t_seq = [float(v) for v in t_arr]
|
|
if any(v != 0.0 for v in t_seq):
|
|
has_any = True
|
|
cur_t[0] += t_seq[0]
|
|
cur_t[1] += t_seq[1]
|
|
cur_t[2] += t_seq[2]
|
|
|
|
if r_arr is not None:
|
|
r_seq = [float(v) for v in r_arr]
|
|
if any(v != 0.0 for v in r_seq):
|
|
has_any = True
|
|
cur_r[0] += r_seq[0]
|
|
cur_r[1] += r_seq[1]
|
|
cur_r[2] += r_seq[2]
|
|
|
|
if s_arr is not None:
|
|
s_seq = [float(v) for v in s_arr]
|
|
if any(v != 1.0 for v in s_seq):
|
|
has_any = True
|
|
cur_s[0] *= s_seq[0]
|
|
cur_s[1] *= s_seq[1]
|
|
cur_s[2] *= s_seq[2]
|
|
|
|
if not has_any:
|
|
return []
|
|
|
|
return [{
|
|
"type": "transform",
|
|
"translation": (cur_t[0], cur_t[1], cur_t[2]),
|
|
"rotation": (cur_r[0], cur_r[1], cur_r[2]),
|
|
"scale": (cur_s[0], cur_s[1], cur_s[2]),
|
|
}] |