Source code for scfile.content.models.matrices

"""Matrix transformations."""

import numpy as np
from numpy.typing import NDArray

from .types import EulerAngles, Quaternion, RotationMatrix, TransformMatrix, Vector3D




def create_rotation_matrix(rotation: EulerAngles) -> RotationMatrix:
    """Convert euler angles (XYZ) to 3×3 rotation matrix."""

    angles = np.radians(rotation)
    cx, cy, cz = np.cos(angles)
    sx, sy, sz = np.sin(angles)

    return np.array(
        [
            [cy * cz, -cy * sz, sy],
            [cx * sz + cz * sx * sy, cx * cz - sx * sy * sz, -cy * sx],
            [sx * sz - cx * cz * sy, cz * sx + cx * sy * sz, cx * cy],
        ],
        dtype=np.float32,
    )





def create_transform_matrix(translation: Vector3D, rotation: EulerAngles) -> TransformMatrix:
    """Convert translation and rotation (R * T) to 4×4 transform matrix."""

    matrix = np.eye(4, dtype=np.float32)
    matrix[:3, :3] = create_rotation_matrix(rotation)
    matrix[:3, 3] = translation

    return matrix





def euler_to_quat(rotation: EulerAngles) -> Quaternion:
    """Convert euler angles (XYZ) to quaternion (XYZW)."""

    x, y, z = np.radians(rotation)
    hx, hy, hz = x * 0.5, y * 0.5, z * 0.5

    cx, cy, cz = np.cos([hx, hy, hz])
    sx, sy, sz = np.sin([hx, hy, hz])

    return np.array(
        [
            sx * cy * cz - cx * sy * sz,
            cx * sy * cz + sx * cy * sz,
            cx * cy * sz - sx * sy * cz,
            cx * cy * cz + sx * sy * sz,
        ],
        dtype=np.float32,
    )





def quaternions_to_euler(rotations: NDArray[np.float32]) -> EulerAngles:
    """Convert quaternion keyframes (XYZW) to continuous XYZ euler angles."""

    x, y, z, w = np.moveaxis(rotations, -1, 0)
    roll = np.arctan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y))
    pitch = np.arcsin(np.clip(2.0 * (w * y - z * x), -1.0, 1.0))
    yaw = np.arctan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))

    angles = np.stack((roll, pitch, yaw), axis=-1)
    return np.degrees(np.unwrap(angles, axis=0)).astype(np.float32)