764 lines
27 KiB
TypeScript
764 lines
27 KiB
TypeScript
import type { Point2D, ConicCoeffs, ConicType, ConicArc, RationalQuadBezier, IntersectionPoint } from "./types";
|
||
|
||
const EPS = 1e-10;
|
||
|
||
// ────────────────────────────────────────────────────────
|
||
// Conic fitting via null space of design matrix
|
||
// ────────────────────────────────────────────────────────
|
||
|
||
/**
|
||
* Fit a general conic Ax²+Bxy+Cy²+Dx+Ey+F=0 through a set of points.
|
||
* Uses SVD-like approach: find the eigenvector of M^T*M with smallest eigenvalue.
|
||
* Points are normalized to [-1,1] before fitting for numerical stability.
|
||
*/
|
||
export function fitConic(points: Point2D[]): ConicCoeffs {
|
||
if (points.length < 5) {
|
||
return { A: 0, B: 0, C: 0, D: 0, E: 0, F: 1 }; // degenerate
|
||
}
|
||
|
||
// Normalize coordinates to [-1,1]
|
||
let mx = 0, my = 0;
|
||
for (const p of points) { mx += p.x; my += p.y; }
|
||
mx /= points.length; my /= points.length;
|
||
let sx = 0, sy = 0;
|
||
for (const p of points) {
|
||
sx = Math.max(sx, Math.abs(p.x - mx));
|
||
sy = Math.max(sy, Math.abs(p.y - my));
|
||
}
|
||
sx = sx || 1; sy = sy || 1;
|
||
|
||
// Build design matrix D (n x 6): each row = [x², xy, y², x, y, 1]
|
||
const n = points.length;
|
||
const D: number[][] = [];
|
||
for (const p of points) {
|
||
const x = (p.x - mx) / sx;
|
||
const y = (p.y - my) / sy;
|
||
D.push([x * x, x * y, y * y, x, y, 1]);
|
||
}
|
||
|
||
// Compute M = D^T * D (6x6)
|
||
const M = Array.from({ length: 6 }, () => new Float64Array(6));
|
||
for (let i = 0; i < 6; i++) {
|
||
for (let j = i; j < 6; j++) {
|
||
let sum = 0;
|
||
for (let k = 0; k < n; k++) sum += D[k][i] * D[k][j];
|
||
M[i][j] = sum;
|
||
M[j][i] = sum;
|
||
}
|
||
}
|
||
|
||
// Find eigenvector with smallest eigenvalue via inverse power iteration
|
||
const coeffs = smallestEigenvector6(M);
|
||
|
||
// Denormalize: transform coefficients back to original coordinate space
|
||
// Substituting x_norm = (x - mx)/sx, y_norm = (y - my)/sy into the conic equation
|
||
const [a, b, c, d, e, f] = coeffs;
|
||
const A = a / (sx * sx);
|
||
const B = b / (sx * sy);
|
||
const C = c / (sy * sy);
|
||
const D2 = d / sx - 2 * a * mx / (sx * sx) - b * my / (sx * sy);
|
||
const E2 = e / sy - 2 * c * my / (sy * sy) - b * mx / (sx * sy);
|
||
const F2 = a * mx * mx / (sx * sx) + b * mx * my / (sx * sy) + c * my * my / (sy * sy)
|
||
- d * mx / sx - e * my / sy + f;
|
||
|
||
return { A, B, C, D: D2, E: E2, F: F2 };
|
||
}
|
||
|
||
/**
|
||
* Find the eigenvector corresponding to the smallest eigenvalue of a 6x6 symmetric matrix.
|
||
* Uses inverse power iteration with a small shift.
|
||
*/
|
||
function smallestEigenvector6(M: Float64Array[]): number[] {
|
||
// Estimate smallest eigenvalue with Gershgorin circle theorem lower bound
|
||
let minEig = Infinity;
|
||
for (let i = 0; i < 6; i++) {
|
||
let r = 0;
|
||
for (let j = 0; j < 6; j++) if (i !== j) r += Math.abs(M[i][j]);
|
||
minEig = Math.min(minEig, M[i][i] - r);
|
||
}
|
||
const shift = minEig - 0.001;
|
||
|
||
// M_shifted = M - shift*I
|
||
const Ms = Array.from({ length: 6 }, (_, i) => {
|
||
const row = new Float64Array(6);
|
||
for (let j = 0; j < 6; j++) row[j] = M[i][j];
|
||
row[i] -= shift;
|
||
return row;
|
||
});
|
||
|
||
// LU decomposition of Ms
|
||
const LU = Ms.map((r) => Float64Array.from(r));
|
||
const piv = Array.from({ length: 6 }, (_, i) => i);
|
||
for (let k = 0; k < 6; k++) {
|
||
let maxVal = 0, maxRow = k;
|
||
for (let i = k; i < 6; i++) {
|
||
if (Math.abs(LU[i][k]) > maxVal) { maxVal = Math.abs(LU[i][k]); maxRow = i; }
|
||
}
|
||
if (maxRow !== k) {
|
||
[LU[k], LU[maxRow]] = [LU[maxRow], LU[k]];
|
||
[piv[k], piv[maxRow]] = [piv[maxRow], piv[k]];
|
||
}
|
||
if (Math.abs(LU[k][k]) < 1e-15) LU[k][k] = 1e-15;
|
||
for (let i = k + 1; i < 6; i++) {
|
||
LU[i][k] /= LU[k][k];
|
||
for (let j = k + 1; j < 6; j++) LU[i][j] -= LU[i][k] * LU[k][j];
|
||
}
|
||
}
|
||
|
||
function solveLU(b: number[]): number[] {
|
||
const pb = piv.map((i) => b[i]);
|
||
// Forward substitution
|
||
for (let i = 1; i < 6; i++) {
|
||
for (let j = 0; j < i; j++) pb[i] -= LU[i][j] * pb[j];
|
||
}
|
||
// Back substitution
|
||
for (let i = 5; i >= 0; i--) {
|
||
for (let j = i + 1; j < 6; j++) pb[i] -= LU[i][j] * pb[j];
|
||
pb[i] /= LU[i][i];
|
||
}
|
||
return pb;
|
||
}
|
||
|
||
// Power iteration on (M - shift*I)^{-1}
|
||
let v = [1, 1, 1, 1, 1, 1];
|
||
for (let iter = 0; iter < 30; iter++) {
|
||
v = solveLU(v);
|
||
let norm = 0;
|
||
for (const x of v) norm += x * x;
|
||
norm = Math.sqrt(norm) || 1;
|
||
for (let i = 0; i < 6; i++) v[i] /= norm;
|
||
}
|
||
return v;
|
||
}
|
||
|
||
// ────────────────────────────────────────────────────────
|
||
// Conic classification
|
||
// ────────────────────────────────────────────────────────
|
||
|
||
export function classifyConic(c: ConicCoeffs): { type: ConicType; eccentricity: number } {
|
||
const delta = c.B * c.B - 4 * c.A * c.C;
|
||
|
||
// 3x3 determinant for degeneracy check
|
||
const det3 = c.A * (c.C * c.F - c.E * c.E / 4)
|
||
- c.B / 2 * (c.B * c.F / 2 - c.E * c.D / 4)
|
||
+ c.D / 2 * (c.B * c.E / 4 - c.C * c.D / 2);
|
||
|
||
if (Math.abs(det3) < EPS) return { type: "degenerate", eccentricity: 0 };
|
||
|
||
if (Math.abs(delta) < EPS) {
|
||
return { type: "parabola", eccentricity: 1 };
|
||
}
|
||
|
||
if (delta < 0) {
|
||
// Ellipse or circle
|
||
if (Math.abs(c.A - c.C) < EPS && Math.abs(c.B) < EPS) {
|
||
return { type: "circle", eccentricity: 0 };
|
||
}
|
||
// Eccentricity from eigenvalues of [[A, B/2], [B/2, C]]
|
||
const tr = c.A + c.C;
|
||
const det = c.A * c.C - (c.B / 2) ** 2;
|
||
const disc = Math.sqrt(Math.max(0, tr * tr - 4 * det));
|
||
const lam1 = (tr + disc) / 2;
|
||
const lam2 = (tr - disc) / 2;
|
||
const ratio = Math.min(Math.abs(lam1), Math.abs(lam2)) / Math.max(Math.abs(lam1), Math.abs(lam2));
|
||
const e = Math.sqrt(Math.max(0, 1 - ratio));
|
||
return { type: "ellipse", eccentricity: e };
|
||
}
|
||
|
||
// Hyperbola
|
||
const tr = c.A + c.C;
|
||
const det = c.A * c.C - (c.B / 2) ** 2;
|
||
const disc = Math.sqrt(Math.max(0, tr * tr - 4 * det));
|
||
const lam1 = (tr + disc) / 2;
|
||
const lam2 = (tr - disc) / 2;
|
||
const absRatio = Math.abs(lam1 / (lam2 || 1));
|
||
const e = Math.sqrt(1 + absRatio);
|
||
return { type: "hyperbola", eccentricity: Math.min(e, 10) }; // cap for display
|
||
}
|
||
|
||
// ────────────────────────────────────────────────────────
|
||
// Route segmentation
|
||
// ────────────────────────────────────────────────────────
|
||
|
||
/**
|
||
* Split route points into overlapping segments for conic fitting.
|
||
* Each segment has maxPerArc points with 2-point overlap.
|
||
*/
|
||
export function segmentRoute(points: Point2D[], maxPerArc = 10): Point2D[][] {
|
||
if (points.length <= maxPerArc) return [points];
|
||
|
||
const segments: Point2D[][] = [];
|
||
const step = maxPerArc - 2; // overlap of 2
|
||
for (let i = 0; i < points.length - 4; i += step) {
|
||
const end = Math.min(i + maxPerArc, points.length);
|
||
segments.push(points.slice(i, end));
|
||
if (end >= points.length) break;
|
||
}
|
||
return segments;
|
||
}
|
||
|
||
// ────────────────────────────────────────────────────────
|
||
// Conic arc → Bézier conversion for SVG rendering
|
||
// ────────────────────────────────────────────────────────
|
||
|
||
/**
|
||
* Convert a conic arc (defined by its fitted points) into piecewise rational quadratic Béziers.
|
||
* For each consecutive pair of points, computes a Bézier arc using the conic gradient for tangents.
|
||
*/
|
||
export function conicArcToBeziers(coeffs: ConicCoeffs, points: Point2D[]): RationalQuadBezier[] {
|
||
if (points.length < 2) return [];
|
||
|
||
const beziers: RationalQuadBezier[] = [];
|
||
for (let i = 0; i < points.length - 1; i++) {
|
||
const p0 = points[i];
|
||
const p2 = points[i + 1];
|
||
|
||
// Tangent at each point: gradient of Ax²+Bxy+Cy²+Dx+Ey+F
|
||
// ∇F = (2Ax+By+D, Bx+2Cy+E), tangent is perpendicular to gradient
|
||
const g0x = 2 * coeffs.A * p0.x + coeffs.B * p0.y + coeffs.D;
|
||
const g0y = coeffs.B * p0.x + 2 * coeffs.C * p0.y + coeffs.E;
|
||
const g2x = 2 * coeffs.A * p2.x + coeffs.B * p2.y + coeffs.D;
|
||
const g2y = coeffs.B * p2.x + 2 * coeffs.C * p2.y + coeffs.E;
|
||
|
||
// Tangent = perpendicular to gradient: (-gy, gx)
|
||
const t0x = -g0y, t0y = g0x;
|
||
const t2x = -g2y, t2y = g2x;
|
||
|
||
// Control point = intersection of tangent lines from p0 and p2
|
||
const p1 = intersectLines(p0.x, p0.y, t0x, t0y, p2.x, p2.y, t2x, t2y);
|
||
|
||
if (p1) {
|
||
// Weight from the conic — use the midpoint deviation
|
||
const mid = { x: (p0.x + p2.x) / 2, y: (p0.y + p2.y) / 2 };
|
||
const d0 = Math.hypot(p1.x - mid.x, p1.y - mid.y);
|
||
const d1 = Math.hypot(p2.x - p0.x, p2.y - p0.y);
|
||
const w = d1 > EPS ? Math.min(Math.max(d1 / (2 * d0 + d1), 0.1), 5) : 1;
|
||
|
||
beziers.push({ p0, p1, p2, w });
|
||
} else {
|
||
// Tangent lines are parallel → straight segment
|
||
beziers.push({
|
||
p0, p2,
|
||
p1: { x: (p0.x + p2.x) / 2, y: (p0.y + p2.y) / 2 },
|
||
w: 1,
|
||
});
|
||
}
|
||
}
|
||
return beziers;
|
||
}
|
||
|
||
/** Intersect two lines: (px, py) + t*(dx, dy) and (qx, qy) + s*(ex, ey) */
|
||
function intersectLines(
|
||
px: number, py: number, dx: number, dy: number,
|
||
qx: number, qy: number, ex: number, ey: number,
|
||
): Point2D | null {
|
||
const denom = dx * ey - dy * ex;
|
||
if (Math.abs(denom) < EPS) return null;
|
||
const t = ((qx - px) * ey - (qy - py) * ex) / denom;
|
||
return { x: px + t * dx, y: py + t * dy };
|
||
}
|
||
|
||
// ────────────────────────────────────────────────────────
|
||
// Conic intersection via Sylvester resultant
|
||
// ────────────────────────────────────────────────────────
|
||
|
||
/**
|
||
* Find intersection points of two general conics.
|
||
* Uses the resultant method: eliminate x to get a degree-4 polynomial in y,
|
||
* then solve the quartic via companion matrix eigenvalues.
|
||
*/
|
||
export function findIntersections(c1: ConicCoeffs, c2: ConicCoeffs): Point2D[] {
|
||
// Each conic: A*x² + (B*y + D)*x + (C*y² + E*y + F) = 0
|
||
// Treat as quadratics in x with y-dependent coefficients:
|
||
// a_i(y)*x² + b_i(y)*x + c_i(y) = 0
|
||
// a₁ = A₁, b₁(y) = B₁y + D₁, c₁(y) = C₁y² + E₁y + F₁
|
||
// Resultant eliminates x, giving poly in y of degree ≤ 4
|
||
|
||
// Build the resultant polynomial coefficients in y
|
||
// Res(f,g) = a₁²c₂² + a₂²c₁² - a₁a₂(b₁c₂+b₂c₁) + b₁²a₂c₂ + ... (Sylvester det)
|
||
// It's cleaner to use the 4x4 Sylvester matrix determinant directly.
|
||
|
||
// Sylvester matrix for two quadratics in x:
|
||
// | a₁ b₁(y) c₁(y) 0 |
|
||
// | 0 a₁ b₁(y) c₁(y)|
|
||
// | a₂ b₂(y) c₂(y) 0 |
|
||
// | 0 a₂ b₂(y) c₂(y)|
|
||
//
|
||
// Each entry is a polynomial in y. The determinant is a polynomial in y of degree ≤ 4.
|
||
// We evaluate at 5 values of y and interpolate to find the coefficients.
|
||
|
||
const yVals = [-2, -1, 0, 1, 2];
|
||
const detVals = yVals.map((y) => {
|
||
const b1 = c1.B * y + c1.D;
|
||
const cc1 = c1.C * y * y + c1.E * y + c1.F;
|
||
const b2 = c2.B * y + c2.D;
|
||
const cc2 = c2.C * y * y + c2.E * y + c2.F;
|
||
|
||
// 4x4 determinant
|
||
const m = [
|
||
[c1.A, b1, cc1, 0],
|
||
[0, c1.A, b1, cc1],
|
||
[c2.A, b2, cc2, 0],
|
||
[0, c2.A, b2, cc2],
|
||
];
|
||
return det4x4(m);
|
||
});
|
||
|
||
// Interpolate: we have 5 points → degree-4 polynomial
|
||
const polyCoeffs = lagrangeInterpolate(yVals, detVals);
|
||
|
||
// Find real roots of the degree-4 polynomial
|
||
const yRoots = findRealRoots(polyCoeffs);
|
||
|
||
// For each y root, solve for x from the original conics
|
||
const results: Point2D[] = [];
|
||
for (const y of yRoots) {
|
||
const xSolutions = solveForX(c1, c2, y);
|
||
for (const x of xSolutions) {
|
||
// Verify solution satisfies both conics (within tolerance)
|
||
const v1 = evalConic(c1, x, y);
|
||
const v2 = evalConic(c2, x, y);
|
||
const maxCoeff = Math.max(
|
||
Math.abs(c1.A) + Math.abs(c1.C) + Math.abs(c1.F),
|
||
Math.abs(c2.A) + Math.abs(c2.C) + Math.abs(c2.F),
|
||
1,
|
||
);
|
||
if (Math.abs(v1) < maxCoeff * 0.1 && Math.abs(v2) < maxCoeff * 0.1) {
|
||
results.push({ x, y });
|
||
}
|
||
}
|
||
}
|
||
|
||
return deduplicatePoints(results);
|
||
}
|
||
|
||
function evalConic(c: ConicCoeffs, x: number, y: number): number {
|
||
return c.A * x * x + c.B * x * y + c.C * y * y + c.D * x + c.E * y + c.F;
|
||
}
|
||
|
||
function det4x4(m: number[][]): number {
|
||
const [a, b, c, d] = m;
|
||
return (
|
||
a[0] * (b[1] * (c[2] * d[3] - c[3] * d[2]) - b[2] * (c[1] * d[3] - c[3] * d[1]) + b[3] * (c[1] * d[2] - c[2] * d[1]))
|
||
- a[1] * (b[0] * (c[2] * d[3] - c[3] * d[2]) - b[2] * (c[0] * d[3] - c[3] * d[0]) + b[3] * (c[0] * d[2] - c[2] * d[0]))
|
||
+ a[2] * (b[0] * (c[1] * d[3] - c[3] * d[1]) - b[1] * (c[0] * d[3] - c[3] * d[0]) + b[3] * (c[0] * d[1] - c[1] * d[0]))
|
||
- a[3] * (b[0] * (c[1] * d[2] - c[2] * d[1]) - b[1] * (c[0] * d[2] - c[2] * d[0]) + b[2] * (c[0] * d[1] - c[1] * d[0]))
|
||
);
|
||
}
|
||
|
||
/**
|
||
* Lagrange interpolation: given (x_i, y_i) pairs, return polynomial coefficients [a0, a1, ..., an]
|
||
* such that p(x) = a0 + a1*x + a2*x² + ...
|
||
*/
|
||
function lagrangeInterpolate(xs: number[], ys: number[]): number[] {
|
||
const n = xs.length;
|
||
const coeffs = new Array(n).fill(0);
|
||
|
||
for (let i = 0; i < n; i++) {
|
||
// Build the i-th Lagrange basis polynomial
|
||
const basis = [ys[i]];
|
||
for (let j = 0; j < n; j++) {
|
||
if (j === i) continue;
|
||
const scale = 1 / (xs[i] - xs[j]);
|
||
const newBasis = new Array(basis.length + 1).fill(0);
|
||
for (let k = 0; k < basis.length; k++) {
|
||
newBasis[k] += basis[k] * (-xs[j]) * scale;
|
||
newBasis[k + 1] += basis[k] * scale;
|
||
}
|
||
basis.length = newBasis.length;
|
||
for (let k = 0; k < newBasis.length; k++) basis[k] = newBasis[k];
|
||
}
|
||
for (let k = 0; k < basis.length && k < n; k++) coeffs[k] += basis[k];
|
||
}
|
||
return coeffs;
|
||
}
|
||
|
||
/**
|
||
* Find real roots of a polynomial a0 + a1*x + a2*x² + ... + an*x^n = 0
|
||
* Uses companion matrix eigenvalues via QR iteration.
|
||
*/
|
||
function findRealRoots(coeffs: number[]): number[] {
|
||
// Trim trailing near-zero coefficients
|
||
while (coeffs.length > 1 && Math.abs(coeffs[coeffs.length - 1]) < EPS) {
|
||
coeffs.pop();
|
||
}
|
||
const deg = coeffs.length - 1;
|
||
if (deg <= 0) return [];
|
||
if (deg === 1) return [-coeffs[0] / coeffs[1]];
|
||
if (deg === 2) return solveQuadratic(coeffs[2], coeffs[1], coeffs[0]);
|
||
|
||
// Build companion matrix
|
||
const n = deg;
|
||
const C = Array.from({ length: n }, () => new Float64Array(n));
|
||
const lead = coeffs[n];
|
||
for (let i = 0; i < n; i++) {
|
||
C[i][n - 1] = -coeffs[i] / lead;
|
||
}
|
||
for (let i = 1; i < n; i++) {
|
||
C[i][i - 1] = 1;
|
||
}
|
||
|
||
// QR iteration to find eigenvalues
|
||
return qrEigenvalues(C);
|
||
}
|
||
|
||
/** Solve ax²+bx+c=0 for real roots */
|
||
function solveQuadratic(a: number, b: number, c: number): number[] {
|
||
if (Math.abs(a) < EPS) {
|
||
if (Math.abs(b) < EPS) return [];
|
||
return [-c / b];
|
||
}
|
||
const disc = b * b - 4 * a * c;
|
||
if (disc < -EPS) return [];
|
||
if (disc < EPS) return [-b / (2 * a)];
|
||
const sq = Math.sqrt(disc);
|
||
return [(-b + sq) / (2 * a), (-b - sq) / (2 * a)];
|
||
}
|
||
|
||
/**
|
||
* QR iteration for eigenvalues of an n×n matrix (n ≤ 4).
|
||
* Returns only real eigenvalues.
|
||
*/
|
||
function qrEigenvalues(A: Float64Array[]): number[] {
|
||
const n = A.length;
|
||
const H = A.map((r) => Float64Array.from(r));
|
||
|
||
// Hessenberg reduction first
|
||
for (let k = 0; k < n - 2; k++) {
|
||
// Find the pivot
|
||
let maxVal = 0, maxIdx = k + 1;
|
||
for (let i = k + 1; i < n; i++) {
|
||
if (Math.abs(H[i][k]) > maxVal) { maxVal = Math.abs(H[i][k]); maxIdx = i; }
|
||
}
|
||
if (maxVal < EPS) continue;
|
||
if (maxIdx !== k + 1) {
|
||
[H[k + 1], H[maxIdx]] = [H[maxIdx], H[k + 1]];
|
||
for (let i = 0; i < n; i++) {
|
||
const tmp = H[i][k + 1]; H[i][k + 1] = H[i][maxIdx]; H[i][maxIdx] = tmp;
|
||
}
|
||
}
|
||
|
||
for (let i = k + 2; i < n; i++) {
|
||
const factor = H[i][k] / H[k + 1][k];
|
||
for (let j = k; j < n; j++) H[i][j] -= factor * H[k + 1][j];
|
||
for (let j = 0; j < n; j++) H[j][k + 1] += factor * H[j][i];
|
||
}
|
||
}
|
||
|
||
// QR iteration with Wilkinson shift
|
||
const eigenvalues: number[] = [];
|
||
let m = n;
|
||
for (let maxIter = 0; maxIter < 200 && m > 0; maxIter++) {
|
||
if (m === 1) {
|
||
eigenvalues.push(H[0][0]);
|
||
break;
|
||
}
|
||
|
||
// Check for convergence on sub-diagonal
|
||
if (Math.abs(H[m - 1][m - 2]) < EPS * (Math.abs(H[m - 1][m - 1]) + Math.abs(H[m - 2][m - 2]) + 1)) {
|
||
eigenvalues.push(H[m - 1][m - 1]);
|
||
m--;
|
||
continue;
|
||
}
|
||
|
||
// Wilkinson shift: eigenvalue of bottom-right 2x2 closer to H[m-1][m-1]
|
||
const a11 = H[m - 2][m - 2], a12 = H[m - 2][m - 1];
|
||
const a21 = H[m - 1][m - 2], a22 = H[m - 1][m - 1];
|
||
const tr = a11 + a22;
|
||
const det = a11 * a22 - a12 * a21;
|
||
const disc = tr * tr - 4 * det;
|
||
let shift: number;
|
||
if (disc >= 0) {
|
||
const sq = Math.sqrt(disc);
|
||
const e1 = (tr + sq) / 2, e2 = (tr - sq) / 2;
|
||
shift = Math.abs(e1 - a22) < Math.abs(e2 - a22) ? e1 : e2;
|
||
} else {
|
||
shift = tr / 2;
|
||
}
|
||
|
||
// QR step: (H - shift*I) = Q*R, then H = R*Q + shift*I
|
||
// Implemented via Givens rotations
|
||
for (let i = 0; i < m; i++) H[i][i] -= shift;
|
||
|
||
const cs = new Float64Array(m - 1);
|
||
const sn = new Float64Array(m - 1);
|
||
for (let i = 0; i < m - 1; i++) {
|
||
const a = H[i][i], b = H[i + 1][i];
|
||
const r = Math.hypot(a, b);
|
||
if (r < EPS) { cs[i] = 1; sn[i] = 0; continue; }
|
||
cs[i] = a / r; sn[i] = b / r;
|
||
// Apply rotation to rows i, i+1
|
||
for (let j = 0; j < m; j++) {
|
||
const t1 = H[i][j], t2 = H[i + 1][j];
|
||
H[i][j] = cs[i] * t1 + sn[i] * t2;
|
||
H[i + 1][j] = -sn[i] * t1 + cs[i] * t2;
|
||
}
|
||
}
|
||
// Apply Q^T from the right
|
||
for (let i = 0; i < m - 1; i++) {
|
||
for (let j = 0; j < m; j++) {
|
||
const t1 = H[j][i], t2 = H[j][i + 1];
|
||
H[j][i] = cs[i] * t1 + sn[i] * t2;
|
||
H[j][i + 1] = -sn[i] * t1 + cs[i] * t2;
|
||
}
|
||
}
|
||
|
||
for (let i = 0; i < m; i++) H[i][i] += shift;
|
||
}
|
||
|
||
return eigenvalues.filter((v) => isFinite(v));
|
||
}
|
||
|
||
/** Given a y value, solve for x from both conics and return candidate x values */
|
||
function solveForX(c1: ConicCoeffs, c2: ConicCoeffs, y: number): number[] {
|
||
// From conic 1: A₁x² + (B₁y+D₁)x + (C₁y²+E₁y+F₁) = 0
|
||
const a1 = c1.A;
|
||
const b1 = c1.B * y + c1.D;
|
||
const cc1 = c1.C * y * y + c1.E * y + c1.F;
|
||
const roots1 = solveQuadratic(a1, b1, cc1);
|
||
|
||
const a2 = c2.A;
|
||
const b2 = c2.B * y + c2.D;
|
||
const cc2 = c2.C * y * y + c2.E * y + c2.F;
|
||
const roots2 = solveQuadratic(a2, b2, cc2);
|
||
|
||
// Return x values that appear in both solutions (within tolerance)
|
||
const results: number[] = [];
|
||
for (const x1 of roots1) {
|
||
for (const x2 of roots2) {
|
||
if (Math.abs(x1 - x2) < Math.max(Math.abs(x1), 1) * 0.01) {
|
||
results.push((x1 + x2) / 2);
|
||
}
|
||
}
|
||
}
|
||
// If no matching roots, return all roots from conic 1 (the resultant guarantees they exist)
|
||
if (results.length === 0) return roots1;
|
||
return results;
|
||
}
|
||
|
||
function deduplicatePoints(pts: Point2D[], tol = 1): Point2D[] {
|
||
const out: Point2D[] = [];
|
||
for (const p of pts) {
|
||
if (!out.some((q) => Math.hypot(q.x - p.x, q.y - p.y) < tol)) {
|
||
out.push(p);
|
||
}
|
||
}
|
||
return out;
|
||
}
|
||
|
||
// ────────────────────────────────────────────────────────
|
||
// High-level: process a route into conic arcs
|
||
// ────────────────────────────────────────────────────────
|
||
|
||
export function fitRoute(points: Point2D[]): ConicArc[] {
|
||
const segments = segmentRoute(points);
|
||
return segments.map((seg) => {
|
||
const coeffs = fitConic(seg);
|
||
const { type, eccentricity } = classifyConic(coeffs);
|
||
const beziers = conicArcToBeziers(coeffs, seg);
|
||
return { coeffs, type, eccentricity, points: seg, beziers };
|
||
});
|
||
}
|
||
|
||
/**
|
||
* Find all intersection points between arcs of two fitted routes.
|
||
* Filters to points that lie within the bounding box of both arcs.
|
||
*/
|
||
export function findAllIntersections(arcsA: ConicArc[], arcsB: ConicArc[]): IntersectionPoint[] {
|
||
const results: IntersectionPoint[] = [];
|
||
|
||
for (let ia = 0; ia < arcsA.length; ia++) {
|
||
for (let ib = 0; ib < arcsB.length; ib++) {
|
||
// Quick bounding-box overlap check
|
||
const bbA = arcBounds(arcsA[ia].points);
|
||
const bbB = arcBounds(arcsB[ib].points);
|
||
if (!boxesOverlap(bbA, bbB)) continue;
|
||
|
||
const pts = findIntersections(arcsA[ia].coeffs, arcsB[ib].coeffs);
|
||
for (const pt of pts) {
|
||
// Filter: point must be within expanded bounding boxes of both arcs
|
||
if (inBox(pt, bbA, 20) && inBox(pt, bbB, 20)) {
|
||
results.push({ point: pt, arcIndexA: ia, arcIndexB: ib });
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
// Deduplicate across arc pairs
|
||
const unique: IntersectionPoint[] = [];
|
||
for (const r of results) {
|
||
if (!unique.some((u) => Math.hypot(u.point.x - r.point.x, u.point.y - r.point.y) < 3)) {
|
||
unique.push(r);
|
||
}
|
||
}
|
||
return unique;
|
||
}
|
||
|
||
interface BBox { minX: number; maxX: number; minY: number; maxY: number }
|
||
|
||
function arcBounds(pts: Point2D[]): BBox {
|
||
let minX = Infinity, maxX = -Infinity, minY = Infinity, maxY = -Infinity;
|
||
for (const p of pts) {
|
||
if (p.x < minX) minX = p.x;
|
||
if (p.x > maxX) maxX = p.x;
|
||
if (p.y < minY) minY = p.y;
|
||
if (p.y > maxY) maxY = p.y;
|
||
}
|
||
return { minX, maxX, minY, maxY };
|
||
}
|
||
|
||
function boxesOverlap(a: BBox, b: BBox): boolean {
|
||
return a.minX <= b.maxX && a.maxX >= b.minX && a.minY <= b.maxY && a.maxY >= b.minY;
|
||
}
|
||
|
||
function inBox(p: Point2D, bb: BBox, margin: number): boolean {
|
||
return p.x >= bb.minX - margin && p.x <= bb.maxX + margin
|
||
&& p.y >= bb.minY - margin && p.y <= bb.maxY + margin;
|
||
}
|
||
|
||
// ────────────────────────────────────────────────────────
|
||
// Ghost conic curve sampling & tangent
|
||
// ────────────────────────────────────────────────────────
|
||
|
||
/**
|
||
* Return the tangent direction at a point on the conic (perpendicular to gradient).
|
||
* Gradient of F = (2Ax+By+D, Bx+2Cy+E), tangent = (-∂F/∂y, ∂F/∂x).
|
||
*/
|
||
export function conicTangentAt(coeffs: ConicCoeffs, pt: Point2D): { dx: number; dy: number } {
|
||
const gx = 2 * coeffs.A * pt.x + coeffs.B * pt.y + coeffs.D;
|
||
const gy = coeffs.B * pt.x + 2 * coeffs.C * pt.y + coeffs.E;
|
||
const len = Math.hypot(gx, gy) || 1;
|
||
return { dx: -gy / len, dy: gx / len };
|
||
}
|
||
|
||
/**
|
||
* Sample the full conic curve extending `extend` units beyond the arc endpoints.
|
||
* Marches along the tangent direction, projecting back onto the conic via Newton's method.
|
||
* Returns the extended point array (backward extension + original arc + forward extension).
|
||
*/
|
||
export function sampleConicCurve(coeffs: ConicCoeffs, arcPoints: Point2D[], extend: number): Point2D[] {
|
||
if (arcPoints.length < 2) return [...arcPoints];
|
||
|
||
const { type } = classifyConic(coeffs);
|
||
if (type === "degenerate") return [...arcPoints];
|
||
|
||
// Clamp extension to 50% of arc length, minimum 20
|
||
const arcLen = arcPoints.reduce((sum, p, i) =>
|
||
i === 0 ? 0 : sum + Math.hypot(p.x - arcPoints[i - 1].x, p.y - arcPoints[i - 1].y), 0);
|
||
const maxExt = Math.max(20, Math.min(extend, arcLen * 0.5));
|
||
const step = Math.max(2, maxExt / 25);
|
||
|
||
const backward = marchConic(coeffs, arcPoints[0], arcPoints[1], -step, maxExt);
|
||
const forward = marchConic(coeffs, arcPoints[arcPoints.length - 1], arcPoints[arcPoints.length - 2], -step, maxExt);
|
||
|
||
// backward is in reverse order, forward is in forward order (away from arc end)
|
||
return [...backward.reverse(), ...arcPoints, ...forward];
|
||
}
|
||
|
||
/**
|
||
* March along a conic curve from `start` in the direction away from `prev`.
|
||
* At each step, advance along the tangent then project back onto the conic.
|
||
*/
|
||
function marchConic(coeffs: ConicCoeffs, start: Point2D, prev: Point2D, stepSize: number, maxDist: number): Point2D[] {
|
||
const points: Point2D[] = [];
|
||
let cur = { ...start };
|
||
|
||
// Initial direction: away from prev
|
||
let dirX = start.x - prev.x;
|
||
let dirY = start.y - prev.y;
|
||
let dLen = Math.hypot(dirX, dirY) || 1;
|
||
dirX /= dLen; dirY /= dLen;
|
||
|
||
const absStep = Math.abs(stepSize);
|
||
let traveled = 0;
|
||
|
||
for (let i = 0; i < 30 && traveled < maxDist; i++) {
|
||
// Advance along tangent direction
|
||
const t = conicTangentAt(coeffs, cur);
|
||
// Choose tangent direction consistent with our travel direction
|
||
const dot = t.dx * dirX + t.dy * dirY;
|
||
const sign = dot >= 0 ? 1 : -1;
|
||
let nx = cur.x + sign * t.dx * absStep;
|
||
let ny = cur.y + sign * t.dy * absStep;
|
||
|
||
// Newton projection: push (nx,ny) back onto the conic
|
||
for (let iter = 0; iter < 8; iter++) {
|
||
const val = evalConicAt(coeffs, nx, ny);
|
||
const gx = 2 * coeffs.A * nx + coeffs.B * ny + coeffs.D;
|
||
const gy = coeffs.B * nx + 2 * coeffs.C * ny + coeffs.E;
|
||
const g2 = gx * gx + gy * gy;
|
||
if (g2 < 1e-20) break;
|
||
const lambda = val / g2;
|
||
nx -= lambda * gx;
|
||
ny -= lambda * gy;
|
||
if (Math.abs(val) < 1e-6) break;
|
||
}
|
||
|
||
// Bail if projection diverged
|
||
if (!isFinite(nx) || !isFinite(ny)) break;
|
||
if (Math.hypot(nx - cur.x, ny - cur.y) > absStep * 3) break;
|
||
|
||
dirX = nx - cur.x; dirY = ny - cur.y;
|
||
dLen = Math.hypot(dirX, dirY) || 1;
|
||
dirX /= dLen; dirY /= dLen;
|
||
|
||
cur = { x: nx, y: ny };
|
||
traveled += absStep;
|
||
points.push({ ...cur });
|
||
}
|
||
|
||
return points;
|
||
}
|
||
|
||
function evalConicAt(c: ConicCoeffs, x: number, y: number): number {
|
||
return c.A * x * x + c.B * x * y + c.C * y * y + c.D * x + c.E * y + c.F;
|
||
}
|
||
|
||
// ────────────────────────────────────────────────────────
|
||
// Bézier → SVG path conversion
|
||
// ────────────────────────────────────────────────────────
|
||
|
||
/**
|
||
* Convert a rational quadratic Bézier to an SVG cubic Bézier path command.
|
||
* Degree elevation: rational quad → approximated cubic.
|
||
*/
|
||
export function bezierToSVGCubic(b: RationalQuadBezier): string {
|
||
// Approximate rational quadratic as cubic via degree elevation
|
||
// For w ≈ 1, this is nearly exact. For w far from 1, sample instead.
|
||
const w = b.w;
|
||
if (w < 0.3 || w > 3) {
|
||
// Sample the rational curve and emit as polyline
|
||
return sampleBezierPath(b, 16);
|
||
}
|
||
|
||
const c1x = b.p0.x + (2 * w * (b.p1.x - b.p0.x)) / (1 + w) * (2 / 3);
|
||
const c1y = b.p0.y + (2 * w * (b.p1.y - b.p0.y)) / (1 + w) * (2 / 3);
|
||
const c2x = b.p2.x + (2 * w * (b.p1.x - b.p2.x)) / (1 + w) * (2 / 3);
|
||
const c2y = b.p2.y + (2 * w * (b.p1.y - b.p2.y)) / (1 + w) * (2 / 3);
|
||
|
||
return `C ${c1x.toFixed(1)} ${c1y.toFixed(1)} ${c2x.toFixed(1)} ${c2y.toFixed(1)} ${b.p2.x.toFixed(1)} ${b.p2.y.toFixed(1)}`;
|
||
}
|
||
|
||
/** Sample a rational quadratic Bézier and return SVG line-to commands */
|
||
function sampleBezierPath(b: RationalQuadBezier, samples: number): string {
|
||
const parts: string[] = [];
|
||
for (let i = 1; i <= samples; i++) {
|
||
const t = i / samples;
|
||
const pt = evalRationalQuadBezier(b, t);
|
||
parts.push(`L ${pt.x.toFixed(1)} ${pt.y.toFixed(1)}`);
|
||
}
|
||
return parts.join(" ");
|
||
}
|
||
|
||
function evalRationalQuadBezier(b: RationalQuadBezier, t: number): Point2D {
|
||
const omt = 1 - t;
|
||
const w0 = omt * omt;
|
||
const w1 = 2 * b.w * t * omt;
|
||
const w2 = t * t;
|
||
const denom = w0 + w1 + w2;
|
||
return {
|
||
x: (w0 * b.p0.x + w1 * b.p1.x + w2 * b.p2.x) / denom,
|
||
y: (w0 * b.p0.y + w1 * b.p1.y + w2 * b.p2.y) / denom,
|
||
};
|
||
}
|