@perplexdotgg/bounce 1.0.0

package/src/math/mat4.ts

@@ -0,0 +1,588 @@
+import { createClass, NumberType, PropertyDefinitionMap } from "monomorph";
+import { Vec3 } from "./vec3";
+import { Mat3 } from "./mat3";
+import { Quat } from "./quat";
+
+const rotationMatrix = /*@__PURE__*/ Mat3.create();
+const scaling = /*@__PURE__*/ Vec3.create();
+const conjugatedRotation = /*@__PURE__*/ Quat.create();
+const rotatedTranslation = /*@__PURE__*/ Vec3.create();
+
+const mat4Props = {
+  e0: NumberType(0),
+  e1: NumberType(0),
+  e2: NumberType(0),
+  e3: NumberType(0),
+
+  e4: NumberType(0),
+  e5: NumberType(0),
+  e6: NumberType(0),
+  e7: NumberType(0),
+
+  e8: NumberType(0),
+  e9: NumberType(0),
+  e10: NumberType(0),
+  e11: NumberType(0),
+
+  e12: NumberType(0),
+  e13: NumberType(0),
+  e14: NumberType(0),
+  e15: NumberType(0),
+} as const satisfies PropertyDefinitionMap;
+
+export class Mat4 extends createClass<Mat4, typeof mat4Props>(mat4Props) {
+  multiply3x3(out: Vec3, a: Vec3): Vec3 {
+    const x = a.x;
+    const y = a.y;
+    const z = a.z;
+    out.x = this.e0 * x + this.e4 * y + this.e8 * z;
+    out.y = this.e1 * x + this.e5 * y + this.e9 * z;
+    out.z = this.e2 * x + this.e6 * y + this.e10 * z;
+    return out;
+  }
+
+  multiply3x3Transposed(out: Vec3, a: Vec3): void {
+    rotationMatrix.fromMat4(this);
+    rotationMatrix.transpose();
+    out.transformVectorFromMat3(a, rotationMatrix);
+  }
+
+  computeScaling(out: Vec3) {
+    let m11 = this.e0;
+    let m12 = this.e1;
+    let m13 = this.e2;
+    let m21 = this.e4;
+    let m22 = this.e5;
+    let m23 = this.e6;
+    let m31 = this.e8;
+    let m32 = this.e9;
+    let m33 = this.e10;
+
+    out.x = Math.sqrt(m11 * m11 + m12 * m12 + m13 * m13);
+    out.y = Math.sqrt(m21 * m21 + m22 * m22 + m23 * m23);
+    out.z = Math.sqrt(m31 * m31 + m32 * m32 + m33 * m33);
+
+    return out;
+  }
+
+  computeRotation(out: Quat): Quat {
+    this.computeScaling(scaling);
+
+    let is1 = 1 / scaling.x;
+    let is2 = 1 / scaling.y;
+    let is3 = 1 / scaling.z;
+
+    let sm11 = this.e0 * is1;
+    let sm12 = this.e1 * is2;
+    let sm13 = this.e2 * is3;
+    let sm21 = this.e4 * is1;
+    let sm22 = this.e5 * is2;
+    let sm23 = this.e6 * is3;
+    let sm31 = this.e8 * is1;
+    let sm32 = this.e9 * is2;
+    let sm33 = this.e10 * is3;
+
+    let trace = sm11 + sm22 + sm33;
+    let S = 0;
+
+    if (trace > 0) {
+      S = Math.sqrt(trace + 1) * 2;
+      out.w = 0.25 * S;
+      out.x = (sm23 - sm32) / S;
+      out.y = (sm31 - sm13) / S;
+      out.z = (sm12 - sm21) / S;
+    } else if (sm11 > sm22 && sm11 > sm33) {
+      S = Math.sqrt(1 + sm11 - sm22 - sm33) * 2;
+      out.w = (sm23 - sm32) / S;
+      out.x = 0.25 * S;
+      out.y = (sm12 + sm21) / S;
+      out.z = (sm31 + sm13) / S;
+    } else if (sm22 > sm33) {
+      S = Math.sqrt(1 + sm22 - sm11 - sm33) * 2;
+      out.w = (sm31 - sm13) / S;
+      out.x = (sm12 + sm21) / S;
+      out.y = 0.25 * S;
+      out.z = (sm23 + sm32) / S;
+    } else {
+      S = Math.sqrt(1 + sm33 - sm11 - sm22) * 2;
+      out.w = (sm12 - sm21) / S;
+      out.x = (sm31 + sm13) / S;
+      out.y = (sm23 + sm32) / S;
+      out.z = 0.25 * S;
+    }
+
+    return out;
+  }
+
+  getTranslation(out: Vec3) {
+    out.x = this.e12;
+    out.y = this.e13;
+    out.z = this.e14;
+  }
+
+  setTranslation(v: Vec3): void {
+    this.e12 = v.x;
+    this.e13 = v.y;
+    this.e14 = v.z;
+    this.e15 = 1;
+  }
+
+  preTranslated(translation: Vec3): Mat4 {
+    this.e12 += this.e0 * translation.x + this.e4 * translation.y + this.e8 * translation.z;
+    this.e13 += this.e1 * translation.x + this.e5 * translation.y + this.e9 * translation.z;
+    this.e14 += this.e2 * translation.x + this.e6 * translation.y + this.e10 * translation.z;
+    this.e15 = 1;
+    return this;
+  }
+
+  postTranslated(translation: Vec3): Mat4 {
+    this.e12 += translation.x;
+    this.e13 += translation.y;
+    this.e14 += translation.z;
+    this.e15 = 1;
+    return this;
+  }
+
+  postTranslatedMatrix(mat: Mat4, translation: Vec3) {
+    this.e0 = mat.e0;
+    this.e1 = mat.e1;
+    this.e2 = mat.e2;
+    this.e3 = mat.e3;
+    this.e4 = mat.e4;
+    this.e5 = mat.e5;
+    this.e6 = mat.e6;
+    this.e7 = mat.e7;
+    this.e8 = mat.e8;
+    this.e9 = mat.e9;
+    this.e10 = mat.e10;
+    this.e11 = mat.e11;
+    this.e12 = mat.e12 + translation.x;
+    this.e13 = mat.e13 + translation.y;
+    this.e14 = mat.e14 + translation.z;
+    this.e15 = 1;
+    return this;
+  }
+
+  fromRotationTranslation(q: Quat, v: Vec3): Mat4 {
+    const x = q.x;
+    const y = q.y;
+    const z = q.z;
+    const w = q.w;
+    const x2 = x + x;
+    const y2 = y + y;
+    const z2 = z + z;
+
+    const xx = x * x2;
+    const xy = x * y2;
+    const xz = x * z2;
+    const yy = y * y2;
+    const yz = y * z2;
+    const zz = z * z2;
+    const wx = w * x2;
+    const wy = w * y2;
+    const wz = w * z2;
+
+    this.e0 = 1 - (yy + zz);
+    this.e1 = xy + wz;
+    this.e2 = xz - wy;
+    this.e3 = 0;
+    this.e4 = xy - wz;
+    this.e5 = 1 - (xx + zz);
+    this.e6 = yz + wx;
+    this.e7 = 0;
+    this.e8 = xz + wy;
+    this.e9 = yz - wx;
+    this.e10 = 1 - (xx + yy);
+    this.e11 = 0;
+    this.e12 = v.x;
+    this.e13 = v.y;
+    this.e14 = v.z;
+    this.e15 = 1;
+
+    return this;
+  }
+
+  fromAxisAngle(axis: Vec3, angle: number): Mat4 {
+    rotation.setAxisAngle(axis, angle);
+    this.fromQuat(rotation);
+    this.e12 = 0;
+    this.e13 = 0;
+    this.e14 = 0;
+    this.e15 = 1;
+    return this;
+  }
+
+  fromQuat(q: Quat): Mat4 {
+    let x = q.x,
+      y = q.y,
+      z = q.z,
+      w = q.w;
+    let x2 = x + x;
+    let y2 = y + y;
+    let z2 = z + z;
+
+    let xx = x * x2;
+    let yx = y * x2;
+    let yy = y * y2;
+    let zx = z * x2;
+    let zy = z * y2;
+    let zz = z * z2;
+    let wx = w * x2;
+    let wy = w * y2;
+    let wz = w * z2;
+
+    this.e0 = 1 - yy - zz;
+    this.e1 = yx + wz;
+    this.e2 = zx - wy;
+    this.e3 = 0;
+
+    this.e4 = yx - wz;
+    this.e5 = 1 - xx - zz;
+    this.e6 = zy + wx;
+    this.e7 = 0;
+
+    this.e8 = zx + wy;
+    this.e9 = zy - wx;
+    this.e10 = 1 - xx - yy;
+    this.e11 = 0;
+
+    this.e12 = 0;
+    this.e13 = 0;
+    this.e14 = 0;
+    this.e15 = 1;
+
+    return this;
+  }
+
+  fromInverseRotationAndTranslation(rotation: Quat, translation: Vec3) {
+    conjugatedRotation.conjugateQuat(rotation);
+    this.fromQuat(conjugatedRotation);
+    this.multiply3x3(rotatedTranslation, translation);
+    rotatedTranslation.negate();
+    this.setTranslation(rotatedTranslation);
+    return this;
+  }
+
+  invert() {
+    const a00 = this.e0;
+    const a01 = this.e1;
+    const a02 = this.e2;
+    const a03 = this.e3;
+    const a10 = this.e4;
+    const a11 = this.e5;
+    const a12 = this.e6;
+    const a13 = this.e7;
+    const a20 = this.e8;
+    const a21 = this.e9;
+    const a22 = this.e10;
+    const a23 = this.e11;
+    const a30 = this.e12;
+    const a31 = this.e13;
+    const a32 = this.e14;
+    const a33 = this.e15;
+
+    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;
+
+    // Calculate the determinant
+    let det = b00 * b11 - b01 * b10 + b02 * b09 + b03 * b08 - b04 * b07 + b05 * b06;
+
+    if (!det) {
+      return null;
+    }
+    det = 1.0 / det;
+
+    this.e0 = (a11 * b11 - a12 * b10 + a13 * b09) * det;
+    this.e1 = (a02 * b10 - a01 * b11 - a03 * b09) * det;
+    this.e2 = (a31 * b05 - a32 * b04 + a33 * b03) * det;
+    this.e3 = (a22 * b04 - a21 * b05 - a23 * b03) * det;
+    this.e4 = (a12 * b08 - a10 * b11 - a13 * b07) * det;
+    this.e5 = (a00 * b11 - a02 * b08 + a03 * b07) * det;
+    this.e6 = (a32 * b02 - a30 * b05 - a33 * b01) * det;
+    this.e7 = (a20 * b05 - a22 * b02 + a23 * b01) * det;
+    this.e8 = (a10 * b10 - a11 * b08 + a13 * b06) * det;
+    this.e9 = (a01 * b08 - a00 * b10 - a03 * b06) * det;
+    this.e10 = (a30 * b04 - a31 * b02 + a33 * b00) * det;
+    this.e11 = (a21 * b02 - a20 * b04 - a23 * b00) * det;
+    this.e12 = (a11 * b07 - a10 * b09 - a12 * b06) * det;
+    this.e13 = (a00 * b09 - a01 * b07 + a02 * b06) * det;
+    this.e14 = (a31 * b01 - a30 * b03 - a32 * b00) * det;
+    this.e15 = (a20 * b03 - a21 * b01 + a22 * b00) * det;
+
+    return this;
+  }
+
+  invertMatrix(a: Mat4) {
+    const a00 = a.e0;
+    const a01 = a.e1;
+    const a02 = a.e2;
+    const a03 = a.e3;
+    const a10 = a.e4;
+    const a11 = a.e5;
+    const a12 = a.e6;
+    const a13 = a.e7;
+    const a20 = a.e8;
+    const a21 = a.e9;
+    const a22 = a.e10;
+    const a23 = a.e11;
+    const a30 = a.e12;
+    const a31 = a.e13;
+    const a32 = a.e14;
+    const a33 = a.e15;
+
+    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;
+
+    // Calculate the determinant
+    let det = b00 * b11 - b01 * b10 + b02 * b09 + b03 * b08 - b04 * b07 + b05 * b06;
+
+    if (!det) {
+      return null;
+    }
+    det = 1.0 / det;
+
+    this.e0 = (a11 * b11 - a12 * b10 + a13 * b09) * det;
+    this.e1 = (a02 * b10 - a01 * b11 - a03 * b09) * det;
+    this.e2 = (a31 * b05 - a32 * b04 + a33 * b03) * det;
+    this.e3 = (a22 * b04 - a21 * b05 - a23 * b03) * det;
+    this.e4 = (a12 * b08 - a10 * b11 - a13 * b07) * det;
+    this.e5 = (a00 * b11 - a02 * b08 + a03 * b07) * det;
+    this.e6 = (a32 * b02 - a30 * b05 - a33 * b01) * det;
+    this.e7 = (a20 * b05 - a22 * b02 + a23 * b01) * det;
+    this.e8 = (a10 * b10 - a11 * b08 + a13 * b06) * det;
+    this.e9 = (a01 * b08 - a00 * b10 - a03 * b06) * det;
+    this.e10 = (a30 * b04 - a31 * b02 + a33 * b00) * det;
+    this.e11 = (a21 * b02 - a20 * b04 - a23 * b00) * det;
+    this.e12 = (a11 * b07 - a10 * b09 - a12 * b06) * det;
+    this.e13 = (a00 * b09 - a01 * b07 + a02 * b06) * det;
+    this.e14 = (a31 * b01 - a30 * b03 - a32 * b00) * det;
+    this.e15 = (a20 * b03 - a21 * b01 + a22 * b00) * det;
+
+    return this;
+  }
+
+  getColumn3(out: Vec3, index: number) {
+    switch (index) {
+      case 0: {
+        out.x = this.e0;
+        out.y = this.e1;
+        out.z = this.e2;
+        break;
+      }
+
+      case 1: {
+        out.x = this.e4;
+        out.y = this.e5;
+        out.z = this.e6;
+        break;
+      }
+
+      case 2: {
+        out.x = this.e8;
+        out.y = this.e9;
+        out.z = this.e10;
+        break;
+      }
+    }
+  }
+
+  multiplyMatrix(b: Mat4) {
+    let a00 = this.e0,
+      a01 = this.e1,
+      a02 = this.e2,
+      a03 = this.e3;
+    let a10 = this.e4,
+      a11 = this.e5,
+      a12 = this.e6,
+      a13 = this.e7;
+    let a20 = this.e8,
+      a21 = this.e9,
+      a22 = this.e10,
+      a23 = this.e11;
+    let a30 = this.e12,
+      a31 = this.e13,
+      a32 = this.e14,
+      a33 = this.e15;
+
+    let b0 = b.e0,
+      b1 = b.e1,
+      b2 = b.e2,
+      b3 = b.e3;
+    this.e0 = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30;
+    this.e1 = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31;
+    this.e2 = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32;
+    this.e3 = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33;
+
+    b0 = b.e4;
+    b1 = b.e5;
+    b2 = b.e6;
+    b3 = b.e7;
+    this.e4 = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30;
+    this.e5 = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31;
+    this.e6 = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32;
+    this.e7 = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33;
+
+    b0 = b.e8;
+    b1 = b.e9;
+    b2 = b.e10;
+    b3 = b.e11;
+    this.e8 = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30;
+    this.e9 = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31;
+    this.e10 = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32;
+    this.e11 = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33;
+
+    b0 = b.e12;
+    b1 = b.e13;
+    b2 = b.e14;
+    b3 = b.e15;
+    this.e12 = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30;
+    this.e13 = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31;
+    this.e14 = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32;
+    this.e15 = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33;
+    return this;
+  }
+
+  multiplyMatrices(a: Mat4, b: Mat4) {
+    let a00 = a.e0,
+      a01 = a.e1,
+      a02 = a.e2,
+      a03 = a.e3;
+    let a10 = a.e4,
+      a11 = a.e5,
+      a12 = a.e6,
+      a13 = a.e7;
+    let a20 = a.e8,
+      a21 = a.e9,
+      a22 = a.e10,
+      a23 = a.e11;
+    let a30 = a.e12,
+      a31 = a.e13,
+      a32 = a.e14,
+      a33 = a.e15;
+
+    let b0 = b.e0,
+      b1 = b.e1,
+      b2 = b.e2,
+      b3 = b.e3;
+    this.e0 = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30;
+    this.e1 = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31;
+    this.e2 = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32;
+    this.e3 = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33;
+
+    b0 = b.e4;
+    b1 = b.e5;
+    b2 = b.e6;
+    b3 = b.e7;
+    this.e4 = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30;
+    this.e5 = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31;
+    this.e6 = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32;
+    this.e7 = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33;
+
+    b0 = b.e8;
+    b1 = b.e9;
+    b2 = b.e10;
+    b3 = b.e11;
+    this.e8 = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30;
+    this.e9 = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31;
+    this.e10 = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32;
+    this.e11 = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33;
+
+    b0 = b.e12;
+    b1 = b.e13;
+    b2 = b.e14;
+    b3 = b.e15;
+    this.e12 = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30;
+    this.e13 = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31;
+    this.e14 = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32;
+    this.e15 = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33;
+    return this;
+  }
+
+  preScaled(scale: Vec3) {
+    this.e0 *= scale.x;
+    this.e1 *= scale.x;
+    this.e2 *= scale.x;
+    this.e4 *= scale.y;
+    this.e5 *= scale.y;
+    this.e6 *= scale.y;
+    this.e8 *= scale.z;
+    this.e9 *= scale.z;
+    this.e10 *= scale.z;
+    return this;
+  }
+
+  toObject() {
+    return {
+      e0: this.e0,
+      e1: this.e1,
+      e2: this.e2,
+      e3: this.e3,
+      e4: this.e4,
+      e5: this.e5,
+      e6: this.e6,
+      e7: this.e7,
+      e8: this.e8,
+      e9: this.e9,
+      e10: this.e10,
+      e11: this.e11,
+      e12: this.e12,
+      e13: this.e13,
+      e14: this.e14,
+      e15: this.e15,
+    };
+  }
+
+  identity() {
+    this.e0 = 1;
+    this.e1 = 0;
+    this.e2 = 0;
+    this.e3 = 0;
+
+    this.e4 = 0;
+    this.e5 = 1;
+    this.e6 = 0;
+    this.e7 = 0;
+
+    this.e8 = 0;
+    this.e9 = 0;
+    this.e10 = 1;
+    this.e11 = 0;
+
+    this.e12 = 0;
+    this.e13 = 0;
+    this.e14 = 0;
+    this.e15 = 1;
+
+    return this;
+  }
+
+  getRotationMatrix4(out: Mat4) {
+    out.copy(this);
+    out.e12 = 0;
+    out.e13 = 0;
+    out.e14 = 0;
+    out.e15 = 1;
+  }
+}
+
+const rotation = /*@__PURE__*/ Quat.create();