Luce packages
LuceLuce BaseLuciaOS

dymokomi/luce-kernel

Source of release 0.1.2

0.1.2 / src / lib / matrix.lucb

# 3×3 and 4×4 matrices, VEX's `matrix3` and `matrix`. Like VEX, a vector multiplies a
# matrix from the left (`v * M`): the rows are the images of the axes (a 4×4's last row is
# the translation), and `a.multiply(b)` applies `a` first, then `b`. Rows are `f32[4]`;
# a `Mat3` row keeps lane 3 at zero.

## A 3×3 matrix: rows `x`, `y` and `z`, the images of the three axes.
pub struct Mat3:
    pub var x: f32[4] = [1.0, 0.0, 0.0, 0.0]
    pub var y: f32[4] = [0.0, 1.0, 0.0, 0.0]
    pub var z: f32[4] = [0.0, 0.0, 1.0, 0.0]

    ## The identity (VEX `ident()`).
    pub static func identity() -> Mat3:
        return Mat3(x = [1.0, 0.0, 0.0, 0.0], y = [0.0, 1.0, 0.0, 0.0], z = [0.0, 0.0, 1.0, 0.0])

    ## The rotation by `angle` radians about `axis` (any length but zero), counterclockwise
    ## looking down the axis, as VEX's `rotate` makes it.
    pub static func rotation(angle: f32, axis: f32[4]) -> Mat3:
        let k = normalized([axis[0], axis[1], axis[2], 0.0])
        let c = math32.cos(angle)
        let s = math32.sin(angle)
        let t = 1.0 - c
        return Mat3(x = [c + k[0] * k[0] * t, k[0] * k[1] * t + k[2] * s, k[0] * k[2] * t - k[1] * s, 0.0], y = [k[0] * k[1] * t - k[2] * s, c + k[1] * k[1] * t, k[1] * k[2] * t + k[0] * s, 0.0], z = [k[0] * k[2] * t + k[1] * s, k[1] * k[2] * t - k[0] * s, c + k[2] * k[2] * t, 0.0])

    ## The scale by `s[0]`, `s[1]` and `s[2]` along the axes.
    pub static func scaling(s: f32[4]) -> Mat3:
        return Mat3(x = [s[0], 0.0, 0.0, 0.0], y = [0.0, s[1], 0.0, 0.0], z = [0.0, 0.0, s[2], 0.0])

    ## Row `row`, column `column` (both 0 to 2; anything else reads 0).
    pub func at(row: i32, column: i32) -> f32:
        if column < 0 or column > 2: return 0.0
        if row == 0: return self.x[column]
        if row == 1: return self.y[column]
        if row == 2: return self.z[column]
        return 0.0

    ## Rows and columns swapped (VEX `transpose`).
    pub func transposed() -> Mat3:
        return Mat3(x = [self.x[0], self.y[0], self.z[0], 0.0], y = [self.x[1], self.y[1], self.z[1], 0.0], z = [self.x[2], self.y[2], self.z[2], 0.0])

    ## The determinant (VEX `determinant`).
    pub func determinant() -> f32:
        return dot(self.x, cross(self.y, self.z))

    ## The inverse (VEX `invert`); a singular matrix (determinant 0) gives the zero matrix.
    pub func inverted() -> Mat3:
        let det = self.determinant()
        if det == 0.0:
            return Mat3(x = f32[4](0.0), y = f32[4](0.0), z = f32[4](0.0))
        let r = 1.0 / det
        let a = cross(self.y, self.z) * r
        let b = cross(self.z, self.x) * r
        let c = cross(self.x, self.y) * r
        return Mat3(x = [a[0], b[0], c[0], 0.0], y = [a[1], b[1], c[1], 0.0], z = [a[2], b[2], c[2], 0.0])

    ## This matrix, then `other`: VEX's `self * other`.
    pub func multiply(other: Mat3) -> Mat3:
        return Mat3(x = other.transform(self.x), y = other.transform(self.y), z = other.transform(self.z))

    ## `v * self`: `v` (lanes 0 to 2) transformed; lane 3 of the result is zero.
    pub func transform(v: f32[4]) -> f32[4]:
        return self.x * v[0] + self.y * v[1] + self.z * v[2]

    ## This matrix, then the rotation by `angle` radians about `axis` (VEX `rotate`).
    pub func rotate(angle: f32, axis: f32[4]) -> Mat3:
        return self.multiply(Mat3.rotation(angle, axis))

    ## The rotation, then this matrix (VEX `prerotate`).
    pub func prerotate(angle: f32, axis: f32[4]) -> Mat3:
        return Mat3.rotation(angle, axis).multiply(self)

    ## This matrix, then the scale `s` (VEX `scale`).
    pub func scale(s: f32[4]) -> Mat3:
        return self.multiply(Mat3.scaling(s))

    ## The scale `s`, then this matrix (VEX `prescale`).
    pub func prescale(s: f32[4]) -> Mat3:
        return Mat3.scaling(s).multiply(self)

