Basic utilities for 3D math

packages/geometry/src/axisAngle.ts

@@ -0,0 +1,60 @@
+import { Vec3, vec3 } from "./vec3";
+
+
+export type AxisAngle = {
+    readonly axis: vec3;
+    readonly radians: number;
+}
+
+const identity: AxisAngle = {
+    radians: 0,
+    axis: [1, 0, 0]
+}
+/**
+ * Note the ordering of arguments here - we follow the math convention of having the outer rotation left of the inner rotation "b (x) a"
+ * when we say b (x) a, we mean that b happens "after" a, this is important because b (x) a =/= a (x) b
+ * this is the opposite order of how programmers often write "reducer"-style functions eg. "fn(old_thing:X, next_event:A)=>X"
+ * @param b the second rotation, in axis-angle form
+ * @param a the first rotation, in axis-angle form
+ * @returns a single rotation which is equivalent to the sequence of rotations "a then b"
+ */
+export function composeRotation(b: AxisAngle, a: AxisAngle): AxisAngle {
+    const a2 = a.radians / 2
+    const b2 = b.radians / 2
+    const sinA = Math.sin(a2)
+    const cosA = Math.cos(a2)
+    const sinB = Math.sin(b2)
+    const cosB = Math.cos(b2)
+    const A = a.axis
+    const B = b.axis
+    const gamma = 2 * Math.acos(cosB * cosA - (Vec3.dot(B, A) * sinB * sinA))
+
+    const D = Vec3.add(
+        Vec3.add(Vec3.scale(B, sinB * cosA),
+            Vec3.scale(A, sinA * cosB)),
+        Vec3.scale(Vec3.cross(B, A), sinB * sinA));
+
+    const dir = Vec3.normalize(D)
+    if (!Vec3.finite(dir)) {
+        return Vec3.finite(a.axis) ? a : identity
+    }
+    return { radians: gamma, axis: dir }
+}
+
+
+/**
+ * rotate a vector about a given axis (through the origin) by the given angle
+ * @param rotation the parameters of the rotation, in axis-angle form, also known as Euler-vector (NOT to be confused with Euler-angles!)
+ * @param v a 3D euclidean vector
+ * @returns the vector v after being rotated
+ */
+export function rotateVector(rotation: AxisAngle, v: vec3): vec3 {
+    // via rodrigues formula from the ancient past
+    // var names from https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula
+    const s = Math.sin(rotation.radians)
+    const c = Math.cos(rotation.radians)
+    const k = Vec3.normalize(rotation.axis)
+
+    return Vec3.add(Vec3.add(Vec3.scale(v, c), Vec3.scale(Vec3.cross(k, v), s)),
+        Vec3.scale(k, Vec3.dot(k, v) * (1 - c)))
+}

packages/geometry/src/matrix.ts

