initial commit of some fun 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
+    // a2 and b2 are called half angles...
+    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));
+    // TODO: normalization wont save us when the vector is near zero, or when the angle is k2pi.... sanitize!
+    const dir = Vec3.normalize(D)
+    if (!Vec3.finite(dir)) {
+        return Vec3.finite(a.axis) ? a : identity
+    }
+    return { radians: gamma, axis: Vec3.normalize(D) }
+}
+
+
+/**
+ * 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,133 @@
+import { VectorLibFactory } from './vector'
+import { Vec3, type vec3 } from './vec3'
+import { type vec4 } from './vec4';
+// specifically, square matricies...
+type VectorConstraint = ReadonlyArray<number>;
+type ColumnConstraint<V extends VectorConstraint> = ReadonlyArray<V>
+
+// examples...
+export type mat4 = readonly [vec4, vec4, vec4, vec4]
+
+
+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
+    ))
+
+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 = (): Matrix => {
+        const mat: mMatrix = blank();
+        for (let i = 0; i < N; i++) {
+            mat[i][i] = 1;
+        }
+        return mat;
+    }
+    const translate = (v: Column): Matrix => {
+        const _mat: Matrix = identity();
+        const mat: mMatrix = [..._mat] as any;
+        mat[N - 1] = ([...v] as any)
+        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 normalize = (a: Matrix): Matrix => {
+        return map(a, (c) => lib.normalize(c))
+    }
+    const toColumnMajorArray = (m: mat4): number[] => m.flatMap(x => x)
+    return {
+        identity, mul, transpose, translate, transform, normalize, toColumnMajorArray
+    }
+
+}
+type Mat4Lib = ReturnType<typeof MatrixLib<4>>;
+export 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;
+    // this function is not lovely, and thats because rotation in 3D is weird.
+    // this is as nice as I can make it...
+    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];
+}
+
+export 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 lib = MatrixLib(4)
+
+export const Mat4 = { ...lib, rotateAboutAxis, rotateAboutPoint }

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

@@ -0,0 +1,179 @@
+import { describe, expect, test } from "vitest";
+import { Mat4, rotateAboutAxis } from "../matrix";
+import { Vec3, vec3 } from "../vec3"
+import { Vec4, type vec4 } from "../vec4";
+import { AxisAngle, composeRotation, rotateVector } from "../axisAngle";
+
+// TODO: delete the quaternion stuff
+// A versor is a (Unit) Quaternion, for representing a rotation in 3D
+export type Versor = Readonly<{
+    qi: number;
+    qj: number;
+    qk: number;
+    qr: number; // the scalar term
+}>
+
+export function versorFromAxisAngle(rotation: AxisAngle): Versor {
+    const sin = Math.sin(rotation.radians / 2)
+    const cos = Math.cos(rotation.radians / 2)
+    const e = Vec3.normalize(rotation.axis)
+    const [x, y, z] = e
+    return {
+        qi: x * sin,
+        qj: y * sin,
+        qk: z * sin,
+        qr: cos
+    }
+}
+export function axisAngleFromVersor(rotation: Versor): AxisAngle {
+    const { qi, qj, qk, qr } = rotation
+    const q: vec3 = [qi, qj, qk]
+    const D = Math.sqrt(Vec3.dot(q, q))
+    const theta = Math.atan2(D, qr)
+    const axis = Vec3.scale(q, 1 / D)
+    return { axis, radians: theta }
+}
+// rotate by q2 "after" q1
+// if you read q2 x q1, then you'd call compose(q2,q1)
+function compose(q2: Versor, q1: Versor): Versor {
+    const { qr: r2, qi: i2, qj: j2, qk: k2 } = q2
+    const { qr: r1, qi: i1, qj: j1, qk: k1 } = q1
+    // source: 
+    const r = (r2 * r1 - i2 * i1 - j2 * j1 - k2 * k1)
+    const i = (r2 * i1 + i2 * r1 + j2 * k1 - k2 * j1)
+    const j = (r2 * j1 - i2 * k1 + j2 * r1 + k2 * i1)
+    const k = (r2 * k1 + i2 * j1 - j2 * i1 + k2 * r1)
+
+    return {
+        qi: i,
+        qj: j,
+        qk: k,
+        qr: r
+    }
+}
+
+
+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 [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 = 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 = rotateAboutAxis(axis, angle * dir)
+                    const aa: AxisAngle = { axis, radians: dir * angle }
+                    // rotateVector v by each 
+                    // console.log('-------------------------------------')
+                    // console.log(`rotate (${v[0].toFixed(4)}, ${v[1].toFixed(4)}, ${v[2].toFixed(4)}) about`)
+                    // console.log(`<${axis[0].toFixed(4)}, ${axis[1].toFixed(4)}, ${axis[2].toFixed(4)}> by ${(angle * dir).toFixed(5)} `)
+                    const v4: vec4 = [...v, 0]
+                    const mResult = Mat4.transform(mat, v4)
+                    const aaResult = rotateVector(aa, v)
+                    nearly(Vec4.xyz(mResult), aaResult)
+                    v = aaResult
+                }
+            })
+        })
+    })
+
+})

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])
+    })
 });