@galfus/engine 0.0.0-stage → 0.31.0-alpha

package/src/math/mat4.ts

@@ -0,0 +1,576 @@
+/**
+ * 4x4 matrix operations for 3D graphics.
+ * Stored as a 16-element Float32Array in column-major order.
+ */
+
+import { EPSILON, equalsApprox } from './common';
+import type { Quat, ReadonlyQuat } from './quat';
+import type { ReadonlyVec3, Vec3 } from './vec3';
+
+export type Mat4Index = 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15;
+
+export type Mat4 = Float32Array & { [K in Mat4Index]: number };
+
+export type ReadonlyMat4 = ArrayLike<number> & { readonly [K in Mat4Index]: number };
+
+export function create(): Mat4 {
+  const out = new Float32Array(16) as unknown as Mat4;
+  out[0] = 1;
+  out[5] = 1;
+  out[10] = 1;
+  out[15] = 1;
+  return out;
+}
+
+export function identity(out: Mat4): Mat4 {
+  out.fill(0);
+  out[0] = 1;
+  out[5] = 1;
+  out[10] = 1;
+  out[15] = 1;
+  return out;
+}
+
+export function copy(out: Mat4, a: ReadonlyMat4): Mat4 {
+  out.set(a as ArrayLike<number>);
+  return out;
+}
+
+export function multiply(out: Mat4, a: ReadonlyMat4, b: ReadonlyMat4): Mat4 {
+  const a00 = a[0],
+    a01 = a[1],
+    a02 = a[2],
+    a03 = a[3];
+  const a10 = a[4],
+    a11 = a[5],
+    a12 = a[6],
+    a13 = a[7];
+  const a20 = a[8],
+    a21 = a[9],
+    a22 = a[10],
+    a23 = a[11];
+  const a30 = a[12],
+    a31 = a[13],
+    a32 = a[14],
+    a33 = a[15];
+
+  for (let i = 0; i < 4; i++) {
+    const i4 = i * 4;
+    const b0 = (b as ArrayLike<number>)[i4] ?? 0;
+    const b1 = (b as ArrayLike<number>)[i4 + 1] ?? 0;
+    const b2 = (b as ArrayLike<number>)[i4 + 2] ?? 0;
+    const b3 = (b as ArrayLike<number>)[i4 + 3] ?? 0;
+    out[i4] = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30;
+    out[i4 + 1] = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31;
+    out[i4 + 2] = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32;
+    out[i4 + 3] = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33;
+  }
+  return out;
+}
+
+export function fromTranslation(out: Mat4, v: ReadonlyVec3): Mat4 {
+  identity(out);
+  out[12] = v[0];
+  out[13] = v[1];
+  out[14] = v[2];
+  return out;
+}
+
+export function fromScaling(out: Mat4, v: ReadonlyVec3): Mat4 {
+  out.fill(0);
+  out[0] = v[0];
+  out[5] = v[1];
+  out[10] = v[2];
+  out[15] = 1;
+  return out;
+}
+
+export function fromQuat(out: Mat4, q: ReadonlyQuat): Mat4 {
+  const x = q[0];
+  const y = q[1];
+  const z = q[2];
+  const w = q[3];
+  const x2 = x + x;
+  const y2 = y + y;
+  const z2 = z + z;
+
+  const xx = x * x2;
+  const yx = y * x2;
+  const yy = y * y2;
+  const zx = z * x2;
+  const zy = z * y2;
+  const zz = z * z2;
+  const wx = w * x2;
+  const wy = w * y2;
+  const wz = w * z2;
+
+  out[0] = 1 - yy - zz;
+  out[1] = yx + wz;
+  out[2] = zx - wy;
+  out[3] = 0;
+
+  out[4] = yx - wz;
+  out[5] = 1 - xx - zz;
+  out[6] = zy + wx;
+  out[7] = 0;
+
+  out[8] = zx + wy;
+  out[9] = zy - wx;
+  out[10] = 1 - xx - yy;
+  out[11] = 0;
+
+  out[12] = 0;
+  out[13] = 0;
+  out[14] = 0;
+  out[15] = 1;
+  return out;
+}
+
+export function fromRotationTranslationScale(
+  out: Mat4,
+  q: ReadonlyQuat,
+  v: ReadonlyVec3,
+  s: ReadonlyVec3
+): Mat4 {
+  const x = q[0],
+    y = q[1],
+    z = q[2],
+    w = q[3];
+  const x2 = x + x,
+    y2 = y + y,
+    z2 = z + z;
+  const xx = x * x2,
+    xy = x * y2,
+    xz = x * z2;
+  const yy = y * y2,
+    yz = y * z2,
+    zz = z * z2;
+  const wx = w * x2,
+    wy = w * y2,
+    wz = w * z2;
+  const sx = s[0],
+    sy = s[1],
+    sz = s[2];
+
+  out[0] = (1 - (yy + zz)) * sx;
+  out[1] = (xy + wz) * sx;
+  out[2] = (xz - wy) * sx;
+  out[3] = 0;
+  out[4] = (xy - wz) * sy;
+  out[5] = (1 - (xx + zz)) * sy;
+  out[6] = (yz + wx) * sy;
+  out[7] = 0;
+  out[8] = (xz + wy) * sz;
+  out[9] = (yz - wx) * sz;
+  out[10] = (1 - (xx + yy)) * sz;
+  out[11] = 0;
+  out[12] = v[0];
+  out[13] = v[1];
+  out[14] = v[2];
+  out[15] = 1;
+  return out;
+}
+
+export function translate(out: Mat4, a: ReadonlyMat4, v: ReadonlyVec3): Mat4 {
+  const x = v[0];
+  const y = v[1];
+  const z = v[2];
+
+  if (a !== out) {
+    out[0] = a[0];
+    out[1] = a[1];
+    out[2] = a[2];
+    out[3] = a[3];
+    out[4] = a[4];
+    out[5] = a[5];
+    out[6] = a[6];
+    out[7] = a[7];
+    out[8] = a[8];
+    out[9] = a[9];
+    out[10] = a[10];
+    out[11] = a[11];
+  }
+
+  out[12] = a[0] * x + a[4] * y + a[8] * z + a[12];
+  out[13] = a[1] * x + a[5] * y + a[9] * z + a[13];
+  out[14] = a[2] * x + a[6] * y + a[10] * z + a[14];
+  out[15] = a[3] * x + a[7] * y + a[11] * z + a[15];
+  return out;
+}
+
+const _scratchRot = new Float32Array(16) as unknown as Mat4;
+
+/**
+ * Creates a rotation matrix from an angle in radians and an axis.
+ */
+export function fromRotation(out: Mat4, rad: number, axis: ReadonlyVec3): Mat4 {
+  let x = axis[0];
+  let y = axis[1];
+  let z = axis[2];
+  let len = Math.hypot(x, y, z);
+  if (len < EPSILON) return identity(out);
+
+  len = 1 / len;
+  x *= len;
+  y *= len;
+  z *= len;
+  const s = Math.sin(rad);
+  const c = Math.cos(rad);
+  const t = 1 - c;
+
+  out[0] = x * x * t + c;
+  out[1] = y * x * t + z * s;
+  out[2] = z * x * t - y * s;
+  out[3] = 0;
+  out[4] = x * y * t - z * s;
+  out[5] = y * y * t + c;
+  out[6] = z * y * t + x * s;
+  out[7] = 0;
+  out[8] = x * z * t + y * s;
+  out[9] = y * z * t - x * s;
+  out[10] = z * z * t + c;
+  out[11] = 0;
+  out[12] = 0;
+  out[13] = 0;
+  out[14] = 0;
+  out[15] = 1;
+  return out;
+}
+
+/**
+ * Rotates a mat4 by the given angle in radians around the given axis.
+ */
+export function rotate(out: Mat4, rad: number, axis: ReadonlyVec3): Mat4;
+export function rotate(out: Mat4, a: ReadonlyMat4, rad: number, axis: ReadonlyVec3): Mat4;
+export function rotate(
+  out: Mat4,
+  aOrRad: ReadonlyMat4 | number,
+  radOrAxis: number | ReadonlyVec3,
+  maybeAxis?: ReadonlyVec3
+): Mat4 {
+  const a = typeof aOrRad === 'number' ? out : aOrRad;
+  const rad = typeof aOrRad === 'number' ? aOrRad : (radOrAxis as number);
+  const axis = (typeof aOrRad === 'number' ? radOrAxis : maybeAxis) as ReadonlyVec3;
+
+  fromRotation(_scratchRot, rad, axis);
+  return multiply(out, a, _scratchRot);
+}
+
+/**
+ * Generates a look-at matrix with the given eye position, focal point, and up-vector.
+ */
+export function lookAt(out: Mat4, eye: ReadonlyVec3, center: ReadonlyVec3, up: ReadonlyVec3): Mat4 {
+  const eyex = eye[0],
+    eyey = eye[1],
+    eyez = eye[2];
+  const upx = up[0],
+    upy = up[1],
+    upz = up[2];
+  const centerx = center[0],
+    centery = center[1],
+    centerz = center[2];
+
+  if (
+    Math.abs(eyex - centerx) < EPSILON &&
+    Math.abs(eyey - centery) < EPSILON &&
+    Math.abs(eyez - centerz) < EPSILON
+  ) {
+    return identity(out);
+  }
+
+  let z0 = eyex - centerx,
+    z1 = eyey - centery,
+    z2 = eyez - centerz;
+  let len = 1 / Math.hypot(z0, z1, z2);
+  z0 *= len;
+  z1 *= len;
+  z2 *= len;
+
+  let x0 = upy * z2 - upz * z1,
+    x1 = upz * z0 - upx * z2,
+    x2 = upx * z1 - upy * z0;
+  len = Math.hypot(x0, x1, x2);
+  if (!len) {
+    x0 = 0;
+    x1 = 0;
+    x2 = 0;
+  } else {
+    len = 1 / len;
+    x0 *= len;
+    x1 *= len;
+    x2 *= len;
+  }
+
+  let y0 = z1 * x2 - z2 * x1,
+    y1 = z2 * x0 - z0 * x2,
+    y2 = z0 * x1 - z1 * x0;
+  len = Math.hypot(y0, y1, y2);
+  if (!len) {
+    y0 = 0;
+    y1 = 0;
+    y2 = 0;
+  } else {
+    len = 1 / len;
+    y0 *= len;
+    y1 *= len;
+    y2 *= len;
+  }
+
+  out[0] = x0;
+  out[1] = y0;
+  out[2] = z0;
+  out[3] = 0;
+  out[4] = x1;
+  out[5] = y1;
+  out[6] = z1;
+  out[7] = 0;
+  out[8] = x2;
+  out[9] = y2;
+  out[10] = z2;
+  out[11] = 0;
+  out[12] = -(x0 * eyex + x1 * eyey + x2 * eyez);
+  out[13] = -(y0 * eyex + y1 * eyey + y2 * eyez);
+  out[14] = -(z0 * eyex + z1 * eyey + z2 * eyez);
+  out[15] = 1;
+  return out;
+}
+
+export function scale(out: Mat4, a: ReadonlyMat4, v: ReadonlyVec3): Mat4 {
+  const x = v[0];
+  const y = v[1];
+  const z = v[2];
+
+  out[0] = a[0] * x;
+  out[1] = a[1] * x;
+  out[2] = a[2] * x;
+  out[3] = a[3] * x;
+  out[4] = a[4] * y;
+  out[5] = a[5] * y;
+  out[6] = a[6] * y;
+  out[7] = a[7] * y;
+  out[8] = a[8] * z;
+  out[9] = a[9] * z;
+  out[10] = a[10] * z;
+  out[11] = a[11] * z;
+  out[12] = a[12];
+  out[13] = a[13];
+  out[14] = a[14];
+  out[15] = a[15];
+  return out;
+}
+
+export function invert(out: Mat4, a: ReadonlyMat4): Mat4 | null {
+  const a00 = a[0],
+    a01 = a[1],
+    a02 = a[2],
+    a03 = a[3];
+  const a10 = a[4],
+    a11 = a[5],
+    a12 = a[6],
+    a13 = a[7];
+  const a20 = a[8],
+    a21 = a[9],
+    a22 = a[10],
+    a23 = a[11];
+  const a30 = a[12],
+    a31 = a[13],
+    a32 = a[14],
+    a33 = a[15];
+
+  const b00 = a00 * a11 - a01 * a10;
+  const b01 = a00 * a12 - a02 * a10;
+  const b02 = a00 * a13 - a03 * a10;
+  const b03 = a01 * a12 - a02 * a11;
+  const b04 = a01 * a13 - a03 * a11;
+  const b05 = a02 * a13 - a03 * a12;
+  const b06 = a20 * a31 - a21 * a30;
+  const b07 = a20 * a32 - a22 * a30;
+  const b08 = a20 * a33 - a23 * a30;
+  const b09 = a21 * a32 - a22 * a31;
+  const b10 = a21 * a33 - a23 * a31;
+  const b11 = a22 * a33 - a23 * a32;
+
+  let det = b00 * b11 - b01 * b10 + b02 * b09 + b03 * b08 - b04 * b07 + b05 * b06;
+  if (!det) {
+    return null;
+  }
+  det = 1.0 / det;
+
+  out[0] = (a11 * b11 - a12 * b10 + a13 * b09) * det;
+  out[1] = (a02 * b10 - a01 * b11 - a03 * b09) * det;
+  out[2] = (a31 * b05 - a32 * b04 + a33 * b03) * det;
+  out[3] = (a22 * b04 - a21 * b05 - a23 * b03) * det;
+  out[4] = (a12 * b08 - a10 * b11 - a13 * b07) * det;
+  out[5] = (a00 * b11 - a02 * b08 + a03 * b07) * det;
+  out[6] = (a32 * b02 - a30 * b05 - a33 * b01) * det;
+  out[7] = (a20 * b05 - a22 * b02 + a23 * b01) * det;
+  out[8] = (a10 * b10 - a11 * b08 + a13 * b06) * det;
+  out[9] = (a01 * b08 - a00 * b10 - a03 * b06) * det;
+  out[10] = (a30 * b04 - a31 * b02 + a33 * b00) * det;
+  out[11] = (a21 * b02 - a20 * b04 - a23 * b00) * det;
+  out[12] = (a11 * b07 - a10 * b09 - a12 * b06) * det;
+  out[13] = (a00 * b09 - a01 * b07 + a02 * b06) * det;
+  out[14] = (a31 * b01 - a30 * b03 - a32 * b00) * det;
+  out[15] = (a20 * b03 - a21 * b01 + a22 * b00) * det;
+  return out;
+}
+
+export function getTranslation(out: Vec3, mat: ReadonlyMat4): Vec3 {
+  out[0] = mat[12];
+  out[1] = mat[13];
+  out[2] = mat[14];
+  return out;
+}
+
+export function getScaling(out: Vec3, mat: ReadonlyMat4): Vec3 {
+  out[0] = Math.hypot(mat[0], mat[1], mat[2]);
+  out[1] = Math.hypot(mat[4], mat[5], mat[6]);
+  out[2] = Math.hypot(mat[8], mat[9], mat[10]);
+  return out;
+}
+
+export function getRotation(out: Quat, mat: ReadonlyMat4): Quat {
+  const scaling = new Float32Array(3) as unknown as Vec3;
+  getScaling(scaling, mat);
+
+  const isx = 1.0 / (scaling[0] || 1);
+  const isy = 1.0 / (scaling[1] || 1);
+  const isz = 1.0 / (scaling[2] || 1);
+
+  const sm11 = mat[0] * isx;
+  const sm12 = mat[1] * isx;
+  const sm13 = mat[2] * isx;
+  const sm21 = mat[4] * isy;
+  const sm22 = mat[5] * isy;
+  const sm23 = mat[6] * isy;
+  const sm31 = mat[8] * isz;
+  const sm32 = mat[9] * isz;
+  const sm33 = mat[10] * isz;
+
+  const trace = sm11 + sm22 + sm33;
+  let s = 0;
+
+  if (trace > 0) {
+    s = Math.sqrt(trace + 1.0) * 2;
+    out[3] = 0.25 * s;
+    out[0] = (sm23 - sm32) / s;
+    out[1] = (sm31 - sm13) / s;
+    out[2] = (sm12 - sm21) / s;
+  } else if (sm11 > sm22 && sm11 > sm33) {
+    s = Math.sqrt(1.0 + sm11 - sm22 - sm33) * 2;
+    out[3] = (sm23 - sm32) / s;
+    out[0] = 0.25 * s;
+    out[1] = (sm12 + sm21) / s;
+    out[2] = (sm31 + sm13) / s;
+  } else if (sm22 > sm33) {
+    s = Math.sqrt(1.0 + sm22 - sm11 - sm33) * 2;
+    out[3] = (sm31 - sm13) / s;
+    out[0] = (sm12 + sm21) / s;
+    out[1] = 0.25 * s;
+    out[2] = (sm23 + sm32) / s;
+  } else {
+    s = Math.sqrt(1.0 + sm33 - sm11 - sm22) * 2;
+    out[3] = (sm12 - sm21) / s;
+    out[0] = (sm31 + sm13) / s;
+    out[1] = (sm23 + sm32) / s;
+    out[2] = 0.25 * s;
+  }
+  return out;
+}
+
+export function equals(a: ReadonlyMat4, b: ReadonlyMat4, epsilon: number = EPSILON): boolean {
+  for (let i = 0; i < 16; i++) {
+    const valA = a[i as keyof ReadonlyMat4] as number;
+    const valB = b[i as keyof ReadonlyMat4] as number;
+    if (!equalsApprox(valA, valB, epsilon)) return false;
+  }
+  return true;
+}
+
+/**
+ * Copies the translation vector components from `from` into `out`.
+ */
+export function copyPosition(out: Mat4, from: ReadonlyMat4): Mat4 {
+  out[12] = from[12];
+  out[13] = from[13];
+  out[14] = from[14];
+  return out;
+}
+
+/**
+ * Sets the translation vector components in `out`.
+ */
+export function setTranslation(
+  out: Mat4,
+  xOrVec: number | ReadonlyVec3,
+  y?: number,
+  z?: number
+): Mat4 {
+  if (typeof xOrVec === 'number') {
+    out[12] = xOrVec;
+    out[13] = y ?? 0;
+    out[14] = z ?? 0;
+  } else {
+    out[12] = xOrVec[0];
+    out[13] = xOrVec[1];
+    out[14] = xOrVec[2];
+  }
+  return out;
+}
+
+/**
+ * Copies the 3x3 rotation basis from `from` into `out`, preserving the scale of `out`.
+ */
+export function copyRotation(out: Mat4, from: ReadonlyMat4): Mat4 {
+  const sx = Math.hypot(out[0], out[1], out[2]) || 1;
+  const sy = Math.hypot(out[4], out[5], out[6]) || 1;
+  const sz = Math.hypot(out[8], out[9], out[10]) || 1;
+
+  const fsx = Math.hypot(from[0], from[1], from[2]) || 1;
+  const fsy = Math.hypot(from[4], from[5], from[6]) || 1;
+  const fsz = Math.hypot(from[8], from[9], from[10]) || 1;
+
+  out[0] = (from[0] / fsx) * sx;
+  out[1] = (from[1] / fsx) * sx;
+  out[2] = (from[2] / fsx) * sx;
+
+  out[4] = (from[4] / fsy) * sy;
+  out[5] = (from[5] / fsy) * sy;
+  out[6] = (from[6] / fsy) * sy;
+
+  out[8] = (from[8] / fsz) * sz;
+  out[9] = (from[9] / fsz) * sz;
+  out[10] = (from[10] / fsz) * sz;
+
+  return out;
+}
+
+/**
+ * Performs a component-wise linear interpolation between two 4x4 transformation matrices.
+ */
+export function lerp(out: Mat4, a: ReadonlyMat4, b: ReadonlyMat4, t: number): Mat4 {
+  for (let i = 0; i < 16; i++) {
+    const ai = a[i as keyof ReadonlyMat4] as number;
+    const bi = b[i as keyof ReadonlyMat4] as number;
+    out[i] = ai + t * (bi - ai);
+  }
+  return out;
+}
+
+/**
+ * Clamps each component of a 4x4 transformation matrix between min and max.
+ */
+export function clamp(
+  out: Mat4,
+  a: ReadonlyMat4,
+  min: ReadonlyMat4 | number,
+  max: ReadonlyMat4 | number
+): Mat4 {
+  for (let i = 0; i < 16; i++) {
+    const ai = a[i as keyof ReadonlyMat4] as number;
+    const minVal = typeof min === 'number' ? min : (min[i as keyof ReadonlyMat4] as number);
+    const maxVal = typeof max === 'number' ? max : (max[i as keyof ReadonlyMat4] as number);
+    out[i] = Math.min(Math.max(ai, minVal), maxVal);
+  }
+  return out;
+}