From 5cff0da6c6a5e54a2063e18725e3f98dd2dbe266 Mon Sep 17 00:00:00 2001 From: William Candillon Date: Sat, 9 Dec 2023 11:51:05 +0100 Subject: [PATCH] =?UTF-8?q?=20fix(=F0=9F=90=9B):=20fix=20bugs=20in=20the?= =?UTF-8?q?=204x4=20matrix=20handling=20(#519)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Matrix4.ts | 275 ++++++++++++++-------------------- src/__tests__/Matrix4.test.ts | 76 ++++++---- src/__tests__/matrix.ts | 164 ++++++++++++++++++++ 3 files changed, 325 insertions(+), 190 deletions(-) create mode 100644 src/__tests__/matrix.ts diff --git a/src/Matrix4.ts b/src/Matrix4.ts index d076b94..d7888c4 100644 --- a/src/Matrix4.ts +++ b/src/Matrix4.ts @@ -1,8 +1,6 @@ -/* eslint-disable prefer-destructuring */ -import type { Matrix3 } from "./Matrix3"; - -export type Vec3 = readonly [number, number, number]; -export type Vec4 = readonly [number, number, number, number]; +type Vec2 = readonly [number, number]; +type Vec3 = readonly [number, number, number]; +type Vec4 = readonly [number, number, number, number]; export type Matrix4 = readonly [ number, @@ -27,6 +25,7 @@ type Transform3dName = | "translateX" | "translateY" | "translateZ" + | "translate" | "scale" | "scaleX" | "scaleY" @@ -41,13 +40,18 @@ type Transform3dName = | "matrix"; type Transformations = { - [Name in Transform3dName]: Name extends "matrix" ? Matrix4 : number; + [Name in Transform3dName]: Name extends "matrix" + ? Matrix4 + : Name extends "translate" + ? Vec3 | Vec2 + : number; }; -export type Transforms3d = ( +type Transform3d = | Pick | Pick | Pick + | Pick | Pick | Pick | Pick @@ -58,103 +62,80 @@ export type Transforms3d = ( | Pick | Pick | Pick - | Pick -)[]; + | Pick; + +export type Transforms3d = Transform3d[]; -/** - * @worklet - */ const exhaustiveCheck = (a: never): never => { "worklet"; throw new Error(`Unexhaustive handling for ${a}`); }; +/** + * @worklet + */ export const identity4: Matrix4 = [ 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, ]; -/** - * @worklet - */ const translate = (x: number, y: number, z: number): Matrix4 => { "worklet"; - return [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, x, y, z, 1]; + return [1, 0, 0, x, 0, 1, 0, y, 0, 0, 1, z, 0, 0, 0, 1]; }; -/** - * @worklet - */ const scale = (sx: number, sy: number, sz: number): Matrix4 => { "worklet"; return [sx, 0, 0, 0, 0, sy, 0, 0, 0, 0, sz, 0, 0, 0, 0, 1]; }; -/** - * @worklet - */ const skewX = (s: number): Matrix4 => { "worklet"; return [1, 0, 0, 0, Math.tan(s), 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1]; }; -/** - * @worklet - */ const skewY = (s: number): Matrix4 => { "worklet"; return [1, Math.tan(s), 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1]; }; -/** - * @worklet - */ const perspective = (p: number): Matrix4 => { "worklet"; return [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, -1 / p, 1]; }; -/** - * @worklet - */ -const rotateX = (r: number): Matrix4 => { +const normalizeVec = (vec: Vec3): Vec3 => { "worklet"; - return [ - 1, - 0, - 0, - 0, - 0, - Math.cos(r), - Math.sin(r), - 0, - 0, - -Math.sin(r), - Math.cos(r), - 0, - 0, - 0, - 0, - 1, - ]; + const [x, y, z] = vec; + const length = Math.sqrt(x * x + y * y + z * z); + // Check for zero length to avoid division by zero + if (length === 0) { + return [0, 0, 0]; + } + return [x / length, y / length, z / length]; }; -/** - * @worklet - */ -const rotateY = (r: number): Matrix4 => { +const rotatedUnitSinCos = ( + axisVec: Vec3, + sinAngle: number, + cosAngle: number +): Matrix4 => { "worklet"; + const [x, y, z] = axisVec; + const c = cosAngle; + const s = sinAngle; + const t = 1 - c; return [ - Math.cos(r), + t * x * x + c, + t * x * y - s * z, + t * x * z + s * y, 0, - -Math.sin(r), + t * x * y + s * z, + t * y * y + c, + t * y * z - s * x, 0, - 0, - 1, - 0, - 0, - Math.sin(r), - 0, - Math.cos(r), + t * x * z - s * y, + t * y * z + s * x, + t * z * z + c, 0, 0, 0, @@ -163,29 +144,13 @@ const rotateY = (r: number): Matrix4 => { ]; }; -/** - * @worklet - */ -const rotateZ = (r: number): Matrix4 => { +const rotate = (axis: Vec3, value: number) => { "worklet"; - return [ - Math.cos(r), - Math.sin(r), - 0, - 0, - -Math.sin(r), - Math.cos(r), - 0, - 0, - 0, - 0, - 1, - 0, - 0, - 0, - 0, - 1, - ]; + return rotatedUnitSinCos( + normalizeVec(axis), + Math.sin(value), + Math.cos(value) + ); }; /** @@ -207,77 +172,34 @@ export const matrixVecMul4 = (m: Matrix4, v: Vec4) => { */ export const mapPoint3d = (m: Matrix4, v: Vec3) => { "worklet"; - const r = matrixVecMul4(m, [v[0], v[1], v[2], 1]); + const r = matrixVecMul4(m, [...v, 1]); return [r[0] / r[3], r[1] / r[3], r[2] / r[3]] as const; }; /** * @worklet */ -export const multiply4 = (a: Matrix4, b: Matrix4) => { +export const multiply4 = (a: Matrix4, b: Matrix4): Matrix4 => { "worklet"; - const out = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]; - const a00 = a[0], - a01 = a[1], - a02 = a[2], - a03 = a[3], - a10 = a[4], - a11 = a[5], - a12 = a[6], - a13 = a[7], - a20 = a[8], - a21 = a[9], - a22 = a[10], - a23 = a[11], - a30 = a[12], - a31 = a[13], - a32 = a[14], - a33 = a[15]; - - let b0 = b[0], - b1 = b[1], - b2 = b[2], - b3 = b[3]; - out[0] = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30; - out[1] = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31; - out[2] = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32; - out[3] = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33; - - b0 = b[4]; - b1 = b[5]; - b2 = b[6]; - b3 = b[7]; - out[4] = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30; - out[5] = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31; - out[6] = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32; - out[7] = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33; - - b0 = b[8]; - b1 = b[9]; - b2 = b[10]; - b3 = b[11]; - out[8] = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30; - out[9] = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31; - out[10] = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32; - out[11] = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33; - - b0 = b[12]; - b1 = b[13]; - b2 = b[14]; - b3 = b[15]; - out[12] = b0 * a00 + b1 * a10 + b2 * a20 + b3 * a30; - out[13] = b0 * a01 + b1 * a11 + b2 * a21 + b3 * a31; - out[14] = b0 * a02 + b1 * a12 + b2 * a22 + b3 * a32; - out[15] = b0 * a03 + b1 * a13 + b2 * a23 + b3 * a33; - return out as unknown as Matrix4; + const result = new Array(16).fill(0); + for (let i = 0; i < 4; i++) { + for (let j = 0; j < 4; j++) { + result[i * 4 + j] = + a[i * 4] * b[j] + + a[i * 4 + 1] * b[j + 4] + + a[i * 4 + 2] * b[j + 8] + + a[i * 4 + 3] * b[j + 12]; + } + } + return result as unknown as Matrix4; }; /** * @worklet */ -export const toMatrix3 = (m: Matrix4): Matrix3 => { +export const toMatrix3 = (m: Matrix4) => { "worklet"; - return [m[0], m[4], m[12], m[1], m[5], m[13], m[3], m[7], m[15]]; + return [m[0], m[1], m[3], m[4], m[5], m[7], m[12], m[13], m[15]]; }; /** @@ -285,59 +207,64 @@ export const toMatrix3 = (m: Matrix4): Matrix3 => { */ export const processTransform3d = (transforms: Transforms3d) => { "worklet"; - return transforms.reduce((acc, transform) => { - const key = Object.keys(transform)[0] as Transform3dName; + return transforms.reduce((acc, val) => { + const key = Object.keys(val)[0] as Transform3dName; + const transform = val as Pick; if (key === "translateX") { - const value = (transform as Pick)[key]; + const value = transform[key]; return multiply4(acc, translate(value, 0, 0)); } + if (key === "translate") { + const [x, y, z = 0] = transform[key]; + return multiply4(acc, translate(x, y, z)); + } if (key === "translateY") { - const value = (transform as Pick)[key]; + const value = transform[key]; return multiply4(acc, translate(0, value, 0)); } if (key === "translateZ") { - const value = (transform as Pick)[key]; + const value = transform[key]; return multiply4(acc, translate(0, 0, value)); } if (key === "scale") { - const value = (transform as Pick)[key]; + const value = transform[key]; return multiply4(acc, scale(value, value, 1)); } if (key === "scaleX") { - const value = (transform as Pick)[key]; + const value = transform[key]; return multiply4(acc, scale(value, 1, 1)); } if (key === "scaleY") { - const value = (transform as Pick)[key]; + const value = transform[key]; return multiply4(acc, scale(1, value, 1)); } if (key === "skewX") { - const value = (transform as Pick)[key]; + const value = transform[key]; return multiply4(acc, skewX(value)); } if (key === "skewY") { - const value = (transform as Pick)[key]; + const value = transform[key]; return multiply4(acc, skewY(value)); } if (key === "rotateX") { - const value = (transform as Pick)[key]; - return multiply4(acc, rotateX(value)); + const value = transform[key]; + return multiply4(acc, rotate([1, 0, 0], value)); } if (key === "rotateY") { - const value = (transform as Pick)[key]; - return multiply4(acc, rotateY(value)); + const value = transform[key]; + return multiply4(acc, rotate([0, 1, 0], value)); } if (key === "perspective") { - const value = (transform as Pick)[key]; + const value = transform[key]; return multiply4(acc, perspective(value)); } if (key === "rotate" || key === "rotateZ") { - const value = (transform as Pick)[key]; - return multiply4(acc, rotateZ(value)); + const value = transform[key]; + return multiply4(acc, rotate([0, 0, 1], value)); } if (key === "matrix") { - const matrix = (transform as Pick)[key]; - return multiply4(acc, matrix); + const value = transform[key]; + return multiply4(acc, value); } return exhaustiveCheck(key); }, identity4); @@ -351,6 +278,30 @@ export const concat4 = (m: Matrix4, transform: Transforms3d) => { return multiply4(m, processTransform3d(transform)); }; +/** + * @worklet + */ +export const rotateX = (value: number) => { + "worklet"; + return rotate([1, 0, 0], value); +}; + +/** + * @worklet + */ +export const rotateY = (value: number) => { + "worklet"; + return rotate([0, 1, 0], value); +}; + +/** + * @worklet + */ +export const rotateZ = (value: number) => { + "worklet"; + return rotate([0, 0, 1], value); +}; + export const Matrix4 = { translate, scale, diff --git a/src/__tests__/Matrix4.test.ts b/src/__tests__/Matrix4.test.ts index bf2dc8a..2a5932b 100644 --- a/src/__tests__/Matrix4.test.ts +++ b/src/__tests__/Matrix4.test.ts @@ -1,12 +1,22 @@ import { concat4, - multiply4, identity4, + mapPoint3d, matrixVecMul4, + multiply4, processTransform3d, - mapPoint3d, } from "../Matrix4"; +import { + identity, + rotated, + multiply, + transformPoint, + translated, + scaled, + transformPoint3d, +} from "./matrix"; + const expectArrayCloseTo = ( a: readonly number[], b: readonly number[], @@ -19,54 +29,57 @@ const expectArrayCloseTo = ( }; test("processTransform3d()", () => { + let ref = identity(); + ref = multiply(ref, rotated([1, 0, 0], Math.PI)); + ref = multiply(ref, rotated([0, 1, 0], Math.PI)); expect( processTransform3d([{ rotateX: Math.PI }, { rotateY: Math.PI }]) - ).toStrictEqual([ - -1, 1.4997597826618576e-32, 1.2246467991473532e-16, 0, 0, -1, - 1.2246467991473532e-16, 0, 1.2246467991473532e-16, 1.2246467991473532e-16, - 1, 0, 0, 0, 0, 1, - ]); + ).toStrictEqual(ref); }); test("multiply4()", () => { + let ref = identity(); + ref = multiply(ref, rotated([1, 0, 0], Math.PI)); + ref = multiply(ref, rotated([0, 1, 0], Math.PI)); expect( multiply4( identity4, processTransform3d([{ rotateX: Math.PI }, { rotateY: Math.PI }]) ) - ).toStrictEqual([ - -1, 1.4997597826618576e-32, 1.2246467991473532e-16, 0, 0, -1, - 1.2246467991473532e-16, 0, 1.2246467991473532e-16, 1.2246467991473532e-16, - 1, 0, 0, 0, 0, 1, - ]); + ).toStrictEqual(ref); }); test("matrixVecMul4()", () => { + let ref = identity(); + ref = multiply(ref, rotated([1, 0, 0], Math.PI)); + ref = multiply(ref, rotated([0, 1, 0], Math.PI)); + const refVec = transformPoint(ref, [0.5, 0.5, 0, 1]); expect( matrixVecMul4( processTransform3d([{ rotateX: Math.PI }, { rotateY: Math.PI }]), [0.5, 0.5, 0, 1] ) - ).toStrictEqual([-0.5, -0.5, 1.2246467991473532e-16, 1]); + ).toStrictEqual(refVec); }); describe("4x4 matrices", () => { it("can make a translated 4x4 matrix", () => { + let ref = identity(); + ref = multiply(ref, translated([5, 6, 7])); expectArrayCloseTo( processTransform3d([ { translateX: 5 }, { translateY: 6 }, { translateZ: 7 }, ]), - [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 5, 6, 7, 1] + ref ); }); it("can make a scaled 4x4 matrix", () => { - expectArrayCloseTo( - processTransform3d([{ scaleX: 5 }, { scaleY: 6 }]), - [5, 0, 0, 0, 0, 6, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1] - ); + let ref = identity(); + ref = multiply(ref, scaled([5, 6, 1])); + expectArrayCloseTo(processTransform3d([{ scaleX: 5 }, { scaleY: 6 }]), ref); }); it("can multiply 4x4 matrices", () => { @@ -78,30 +91,37 @@ describe("4x4 matrices", () => { 2.0, 3.0, 4.0, 5.0, -3.0, -4.0, -5.0, -6.0, 7.0, 8.0, 9.0, 10.0, -4.0, -3.0, -2.0, -1.0, ] as const; - const expected = [ - 0.8000000000000003, -3.9000000000000004, -2.5000000000000004, - -1.0999999999999996, -1.2000000000000002, 4.5, 2.7000000000000006, - 0.8999999999999995, 2.799999999999999, -6.8999999999999995, - -3.500000000000001, -0.09999999999999876, -1.6, -0.2999999999999996, - -1.2999999999999998, -2.3000000000000003, - ] as const; - expect(multiply4(a, b)).toEqual(expected); + let ref = identity(); + ref = multiply(a, b); + expect(multiply4(a, b)).toEqual(ref); }); it("maps 3D points correctly with a 3x3 matrix", () => { + // 1. + let ref = identity(); + ref = multiply(ref, translated([10, 0, 0])); + ref = multiply(ref, scaled([2, 2, 1])); + ref = multiply(ref, translated([20, 0, 10])); + let refVec = transformPoint3d(ref, [0, 0, 10]); const a = processTransform3d([ { translateX: 10 }, { scale: 2 }, { translateX: 20 }, { translateZ: 10 }, ]); - expect(mapPoint3d(a, [0, 0, 10])).toEqual([50, 0, 20]); + expect(a).toEqual(ref); + expect(mapPoint3d(a, [0, 0, 10])).toEqual(refVec); + // 2. + ref = identity(); + ref = multiply(ref, translated([10, 20, 30])); + refVec = transformPoint3d(ref, [0, 0, 0]); const b = processTransform3d([ { translateX: 10 }, { translateY: 20 }, { translateZ: 30 }, ]); - expect(mapPoint3d(b, [0, 0, 0])).toEqual([10, 20, 30]); + expect(mapPoint3d(b, [0, 0, 0])).toEqual(refVec); + // 3. expect( concat4( concat4(b, [ diff --git a/src/__tests__/matrix.ts b/src/__tests__/matrix.ts new file mode 100644 index 0000000..e4e8006 --- /dev/null +++ b/src/__tests__/matrix.ts @@ -0,0 +1,164 @@ +/* eslint-disable no-shadow */ +/* eslint-disable @typescript-eslint/no-explicit-any */ +export const sdot = (...args: number[]): number => { + let acc = 0; + for (let i = 0; i < args.length - 1; i += 2) { + acc += args[i] * args[i + 1]; + } + return acc; +}; + +const identityN = (n: number): number[] => { + let size = n * n; + const m = new Array(size); + while (size--) { + m[size] = size % (n + 1) === 0 ? 1.0 : 0.0; + } + return m; +}; + +export const identity = () => identityN(4); + +function isnumber(val: number) { + return !isNaN(val); +} + +const m = (m1: number[], m2: number[]) => { + const size = 4; + if (!m1.every(isnumber) || !m2.every(isnumber)) { + throw "Some members of matrices are NaN m1=" + m1 + ", m2=" + m2 + ""; + } + if (m1.length !== m2.length) { + throw ( + "Undefined for matrices of different sizes. m1.length=" + + m1.length + + ", m2.length=" + + m2.length + ); + } + if (size * size !== m1.length) { + throw "Undefined for non-square matrices. array size was " + size; + } + + const result = Array(m1.length); + for (let r = 0; r < size; r++) { + for (let c = 0; c < size; c++) { + // accumulate a sum of m1[r,k]*m2[k, c] + let acc = 0; + for (let k = 0; k < size; k++) { + acc += m1[size * r + k] * m2[size * k + c]; + } + result[r * size + c] = acc; + } + } + return result; +}; + +export const multiply = (...listOfMatrices: any[]) => { + if (listOfMatrices.length < 2) { + throw "multiplication expected two or more matrices"; + } + let result = m(listOfMatrices[0], listOfMatrices[1]); + let next = 2; + while (next < listOfMatrices.length) { + result = m(result, listOfMatrices[next]); + next++; + } + return result; +}; + +export const stride = ( + v: number[], + m: number[], + width: number, + offset: number, + colStride: number +): number[] => { + for (let i = 0; i < v.length; i++) { + m[i * width + ((i * colStride + offset + width) % width)] = v[i]; + } + return m; +}; + +const dot = (a: number[], b: number[]): number => { + if (a.length !== b.length) { + throw new Error("Arrays must have the same length for dot product."); + } + return a.reduce((acc, val, i) => acc + val * b[i], 0); +}; + +const vectorLengthSquared = (v: number[]): number => dot(v, v); +const vectorLength = (v: number[]): number => Math.sqrt(vectorLengthSquared(v)); +const mulScalar = (v: number[], s: number): number[] => v.map((val) => val * s); +// const addVectors = (a: number[], b: number[]): number[] => +// a.map((val, i) => val + b[i]); +// const subVectors = (a: number[], b: number[]): number[] => +// a.map((val, i) => val - b[i]); +const normalize = (v: number[]): number[] => mulScalar(v, 1 / vectorLength(v)); + +// const cross = (a: number[], b: number[]): number[] => { +// if (a.length !== 3 || b.length !== 3) { +// throw new Error("Cross product is only defined for 3-dimensional vectors."); +// } +// return [ +// a[1] * b[2] - a[2] * b[1], +// a[2] * b[0] - a[0] * b[2], +// a[0] * b[1] - a[1] * b[0], +// ]; +// }; + +// Matrix operations +export const translated = (vec: number[]): number[] => + stride(vec, identityN(4), 4, 3, 0); + +export const scaled = (vec: number[]): number[] => + stride(vec, identityN(4), 4, 0, 1); + +export const rotated = (axisVec: number[], radians: number): number[] => { + const normalizedAxisVec = normalize(axisVec); + const sinRadians = Math.sin(radians); + const cosRadians = Math.cos(radians); + return rotatedUnitSinCos(normalizedAxisVec, sinRadians, cosRadians); +}; + +const rotatedUnitSinCos = ( + axisVec: number[], + sinAngle: number, + cosAngle: number +): number[] => { + const [x, y, z] = axisVec; + const c = cosAngle; + const s = sinAngle; + const t = 1 - c; + return [ + t * x * x + c, + t * x * y - s * z, + t * x * z + s * y, + 0, + t * x * y + s * z, + t * y * y + c, + t * y * z - s * x, + 0, + t * x * z - s * y, + t * y * z + s * x, + t * z * z + c, + 0, + 0, + 0, + 0, + 1, + ]; +}; + +export const transformPoint = (m4: number[], t: number[]): number[] => { + const x = m4[0] * t[0] + m4[4] * t[1] + m4[8] * t[2] + m4[12] * t[3]; + const y = m4[1] * t[0] + m4[5] * t[1] + m4[9] * t[2] + m4[13] * t[3]; + const z = m4[2] * t[0] + m4[6] * t[1] + m4[10] * t[2] + m4[14] * t[3]; + const w = m4[3] * t[0] + m4[7] * t[1] + m4[11] * t[2] + m4[15] * t[3]; + return [x, y, z, w]; +}; + +export const transformPoint3d = (m4: number[], t: number[]): number[] => { + const [x, y, z, w] = transformPoint(m4, [...t, 1]); + return [x / w, y / w, z / w]; +};