## A 4×4 matrix: rows `x`, `y` and `z` (the images of the axes, lane 3 their projective
## part) and `w`, whose lanes 0 to 2 are the translation.
pub struct Mat4:
    pub var x: f32[4] = [1.0, 0.0, 0.0, 0.0]
    pub var y: f32[4] = [0.0, 1.0, 0.0, 0.0]
    pub var z: f32[4] = [0.0, 0.0, 1.0, 0.0]
    pub var w: f32[4] = [0.0, 0.0, 0.0, 1.0]

    ## The identity (VEX `ident()`).
    pub static func identity() -> Mat4:
        return Mat4(x = [1.0, 0.0, 0.0, 0.0], y = [0.0, 1.0, 0.0, 0.0], z = [0.0, 0.0, 1.0, 0.0], w = [0.0, 0.0, 0.0, 1.0])

    ## `m` as the linear part, translating by `t` (lanes 0 to 2).
    pub static func of(m: Mat3, t: f32[4]) -> Mat4:
        return Mat4(x = m.x, y = m.y, z = m.z, w = [t[0], t[1], t[2], 1.0])

    ## The translation by `t`.
    pub static func translation(t: f32[4]) -> Mat4:
        return Mat4.of(Mat3.identity(), t)

    ## The rotation by `angle` radians about `axis` (see Mat3.rotation).
    pub static func rotation(angle: f32, axis: f32[4]) -> Mat4:
        return Mat4.of(Mat3.rotation(angle, axis), f32[4](0.0))

    ## The scale by `s[0]`, `s[1]` and `s[2]`.
    pub static func scaling(s: f32[4]) -> Mat4:
        return Mat4.of(Mat3.scaling(s), f32[4](0.0))

    ## Row `row`, column `column` (both 0 to 3; anything else reads 0).
    pub func at(row: i32, column: i32) -> f32:
        if column < 0 or column > 3: return 0.0
        if row == 0: return self.x[column]
        if row == 1: return self.y[column]
        if row == 2: return self.z[column]
        if row == 3: return self.w[column]
        return 0.0

    ## The upper-left 3×3: the linear part.
    pub func mat3() -> Mat3:
        return Mat3(x = [self.x[0], self.x[1], self.x[2], 0.0], y = [self.y[0], self.y[1], self.y[2], 0.0], z = [self.z[0], self.z[1], self.z[2], 0.0])

    ## The translation (lanes 0 to 2 of row `w`).
    pub func translation_part() -> f32[4]:
        return [self.w[0], self.w[1], self.w[2], 0.0]

    ## Rows and columns swapped (VEX `transpose`).
    pub func transposed() -> Mat4:
        return Mat4(x = [self.x[0], self.y[0], self.z[0], self.w[0]], y = [self.x[1], self.y[1], self.z[1], self.w[1]], z = [self.x[2], self.y[2], self.z[2], self.w[2]], w = [self.x[3], self.y[3], self.z[3], self.w[3]])

    ## The determinant (VEX `determinant`).
    pub func determinant() -> f32:
        let m = minors(self)
        return m.s[0] * m.c[3] - m.s[1] * m.c[2] + m.s[2] * m.c[1] + m.s[3] * m.c[0]

    ## The inverse (VEX `invert`); a singular matrix (determinant 0) gives the zero matrix.
    pub func inverted() -> Mat4:
        let m = minors(self)
        let det = m.s[0] * m.c[3] - m.s[1] * m.c[2] + m.s[2] * m.c[1] + m.s[3] * m.c[0]
        if det == 0.0:
            return Mat4(x = f32[4](0.0), y = f32[4](0.0), z = f32[4](0.0), w = f32[4](0.0))
        let r = 1.0 / det
        let a = self
        # The adjugate from the 2×2 minors of the top rows (s) and the bottom rows (c).
        let s0 = m.s[0]
        let s1 = m.s[1]
        let s2 = m.s[2]
        let s3 = m.s[3]
        let s4 = m.t[0]
        let s5 = m.t[1]
        let c5 = m.c[3]
        let c4 = m.c[2]
        let c3 = m.c[1]
        let c2 = m.c[0]
        let c1 = m.t[2]
        let c0 = m.t[3]
        return Mat4(x = [(a.y[1] * c5 - a.y[2] * c4 + a.y[3] * c3) * r, (-a.x[1] * c5 + a.x[2] * c4 - a.x[3] * c3) * r, (a.w[1] * s5 - a.w[2] * s4 + a.w[3] * s3) * r, (-a.z[1] * s5 + a.z[2] * s4 - a.z[3] * s3) * r], y = [(-a.y[0] * c5 + a.y[2] * c2 - a.y[3] * c1) * r, (a.x[0] * c5 - a.x[2] * c2 + a.x[3] * c1) * r, (-a.w[0] * s5 + a.w[2] * s2 - a.w[3] * s1) * r, (a.z[0] * s5 - a.z[2] * s2 + a.z[3] * s1) * r], z = [(a.y[0] * c4 - a.y[1] * c2 + a.y[3] * c0) * r, (-a.x[0] * c4 + a.x[1] * c2 - a.x[3] * c0) * r, (a.w[0] * s4 - a.w[1] * s2 + a.w[3] * s0) * r, (-a.z[0] * s4 + a.z[1] * s2 - a.z[3] * s0) * r], w = [(-a.y[0] * c3 + a.y[1] * c1 - a.y[2] * c0) * r, (a.x[0] * c3 - a.x[1] * c1 + a.x[2] * c0) * r, (-a.w[0] * s3 + a.w[1] * s1 - a.w[2] * s0) * r, (a.z[0] * s3 - a.z[1] * s1 + a.z[2] * s0) * r])

    ## This matrix, then `other`: VEX's `self * other`.
    pub func multiply(other: Mat4) -> Mat4:
        return Mat4(x = other.transform4(self.x), y = other.transform4(self.y), z = other.transform4(self.z), w = other.transform4(self.w))

    ## `v * self` for all four lanes of `v`.
    pub func transform4(v: f32[4]) -> f32[4]:
        return self.x * v[0] + self.y * v[1] + self.z * v[2] + self.w * v[3]

    ## A position transformed (VEX `P * M`, lane 3 taken as 1); the result's lane 3 is zero
    ## and there is no divide by the projective part (an affine matrix has none).
    pub func transform_point(p: f32[4]) -> f32[4]:
        let moved = self.x * p[0] + self.y * p[1] + self.z * p[2] + self.w
        return [moved[0], moved[1], moved[2], 0.0]

    ## A direction transformed: the linear part only, no translation.
    pub func transform_vector(v: f32[4]) -> f32[4]:
        let moved = self.x * v[0] + self.y * v[1] + self.z * v[2]
        return [moved[0], moved[1], moved[2], 0.0]

    ## This matrix, then the rotation by `angle` radians about `axis` (VEX `rotate`).
    pub func rotate(angle: f32, axis: f32[4]) -> Mat4:
        return self.multiply(Mat4.rotation(angle, axis))

    ## The rotation, then this matrix (VEX `prerotate`).
    pub func prerotate(angle: f32, axis: f32[4]) -> Mat4:
        return Mat4.rotation(angle, axis).multiply(self)

    ## This matrix, then the scale `s` (VEX `scale`).
    pub func scale(s: f32[4]) -> Mat4:
        return self.multiply(Mat4.scaling(s))

    ## The scale `s`, then this matrix (VEX `prescale`).
    pub func prescale(s: f32[4]) -> Mat4:
        return Mat4.scaling(s).multiply(self)

    ## This matrix, then the translation by `t` (VEX `translate`).
    pub func translate(t: f32[4]) -> Mat4:
        return self.multiply(Mat4.translation(t))

    ## The translation by `t`, then this matrix (VEX `pretranslate`).
    pub func pretranslate(t: f32[4]) -> Mat4:
        return Mat4.translation(t).multiply(self)

