implement vector

src/matrix.ts

@@ -7,6 +7,11 @@ export type Mat3 = readonly [number, number, number, number, number, number, num
 // Like Mat3 but mutable.
 export type MutMat3 = Mutable<Mat3>
 
+// Creates a new empty Matrix.
+export function zero(): MutMat3 {
+	return [0, 0, 0, 0, 0, 0, 0, 0, 0]
+}
+
 // Creates a new Identity Matrix.
 export function identity(): MutMat3 {
 	return [1, 0, 0, 0, 1, 0, 0, 0, 1]
@@ -90,7 +95,7 @@ export function rotZ(angle: Angle, m?: MutMat3): MutMat3 {
 	}
 }
 
-// Creates a new mutable Matrix from matrix.
+// Creates a new mutable Matrix from the given matrix.
 export function clone(m: Mat3): MutMat3 {
 	return [...m]
 }

src/vector.test.ts

@@ -0,0 +1,83 @@
+import { expect, test } from 'bun:test'
+import { deg } from './angle'
+import { PI, PIOVERTWO } from './constants'
+import { angle, cross, div, divScalar, dot, minus, minusScalar, mul, mulScalar, negate, normalize, plane, plus, plusScalar, rotateByRodrigues, xAxis, yAxis, zAxis, type MutVec3, type Vec3 } from './vector'
+
+test('angle', () => {
+	expect(angle(xAxis(), yAxis())).toBe(PIOVERTWO)
+	expect(angle([1, 2, 3], [-1, -2, -3])).toBe(PI)
+	expect(angle([2, -3, 1], [4, -6, 2])).toBe(0)
+	expect(angle([3, 4, 5], [1, 2, 2])).toBeCloseTo(Math.acos(1.4 / Math.sqrt(2)), 14)
+})
+
+test('normalize', () => {
+	const a = Math.sqrt(14)
+	expect(normalize([3, 2, -1])).toEqual([3 / a, 2 / a, -1 / a])
+
+	const o: MutVec3 = [0, 0, 0]
+	expect(normalize(o)).toEqual([0, 0, 0])
+
+	normalize([3, 2, -1], o)
+	expect(o).not.toEqual(a)
+	expect(o).toEqual([3 / a, 2 / a, -1 / a])
+})
+
+test('plus', () => {
+	expect(plusScalar([2, 3, 2], 2)).toEqual([4, 5, 4])
+	expect(plus([2, 3, 2], [2, 3, 2])).toEqual([4, 6, 4])
+})
+
+test('minus', () => {
+	expect(minusScalar([2, 3, 2], 2)).toEqual([0, 1, 0])
+	expect(minus([2, 3, 2], [-2, -3, -2])).toEqual([4, 6, 4])
+})
+
+test('mul', () => {
+	expect(mulScalar([2, 3, 2], 2)).toEqual([4, 6, 4])
+	expect(mul([2, 3, 2], [2, 3, 2])).toEqual([4, 9, 4])
+})
+
+test('div', () => {
+	expect(divScalar([2, 3, 2], 2)).toEqual([1, 1.5, 1])
+	expect(div([2, 3, 2], [2, 3, 2])).toEqual([1, 1, 1])
+})
+
+test('dot', () => {
+	expect(dot([2, 3, 2], [2, 3, 2])).toBe(17)
+	expect(dot([2, 3, 2], negate([2, 3, 2]))).toBe(-17)
+})
+
+test('cross', () => {
+	expect(cross([2, 3, 2], [3, 2, 3])).toEqual([5, 0, -5])
+})
+
+test('rotateByRodrigues', () => {
+	const x = xAxis()
+	expect(rotateByRodrigues(x, x, PI)).toEqual(x)
+
+	const y = yAxis()
+	expect(rotateByRodrigues(y, y, PI)).toEqual(y)
+
+	const z = zAxis()
+	expect(rotateByRodrigues(z, z, PI)).toEqual(z)
+
+	const v: Vec3 = [1, 2, 3]
+	expect(rotateByRodrigues(v, x, PI / 4)).toEqual([1, -0.7071067811865472, 3.5355339059327378])
+	expect(rotateByRodrigues(v, y, PI / 4)).toEqual([2.82842712474619, 2, 1.4142135623730954])
+	expect(rotateByRodrigues(v, z, PI / 4)).toEqual([-0.7071067811865474, 2.121320343559643, 3])
+
+	const axis: Vec3 = [3, 4, 5]
+	expect(rotateByRodrigues(v, axis, 0)).toEqual(v)
+	expect(rotateByRodrigues(v, axis, deg(29.6512852))).toEqual([1.2132585570946925, 1.7306199385433279, 3.087548914908522])
+	expect(rotateByRodrigues(v, axis, deg(120.3053274))).toEqual([2.0867722943019413, 1.6319848922736107, 2.642348709599946])
+	expect(rotateByRodrigues(v, axis, deg(230.6512852))).toEqual([1.6963389417184784, 2.5681684228867847, 2.1276618966594847])
+	expect(rotateByRodrigues(v, axis, deg(359.6139797))).toEqual([0.9981071206640635, 2.0038129934994813, 2.9980853328019763])
+
+	const o: MutVec3 = [0, 0, 0]
+	rotateByRodrigues(v, axis, deg(29.6512852), o)
+	expect(o).toEqual([1.2132585570946925, 1.7306199385433279, 3.087548914908522])
+})
+
+test('plane', () => {
+	expect(plane([1, -2, 1], [4, -2, -2], [4, 1, 4])).toEqual([9, -18, 9])
+})

src/vector.ts

@@ -0,0 +1,162 @@
+import type { Mutable } from 'utility-types'
+import type { Angle } from './angle'
+import { PI } from './constants'
+
+// Vector of numbers with three axis.
+export type Vec3 = readonly [number, number, number]
+
+// Like Vec3 but mutable.
+export type MutVec3 = Mutable<Vec3>
+
+// Scalar product between the vectors a and b.
+export function dot(a: Vec3, b: Vec3) {
+	return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
+}
+
+// Fills the vector.
+export function fill(v: MutVec3, a: number, b: number, c: number): MutVec3 {
+	v[0] = a
+	v[1] = b
+	v[2] = c
+	return v
+}
+
+// Cross product between the vectors a and b.
+export function cross(a: Vec3, b: Vec3, o?: MutVec3): MutVec3 {
+	const c = a[1] * b[2] - a[2] * b[1]
+	const d = a[2] * b[0] - a[0] * b[2]
+	const e = a[0] * b[1] - a[1] * b[0]
+
+	if (o) return fill(o, c, d, e)
+	else return [c, d, e]
+}
+
+export function length(v: Vec3) {
+	return Math.sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2])
+}
+
+export function distance(a: Vec3, b: Vec3) {
+	const c = a[0] - b[0]
+	const d = a[1] - b[1]
+	const e = a[2] - b[2]
+	return Math.sqrt(c * c + d * d + e * e)
+}
+
+// Creates a new mutable vector from the given vector.
+export function clone(v: Vec3): MutVec3 {
+	return [...v]
+}
+
+// Computes the angle between the vectors a and b.
+export function angle(a: Vec3, b: Vec3): Angle {
+	// https://people.eecs.berkeley.edu/~wkahan/Mindless.pdf
+	// const c = mulScalar(a, length(b))
+	// const d = mulScalar(b, length(a))
+	// return 2 * Math.atan2(length(minus(c, d)), length(plus(c, d)))
+
+	const d = dot(a, b)
+	const v = d / (length(a) * length(b))
+	if (Math.abs(v) > 1.0)
+		if (v < 0.0) return PI
+		else return 0
+	else return Math.acos(v)
+}
+
+// Creates a new empty vector.
+export function zero(): MutVec3 {
+	return [0, 0, 0]
+}
+
+export function xAxis(): MutVec3 {
+	return [1, 0, 0]
+}
+
+export function yAxis(): MutVec3 {
+	return [0, 1, 0]
+}
+
+export function zAxis(): MutVec3 {
+	return [0, 0, 1]
+}
+
+export function latitude(v: Vec3) {
+	return Math.acos(v[2])
+}
+
+export function longitude(v: Vec3) {
+	return Math.atan2(v[1], v[0])
+}
+
+// Negates the vector.
+export function negate(a: Vec3, o?: MutVec3): MutVec3 {
+	if (o) return fill(o, -a[0], -a[1], -a[2])
+	else return [-a[0], -a[1], -a[2]]
+}
+
+export function plusScalar(a: Vec3, scalar: number, o?: MutVec3): MutVec3 {
+	if (o) return fill(o, a[0] + scalar, a[1] + scalar, a[2] + scalar)
+	else return [a[0] + scalar, a[1] + scalar, a[2] + scalar]
+}
+
+export function minusScalar(a: Vec3, scalar: number, o?: MutVec3): MutVec3 {
+	if (o) return fill(o, a[0] - scalar, a[1] - scalar, a[2] - scalar)
+	else return [a[0] - scalar, a[1] - scalar, a[2] - scalar]
+}
+
+export function mulScalar(a: Vec3, scalar: number, o?: MutVec3): MutVec3 {
+	if (o) return fill(o, a[0] * scalar, a[1] * scalar, a[2] * scalar)
+	else return [a[0] * scalar, a[1] * scalar, a[2] * scalar]
+}
+
+export function divScalar(a: Vec3, scalar: number, o?: MutVec3): MutVec3 {
+	if (o) return fill(o, a[0] / scalar, a[1] / scalar, a[2] / scalar)
+	else return [a[0] / scalar, a[1] / scalar, a[2] / scalar]
+}
+
+export function plus(a: Vec3, b: Vec3, o?: MutVec3): MutVec3 {
+	if (o) return fill(o, a[0] + b[0], a[1] + b[1], a[2] + b[2])
+	else return [a[0] + b[0], a[1] + b[1], a[2] + b[2]]
+}
+
+export function minus(a: Vec3, b: Vec3, o?: MutVec3): MutVec3 {
+	if (o) return fill(o, a[0] - b[0], a[1] - b[1], a[2] - b[2])
+	else return [a[0] - b[0], a[1] - b[1], a[2] - b[2]]
+}
+
+export function mul(a: Vec3, b: Vec3, o?: MutVec3): MutVec3 {
+	if (o) return fill(o, a[0] * b[0], a[1] * b[1], a[2] * b[2])
+	else return [a[0] * b[0], a[1] * b[1], a[2] * b[2]]
+}
+
+export function div(a: Vec3, b: Vec3, o?: MutVec3): MutVec3 {
+	if (o) return fill(o, a[0] / b[0], a[1] / b[1], a[2] / b[2])
+	else return [a[0] / b[0], a[1] / b[1], a[2] / b[2]]
+}
+
+// Normalizes the vector.
+export function normalize(v: Vec3, o?: MutVec3): MutVec3 {
+	const len = length(v)
+
+	if (len === 0)
+		if (o) return fill(o, ...v)
+		else return clone(v)
+	else return divScalar(v, len, o)
+}
+
+// Efficient algorithm for rotating a vector in space, given an axis and angle of rotation.
+export function rotateByRodrigues(v: Vec3, axis: Vec3, angle: Angle, o?: MutVec3): Vec3 {
+	const cosa = Math.cos(angle)
+	const b: MutVec3 = [0, 0, 0]
+	const c: MutVec3 = [0, 0, 0]
+	const k = normalize(axis, o)
+	mulScalar(cross(k, v, b), Math.sin(angle), b)
+	mulScalar(k, dot(k, v), c)
+	plus(mulScalar(v, cosa, k), b, b)
+	return plus(b, mulScalar(c, 1.0 - cosa, c), o)
+}
+
+export function plane(a: Vec3, b: Vec3, c: Vec3, o?: MutVec3): MutVec3 {
+	const d = minus(b, a, o)
+	const e = minus(c, b)
+	return cross(d, e, o)
+}