@@ -0,0 +1,146 @@
+import { VectorLibFactory } from './vector'
+import { Vec3, type vec3 } from './vec3'
+import { type vec4 } from './vec4';
+
+
+export type mat4 = readonly [vec4, vec4, vec4, vec4]
+
+
+// these vec types are for internal use only - just trickery to help get TS to treat
+// a literal number as a length in a generic-but-static-ish manner - as you can see,
+// some work might be needed to extend this past a 4x4 matrix
+type _Vec<N extends number, E> = N extends 2 ? [E, E] :
+    (N extends 3 ? [E, E, E] : (
+        N extends 4 ? [E, E, E, E] : never
+    ))
+type Vec<N extends number, E> = N extends 2 ? readonly [E, E] :
+    (N extends 3 ? readonly [E, E, E] : (
+        N extends 4 ? readonly [E, E, E, E] : never
+    ))
+
+// a template for generating utils for square matricies
+// note that you'll see a few 'as any' in the implementation here -
+// I've been having trouble getting TS to use a literal number as a template that relates to the length of tuples
+// all such cases are very short, and easy to verify as not-actually-risky
+
+function MatrixLib<Dim extends 2 | 3 | 4>(N: Dim) {
+    type mColumn = _Vec<Dim, number>
+    type mMatrix = _Vec<Dim, mColumn>
+    // the mutable types are helpful for all the internal junk we've got to do in here
+    type Column = Vec<Dim, number>
+    type Matrix = Vec<Dim, Column>
+
+    const lib = VectorLibFactory<Column>();
+
+    const fill = <T>(t: T): _Vec<Dim, T> => {
+        const arr = new Array<T>(N);
+        return arr.fill(t) as any
+        // yup, typescript is lost here - thats ok, this function is very short and you can see its
+        // making an array of a specific length, just as we expect
+    }
+    const map = <T>(vec: Vec<Dim, T>, fn: (t: T, i: number) => T): Vec<Dim, T> => {
+        return vec.map(fn) as any; // sorry TS - you tried. we can see this is fine though
+    }
+    const zeros: () => mColumn = () => fill(0);
+    const blank: () => mMatrix = () => {
+        const z = zeros();
+        const arr = new Array<mColumn>(N);
+        for (let c = 0; c < N; c++) {
+            arr[c] = [...z]
+        }
+        return arr as any;
+    }
+    const _identity = (): mMatrix => {
+        const mat: mMatrix = blank();
+        for (let i = 0; i < N; i++) {
+            mat[i][i] = 1;
+        }
+        return mat;
+    }
+    const identity = (): Matrix => {
+        return _identity();
+    }
+    const translate = (v: Column): Matrix => {
+        const mat: mMatrix = _identity();
+        mat[N - 1] = v as mColumn;
+        return mat;
+    }
+    const transpose = (m: Matrix): Matrix => {
+        const mat = blank();
+        for (let i = 0; i < N; i++) {
+            for (let j = 0; j < N; j++) {
+                mat[j][i] = m[i][j]
+            }
+        }
+        return mat;
+    }
+    const mul = (a: Matrix, b: Matrix): Matrix => {
+        const B = transpose(b);
+        return map(a, (col: Column) => map(col, (_, r) => lib.dot(col, B[r])))
+    }
+    const transform = (a: Matrix, v: Column): Column => {
+        const T = transpose(a)
+        return map(v, (_, i) => lib.dot(v, T[i]))
+    }
+
+    const toColumnMajorArray = (m: mat4): number[] => m.flatMap(x => x)
+    return {
+        identity, mul, transpose, translate, transform, toColumnMajorArray
+    }
+
+}
+type Mat4Lib = ReturnType<typeof MatrixLib<4>>;
+
+/**
+ * @param axis 
+ * @param radians 
+ * @returns a 4x4 matrix, expressing a rotation about the @param axis through the origin
+ */
+function rotateAboutAxis(axis: vec3, radians: number): mat4 {
+    const sin = Math.sin(radians);
+    const cos = Math.cos(radians);
+    const icos = 1 - cos;
+    const [x, y, z] = axis;
+    const xx = x * x;
+    const xy = x * y;
+    const xz = x * z;
+    const yz = y * z;
+    const yy = y * y;
+    const zz = z * z;
+
+    // todo: there is a pattern here - perhaps someone clever could
+    // express it in a more concise way
+    const X: vec4 = [
+        xx * icos + cos,
+        xy * icos + (z * sin),
+        xz * icos - (y * sin),
+        0];
+
+    const Y: vec4 = [
+        xy * icos - (z * sin),
+        yy * icos + cos,
+        yz * icos + (x * sin),
+        0
+    ]
+    const Z: vec4 = [
+        xz * icos + (y * sin),
+        yz * icos - (x * sin),
+        zz * icos + cos,
+        0
+    ]
+    const W: vec4 = [0, 0, 0, 1];
+    return [X, Y, Z, W];
+}
+function rotateAboutPoint(lib: Mat4Lib, axis: vec3, radians: number, point: vec3): mat4 {
+    const mul = lib.mul;
+    const back = lib.translate([...point, 1])
+    const rot = rotateAboutAxis(axis, radians);
+    const move = lib.translate([...Vec3.scale(point, -1), 1])
+
+    return mul(mul(move, rot), back);
+
+}
+const moverotmove = (lib: Mat4Lib) => (axis: vec3, radians: number, point: vec3) => rotateAboutPoint(lib, axis, radians, point)
+const lib = MatrixLib(4)
+
+export const Mat4 = { ...lib, rotateAboutAxis, rotateAboutPoint: moverotmove(lib) }

packages/geometry/src/tests/rotation.test.ts