## The 2×2 minors a 4×4 determinant and inverse share: `s` and `t[0..1]` of the top two
## rows, `c` and `t[2..3]` of the bottom two.
struct Minors:
    var s: f32[4]
    var c: f32[4]
    var t: f32[4]

func minors(a: Mat4) -> Minors:
    let s = [a.x[0] * a.y[1] - a.y[0] * a.x[1], a.x[0] * a.y[2] - a.y[0] * a.x[2], a.x[0] * a.y[3] - a.y[0] * a.x[3], a.x[1] * a.y[2] - a.y[1] * a.x[2]]
    let c = [a.z[0] * a.w[3] - a.w[0] * a.z[3], a.z[1] * a.w[2] - a.w[1] * a.z[2], a.z[1] * a.w[3] - a.w[1] * a.z[3], a.z[2] * a.w[3] - a.w[2] * a.z[3]]
    let t = [a.x[1] * a.y[3] - a.y[1] * a.x[3], a.x[2] * a.y[3] - a.y[2] * a.x[3], a.z[0] * a.w[2] - a.w[0] * a.z[2], a.z[0] * a.w[1] - a.w[0] * a.z[1]]
    return Minors(s = s, c = c, t = t)

## The 3×3 identity (VEX `ident()` as a `matrix3`).
pub func ident3() -> Mat3:
    return Mat3.identity()

## The 4×4 identity (VEX `ident()` as a `matrix`).
pub func ident4() -> Mat4:
    return Mat4.identity()