Rank-safe plane fit for contact patches
The least-squares fit of the exciter's rigid face plane solved the normal
equations with a 3x3 elimination, which is singular whenever the patch
points are collinear, as a stick's line of springs always is. The failure
was masked by round-off in most geometries and surfaced as "singular 3x3
system" for particular strike positions.
Fit by Gram-Schmidt on the columns (1, x, y) instead, dropping a column
that is dependent on the previous ones. This reproduces the minimum-norm
solution that numpy's lstsq gives in the Python package: a line fit for
collinear points and a zero residual for a single point. Both cases are
added to the plane-fit test.
2 changed files+33−8
src/model/linalg.tsmodified+28−8View file
@@ -50,16 +50,36 @@ export function solve3(A: number[], b: number[]): number[] {
5050 return [M[0][3] / M[0][0], M[1][3] / M[1][1], M[2][3] / M[2][2]];
5151 }
5252
53-/** Least-squares fit z = a x + b y + c through points (x_i, y_i, z_i); returns the residuals z - fit. */
53+/** Least-squares fit z = a x + b y + c through points (x_i, y_i, z_i); returns the residuals z - fit.
54+ *
55+ * The fit is done by Gram-Schmidt on the columns (1, x, y), dropping a column that is (nearly)
56+ * dependent on the previous ones, which is what numpy's minimum-norm least squares does in the
57+ * Python package: the collinear points of a stick's line get a line fit, a single point a zero
58+ * residual. The normal equations would be singular in those cases. */
5459 export function planeResiduals(x: ArrayLike<number>, y: ArrayLike<number>, z: ArrayLike<number>): Float64Array {
5560 const n = x.length;
56- const AtA = new Array(9).fill(0), Atb = [0, 0, 0];
57- for (let i = 0; i < n; i++) {
58- const row = [x[i], y[i], 1];
59- for (let a = 0; a < 3; a++) { Atb[a] += row[a] * z[i]; for (let b = 0; b < 3; b++) AtA[3 * a + b] += row[a] * row[b]; }
61+ const res = Float64Array.from(z as number[]);
62+ const basis: Float64Array[] = [];
63+ for (const col of [Array.from({ length: n }, () => 1), Array.from(x), Array.from(y)]) {
64+ const v = Float64Array.from(col);
65+ let norm0 = 0;
66+ for (let i = 0; i < n; i++) norm0 += v[i] * v[i];
67+ for (const b of basis) {
68+ let d = 0;
69+ for (let i = 0; i < n; i++) d += b[i] * v[i];
70+ for (let i = 0; i < n; i++) v[i] -= d * b[i];
71+ }
72+ let norm = 0;
73+ for (let i = 0; i < n; i++) norm += v[i] * v[i];
74+ if (norm === 0 || norm <= 1e-20 * norm0) continue;
75+ const inv = 1 / Math.sqrt(norm);
76+ for (let i = 0; i < n; i++) v[i] *= inv;
77+ basis.push(v);
78+ }
79+ for (const b of basis) {
80+ let d = 0;
81+ for (let i = 0; i < n; i++) d += b[i] * res[i];
82+ for (let i = 0; i < n; i++) res[i] -= d * b[i];
6083 }
61- const coef = solve3(AtA, Atb);
62- const res = new Float64Array(n);
63- for (let i = 0; i < n; i++) res[i] = z[i] - (coef[0] * x[i] + coef[1] * y[i] + coef[2]);
6484 return res;
6585 }
tests/dkt.test.tsmodified+5−0View file
@@ -32,4 +32,9 @@ test("Jacobi eigenvalues and plane fit", () => {
3232 assert.ok(maxAbsDiff(ev, [1, 3, 5]) < 1e-12);
3333 const res = planeResiduals([0, 1, 0, 1], [0, 0, 1, 1], [1, 3, 4, 6]);
3434 assert.ok(maxAbs(res) < 1e-12);
35+ // collinear points (a stick's line): the fit degenerates to a line, as numpy's lstsq gives
36+ const line = planeResiduals([-1, 0, 1], [-2, 0, 2], [1, 2, 3.5]);
37+ assert.ok(maxAbsDiff(line, [1 / 12, -1 / 6, 1 / 12]) < 1e-12);
38+ // a single point: zero residual
39+ assert.ok(maxAbs(planeResiduals([0.3], [0.1], [0.02])) < 1e-15);
3540 });