@@ -0,0 +1,144 @@
+import { describe, expect, test } from "vitest";
+import { Mat4 } from "../matrix";
+import { Vec3, vec3 } from "../vec3"
+import { Vec4, type vec4 } from "../vec4";
+import { AxisAngle, composeRotation, rotateVector } from "../axisAngle";
+
+function nearly(actual: vec3, expected: vec3) {
+    const dst = Vec3.length(Vec3.sub(actual, expected))
+    if (dst > 0.0001) {
+        console.log('expected', expected, 'recieved: ', actual)
+    }
+    for (let i = 0; i < 3; i++) {
+        expect(actual[i]).toBeCloseTo(expected[i])
+    }
+}
+
+describe('rotation in various ways', () => {
+    describe('axis angle...', () => {
+        test('basics', () => {
+            const rot: AxisAngle = {
+                radians: Math.PI / 2, // 90 degrees
+                axis: [0, 0, 1]
+            }
+            const v: vec3 = [1, 0, 0]
+            nearly(rotateVector(rot, v), [0, 1, 0])
+            // 90+90+90
+            const twoSeventy = composeRotation(rot, composeRotation(rot, rot))
+            nearly(rotateVector(twoSeventy, v), [0, -1, 0])
+        })
+        test('non-axis aligned...', () => {
+            const thirty: AxisAngle = {
+                axis: Vec3.normalize([1, 1, 0]),
+                radians: Math.PI / 6
+            }
+            const v: vec3 = [-1, 1, 0]
+            const ninty = composeRotation(thirty, composeRotation(thirty, thirty))
+            nearly(ninty.axis, Vec3.normalize([1, 1, 0]))
+            expect(ninty.radians).toBeCloseTo(Math.PI / 2)
+            nearly(rotateVector(ninty, v), [0, 0, Vec3.length(v)])
+        })
+        test('degenerate radians', () => {
+            const nada: AxisAngle = {
+                axis: Vec3.normalize([1, 1, 0]),
+                radians: 0,
+            }
+            const v: vec3 = [-1, 1, 0]
+            const r = composeRotation(nada, nada)
+            nearly(rotateVector(r, v), v)
+
+        })
+        test('degenerate axis', () => {
+            const nada: AxisAngle = {
+                axis: Vec3.normalize([0, 0, 0]),
+                radians: Math.PI / 4,
+            }
+            const fine: AxisAngle = {
+                axis: Vec3.normalize([1, 0, 0]),
+                radians: Math.PI / 4,
+            }
+            const v: vec3 = [-1, 1, 0]
+            const r = composeRotation(nada, nada)
+            nearly(rotateVector(r, v), v)
+            const r2 = composeRotation(nada, fine)
+            nearly(rotateVector(r2, v), rotateVector(fine, v))
+
+        })
+        test('error does not accumulate at this scale', () => {
+            const steps = 10000; // divide a rotation into ten thousand little steps, then compose each to re-build a 180-degree rotation
+            const little: AxisAngle = {
+                axis: Vec3.normalize([1, 1, 1]),
+                radians: Math.PI / steps
+            }
+            const expectedRotation: AxisAngle = {
+                axis: Vec3.normalize([1, 1, 1]),
+                radians: Math.PI
+            }
+            const v: vec3 = [-22, 33, 2]
+            let total = little;
+            for (let i = 1; i < steps; i++) {
+                total = composeRotation(little, total)
+            }
+            nearly(rotateVector(total, v), rotateVector(expectedRotation, v))
+            nearly(rotateVector(composeRotation(total, total), v), v)
+        })
+    })
+    describe('matrix works the same', () => {
+        const randomAxis = (): vec3 => {
+            const theta = Math.PI * 100 * Math.random()
+            const sigma = Math.PI * 100 * Math.random()
+            const x = Math.cos(theta) * Math.sin(sigma)
+            const y = Math.sin(theta) * Math.sin(sigma)
+            const z = Math.cos(sigma);
+            // always has length 1... I think?
+            return Vec3.normalize([x, y, z])
+        }
+        test('rotateAboutAxis and axis angle agree (right hand rule... right?)', () => {
+            const axis: vec3 = Vec3.normalize([0.5904, -0.6193, -0.5175])
+            expect(Vec3.length(axis)).toBeCloseTo(1, 8)
+            const v: vec3 = [0.4998, 0.0530, 0.8645]
+            expect(Vec3.length(v)).toBeCloseTo(1, 3)
+            const angle = -Math.PI / 4
+            const mat = Mat4.rotateAboutAxis(axis, angle)
+            const aa: AxisAngle = { axis, radians: angle }
+            const a = rotateVector(aa, v)
+            const b = Vec4.xyz(Mat4.transform(mat, [...v, 0]))
+            nearly(b, a)
+        })
+        test('repeated rotations about random axes match the equivalent matrix rotateVector...', () => {
+            let v: vec3 = [1, 0, 0]
+            for (let i = 0; i < 300; i++) {
+                const axis = randomAxis()
+                expect(Vec3.length(axis)).toBeCloseTo(1)
+                const angle = (Math.PI / 360) + (Math.random() * Math.PI)
+                const dir = Math.random() > 0.5 ? -1 : 1
+                const mat = Mat4.rotateAboutAxis(axis, angle * dir)
+                const aa: AxisAngle = { axis, radians: dir * angle }
+                // rotateVector v by each 
+                const v4: vec4 = [...v, 0]
+                const mResult = Mat4.transform(mat, v4)
+                const aaResult = rotateVector(aa, v)
+                nearly(Vec4.xyz(mResult), aaResult)
+                v = aaResult
+            }
+        })
+    })
+    describe('rotation about a point which is not the origin', () => {
+        test('an easy to understand example', () => {
+            let v: vec4 = [1, 0, 0, 1]
+            let yAxis: vec3 = [0, 1, 0]
+            let origin: vec3 = [2, 0, 0]
+
+            // o----v---|---x
+            // 0----1---2---3---4---
+
+            // o = the true origin
+            // v = the point v
+            // | = the y-axis through the point [2,0,0]
+            // x = imagine rotating v around the | by 180, you get x
+            const rot = Mat4.rotateAboutPoint(yAxis, Math.PI, origin)
+            nearly(Vec4.xyz(Mat4.transform(rot, v)), [3, 0, 0])
+        })
+    })
+
+})

