Luce packages
LuceLuce BaseLuciaOS

dymokomi/luce-fbx

Source of release 0.12.2

0.12.2 / src / scene / rotation.lucb

## Rotations and translation-rotation-scale transforms, as FBX (and ufbx)
## compose them: Euler angles in degrees applied in a rotation order, kept
## as unit quaternions until a matrix is needed.

pub struct Quat:
    pub var x: f64 = 0.0
    pub var y: f64 = 0.0
    pub var z: f64 = 0.0
    pub var w: f64 = 1.0

    ## This rotation after `other` (other is applied first).
    pub func times(other: Quat) -> Quat:
        return Quat(self.w * other.x + self.x * other.w + self.y * other.z - self.z * other.y,
                    self.w * other.y - self.x * other.z + self.y * other.w + self.z * other.x,
                    self.w * other.z + self.x * other.y - self.y * other.x + self.z * other.w,
                    self.w * other.w - self.x * other.x - self.y * other.y - self.z * other.z)

    pub func inverse() -> Quat:
        return Quat(-self.x, -self.y, -self.z, self.w)

    pub func rotate(v: Vector3) -> Vector3:
        # v + 2w(q x v) + 2 q x (q x v)
        let q = Vector3(self.x, self.y, self.z)
        let t = q.cross(v).multiply_scalar(2.0)
        return v.add(t.multiply_scalar(self.w)).add(q.cross(t))

    pub func is_identity() -> bool:
        return self.x == 0.0 and self.y == 0.0 and self.z == 0.0 and self.w == 1.0

## The rotation `t` of the way from `a` to `b` (ufbx_quat_slerp).
pub func slerp(a: Quat, b: Quat, t: f64) -> Quat:
    var other = b
    var dot = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w
    if dot < 0.0:
        dot = -dot
        other = Quat(-b.x, -b.y, -b.z, -b.w)
    let omega = math.acos(math.min(math.max(dot, 0.0), 1.0))
    if omega <= 1.175494351e-38: return a
    let reciprocal = 1.0 / math.sin(omega)
    let af = math.sin((1.0 - t) * omega) * reciprocal
    let bf = math.sin(t * omega) * reciprocal
    let x = af * a.x + bf * other.x
    let y = af * a.y + bf * other.y
    let z = af * a.z + bf * other.z
    let w = af * a.w + bf * other.w
    let length = 1.0 / math.sqrt(x * x + y * y + z * z + w * w)
    return Quat(x * length, y * length, z * length, w * length)

## Euler angles in degrees of rotation `q` in `order` (ufbx_quat_to_euler,
## its formulas generated for each order).
pub func quat_euler(q: Quat, order: i64) -> Vector3:
    let eps = 0.999999999
    let qx = q.x
    let qy = q.y
    let qz = q.z
    let qw = q.w
    var vx: f64 = 0.0
    var vy: f64 = 0.0
    var vz: f64 = 0.0
    var t: f64 = 0.0
    if order == 0:
        t = 2.0 * (qw * qy - qx * qz)
        if math.abs(t) < eps:
            vy = math.asin(t)
            vz = math.atan2(2.0 * (qw * qz + qx * qy), 2.0 * (qw * qw + qx * qx) - 1.0)
            vx = -math.atan2(-2.0 * (qw * qx + qy * qz), 2.0 * (qw * qw + qz * qz) - 1.0)
        else:
            vy = (math.pi * 0.5 if t >= 0.0 else -math.pi * 0.5)
            vz = math.atan2(-2.0 * t * (qw * qx - qy * qz), t * (2.0 * qw * qy + 2.0 * qx * qz))
            vx = 0.0
    elif order == 1:
        t = 2.0 * (qw * qz + qx * qy)
        if math.abs(t) < eps:
            vz = math.asin(t)
            vy = math.atan2(2.0 * (qw * qy - qx * qz), 2.0 * (qw * qw + qx * qx) - 1.0)
            vx = -math.atan2(-2.0 * (qw * qx - qy * qz), 2.0 * (qw * qw + qy * qy) - 1.0)
        else:
            vz = (math.pi * 0.5 if t >= 0.0 else -math.pi * 0.5)
            vy = math.atan2(2.0 * t * (qw * qx + qy * qz), -t * (2.0 * qx * qy - 2.0 * qw * qz))
            vx = 0.0
    elif order == 2:
        t = 2.0 * (qw * qz - qx * qy)
        if math.abs(t) < eps:
            vz = math.asin(t)
            vx = math.atan2(2.0 * (qw * qx + qy * qz), 2.0 * (qw * qw + qy * qy) - 1.0)
            vy = -math.atan2(-2.0 * (qw * qy + qx * qz), 2.0 * (qw * qw + qx * qx) - 1.0)
        else:
            vz = (math.pi * 0.5 if t >= 0.0 else -math.pi * 0.5)
            vx = math.atan2(-2.0 * t * (qw * qy - qx * qz), t * (2.0 * qw * qz + 2.0 * qx * qy))
            vy = 0.0
    elif order == 3:
        t = 2.0 * (qw * qx + qy * qz)
        if math.abs(t) < eps:
            vx = math.asin(t)
            vz = math.atan2(2.0 * (qw * qz - qx * qy), 2.0 * (qw * qw + qy * qy) - 1.0)
            vy = -math.atan2(-2.0 * (qw * qy - qx * qz), 2.0 * (qw * qw + qz * qz) - 1.0)
        else:
            vx = (math.pi * 0.5 if t >= 0.0 else -math.pi * 0.5)
            vz = math.atan2(2.0 * t * (qw * qy + qx * qz), -t * (2.0 * qy * qz - 2.0 * qw * qx))
            vy = 0.0
    elif order == 4:
        t = 2.0 * (qw * qx - qy * qz)
        if math.abs(t) < eps:
            vx = math.asin(t)
            vy = math.atan2(2.0 * (qw * qy + qx * qz), 2.0 * (qw * qw + qz * qz) - 1.0)
            vz = -math.atan2(-2.0 * (qw * qz + qx * qy), 2.0 * (qw * qw + qy * qy) - 1.0)
        else:
            vx = (math.pi * 0.5 if t >= 0.0 else -math.pi * 0.5)
            vy = math.atan2(-2.0 * t * (qw * qz - qx * qy), t * (2.0 * qw * qx + 2.0 * qy * qz))
            vz = 0.0
    elif order == 5:
        t = 2.0 * (qw * qy + qx * qz)
        if math.abs(t) < eps:
            vy = math.asin(t)
            vx = math.atan2(2.0 * (qw * qx - qy * qz), 2.0 * (qw * qw + qz * qz) - 1.0)
            vz = -math.atan2(-2.0 * (qw * qz - qx * qy), 2.0 * (qw * qw + qx * qx) - 1.0)
        else:
            vy = (math.pi * 0.5 if t >= 0.0 else -math.pi * 0.5)
            vx = math.atan2(2.0 * t * (qw * qz + qx * qy), -t * (2.0 * qx * qz - 2.0 * qw * qy))
            vz = 0.0
    return Vector3(vx * (180.0 / math.pi), vy * (180.0 / math.pi), vz * (180.0 / math.pi))

