71 lines
2.5 KiB
TypeScript
71 lines
2.5 KiB
TypeScript
import type { Vec3 } from '../types';
|
|
|
|
/** Tolerance used for geometric comparisons throughout the router (ft). */
|
|
export const EPS = 1e-6;
|
|
|
|
export const add = (a: Vec3, b: Vec3): Vec3 => [a[0] + b[0], a[1] + b[1], a[2] + b[2]];
|
|
export const sub = (a: Vec3, b: Vec3): Vec3 => [a[0] - b[0], a[1] - b[1], a[2] - b[2]];
|
|
export const scale = (a: Vec3, s: number): Vec3 => [a[0] * s, a[1] * s, a[2] * s];
|
|
export const dot = (a: Vec3, b: Vec3) => a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
|
|
export const cross = (a: Vec3, b: Vec3): Vec3 => [
|
|
a[1] * b[2] - a[2] * b[1],
|
|
a[2] * b[0] - a[0] * b[2],
|
|
a[0] * b[1] - a[1] * b[0],
|
|
];
|
|
export const length = (a: Vec3) => Math.hypot(a[0], a[1], a[2]);
|
|
export const dist = (a: Vec3, b: Vec3) => Math.hypot(a[0] - b[0], a[1] - b[1], a[2] - b[2]);
|
|
export const manhattan = (a: Vec3, b: Vec3) =>
|
|
Math.abs(a[0] - b[0]) + Math.abs(a[1] - b[1]) + Math.abs(a[2] - b[2]);
|
|
export const vecEquals = (a: Vec3, b: Vec3, eps = EPS) =>
|
|
Math.abs(a[0] - b[0]) < eps && Math.abs(a[1] - b[1]) < eps && Math.abs(a[2] - b[2]) < eps;
|
|
|
|
/**
|
|
* The six axis-aligned unit directions. Index layout: even = positive,
|
|
* odd = negative, so `idx >> 1` is the axis (0=X 1=Y 2=Z) and `idx ^ 1`
|
|
* is the reversed direction.
|
|
*/
|
|
export const AXIS_DIRS: readonly Vec3[] = [
|
|
[1, 0, 0],
|
|
[-1, 0, 0],
|
|
[0, 1, 0],
|
|
[0, -1, 0],
|
|
[0, 0, 1],
|
|
[0, 0, -1],
|
|
];
|
|
|
|
export const axisOfDir = (dirIdx: number) => dirIdx >> 1;
|
|
export const signOfDir = (dirIdx: number) => (dirIdx & 1 ? -1 : 1);
|
|
export const reverseDir = (dirIdx: number) => dirIdx ^ 1;
|
|
|
|
/** Index into AXIS_DIRS for an (exactly) axis-aligned unit vector, or -1. */
|
|
export function dirIndexOf(v: Vec3, eps = EPS): number {
|
|
for (let i = 0; i < 6; i++) {
|
|
const d = AXIS_DIRS[i];
|
|
if (Math.abs(v[0] - d[0]) < eps && Math.abs(v[1] - d[1]) < eps && Math.abs(v[2] - d[2]) < eps) {
|
|
return i;
|
|
}
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
/**
|
|
* Snap a roughly axis-aligned direction to the nearest exact axis direction.
|
|
* Returns null when the vector is degenerate or too far from any axis
|
|
* (more than ~25° off).
|
|
*/
|
|
export function snapAxisDir(v: Vec3): Vec3 | null {
|
|
const l = length(v);
|
|
if (l < EPS) return null;
|
|
const n: Vec3 = [v[0] / l, v[1] / l, v[2] / l];
|
|
let best = -1;
|
|
let bestDot = 0.9; // cos(~25°) — refuse wildly diagonal directions
|
|
for (let i = 0; i < 6; i++) {
|
|
const d = dot(n, AXIS_DIRS[i]);
|
|
if (d > bestDot) {
|
|
bestDot = d;
|
|
best = i;
|
|
}
|
|
}
|
|
return best >= 0 ? ([...AXIS_DIRS[best]] as Vec3) : null;
|
|
}
|