packages/geometry/src/tests/vec3.test.ts

@@ -128,4 +128,13 @@ describe('Vec3', () => {
         expect(Vec3.isVec3([1, 2, 2, 3])).toBeFalsy();
         expect(Vec3.isVec3([1, 2])).toBeFalsy();
     });
+    test('cross, follows right-hand rule for easy-to-follow cases', () => {
+        expect(Vec3.cross([1, 0, 0], [0, 1, 0])).toEqual([0, 0, 1])
+        expect(Vec3.cross([1, 0, 0], [0, 0, 1])).toEqual([0, -1, 0])
+        expect(Vec3.cross([0, 1, 0], [0, 0, 1])).toEqual([1, 0, 0])
+    })
+    test('cross for degenerate cases', () => {
+        expect(Vec3.cross([1, 0, 0], [1, 0, 0])).toEqual([0, 0, 0])
+        expect(Vec3.cross([1, 0, 0], [-1, 0, 0])).toEqual([0, -0, 0])
+    })
 });

packages/geometry/src/vec3.ts

@@ -4,4 +4,10 @@ export type vec3 = readonly [number, number, number];
 // add things that vec3 has that vec2 does not have!
 const xy = (xyz: vec3) => [xyz[0], xyz[1]] as readonly [number, number];
 const isVec3 = (v: ReadonlyArray<number>): v is vec3 => v.length === 3;
-export const Vec3 = { ...VectorLibFactory<vec3>(), xy, isVec3 };
+const cross = (a: vec3, b: vec3): vec3 => {
+    const x = (a[1] * b[2]) - (a[2] * b[1])
+    const y = (a[2] * b[0]) - (a[0] * b[2])
+    const z = (a[0] * b[1]) - (a[1] * b[0])
+    return [x, y, z]
+}
+export const Vec3 = { ...VectorLibFactory<vec3>(), xy, isVec3, cross };

packages/geometry/src/vec4.ts

@@ -1,5 +1,6 @@
+import type { vec3 } from './vec3';
 import { VectorLibFactory } from './vector';
 
 export type vec4 = readonly [number, number, number, number];
-
-export const Vec4 = VectorLibFactory<vec4>();
+const xyz = (v: vec4): vec3 => [v[0], v[1], v[2]]
+export const Vec4 = { ...VectorLibFactory<vec4>(), xyz }