## A rotation of `degrees` about axis `axis` (0 X, 1 Y, 2 Z).
func axis_rotation(axis: usize, degrees: f64) -> Quat:
    let half = degrees * (math.pi / 360.0)
    let s = math.sin(half)
    let c = math.cos(half)
    match axis:
        0: return Quat(s, 0.0, 0.0, c)
        1: return Quat(0.0, s, 0.0, c)
        _: return Quat(0.0, 0.0, s, c)

## The axes of each FBX rotation order, first applied first: XYZ, XZY, YZX,
## YXZ, ZXY, ZYX; spheric XYZ (6) rotates as XYZ, as ufbx does.
let order_axes: u8[18] = [0, 1, 2, 0, 2, 1, 1, 2, 0, 1, 0, 2, 2, 0, 1, 2, 1, 0]

## Euler angles in degrees as a rotation, in `order` (an FBX RotationOrder).
pub func euler(degrees: Vector3, order: i64) -> Quat:
    var row: usize = 0
    if order >= 0 and order < 6: row = (usize)order * 3
    let angles: f64[3] = [degrees.x, degrees.y, degrees.z]
    var result = Quat()
    for step: usize in 0..<3:
        let axis = (usize)order_axes[row + step]
        if angles[axis] != 0.0: result = axis_rotation(axis, angles[axis]).times(result)
    return result

## Translation, rotation and scale: the point is scaled, rotated, then moved.
pub struct Trs:
    pub var translation: Vector3 = Vector3()
    pub var rotation: Quat = Quat()
    pub var scale: Vector3 = Vector3(1.0, 1.0, 1.0)

    ## Scale the whole transform (as S * T, ufbx's ufbxi_mul_scale).
    pub func scaled(by: Vector3):
        self.translation = Vector3(self.translation.x * by.x, self.translation.y * by.y, self.translation.z * by.z)
        self.scale = Vector3(self.scale.x * by.x, self.scale.y * by.y, self.scale.z * by.z)

    ## Rotate the whole transform (as R * T).
    pub func rotated(by: Quat):
        if by.is_identity(): return
        self.rotation = by.times(self.rotation)
        self.translation = by.rotate(self.translation)

    pub func moved(by: Vector3):
        self.translation = self.translation.add(by)

    pub func matrix() -> Matrix4:
        let q = self.rotation
        let x = q.rotate(Vector3(self.scale.x, 0.0, 0.0))
        let y = q.rotate(Vector3(0.0, self.scale.y, 0.0))
        let z = q.rotate(Vector3(0.0, 0.0, self.scale.z))
        return Matrix4(x, y, z, self.translation)

    ## The matrix without the scale.
    pub func unscaled() -> Matrix4:
        var copy = self
        copy.scale = Vector3(1.0, 1.0, 1.0)
        return copy.matrix()

    pub func is_identity() -> bool:
        return self.translation == Vector3() and self.rotation.is_identity() and self.scale == Vector3(1.0, 1.0, 1.0)