From 6aa89d3f5a62f449b22027f7240fbd179e26a581 Mon Sep 17 00:00:00 2001 From: Francois Date: Sun, 19 Oct 2025 19:50:15 +0900 Subject: [PATCH] feat(physics): add spd enforcement --- ROADMAP.md | 2 +- docs/index.d.ts | 13 +++++++ examples/foldSpd.ts | 11 ++++++ src/index.ts | 3 ++ src/physics/fold/index.ts | 1 + src/physics/fold/spd.ts | 76 +++++++++++++++++++++++++++++++++++++++ tests/index.test.ts | 2 ++ tests/spd.test.ts | 58 ++++++++++++++++++++++++++++++ 8 files changed, 165 insertions(+), 1 deletion(-) create mode 100644 examples/foldSpd.ts create mode 100644 src/physics/fold/spd.ts create mode 100644 tests/spd.test.ts diff --git a/ROADMAP.md b/ROADMAP.md index 90342fa..d1d15c5 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -134,7 +134,7 @@ - [x] Friction potential tied to contact force magnitude - [x] Matrix assembly with cached contact index tables - [x] Gap evaluators for point/triangle, edge/edge, and wall constraints - - [ ] SPD enforcement pass for elasticity Hessian blocks + - [x] SPD enforcement pass for elasticity Hessian blocks ### LLM‑Optimised Additions (Priority Rationale) diff --git a/docs/index.d.ts b/docs/index.d.ts index 927d9f5..b360ae5 100644 --- a/docs/index.d.ts +++ b/docs/index.d.ts @@ -128,6 +128,7 @@ export const examples: { readonly computePointTriangleGap: 'examples/foldGapEvaluators.ts'; readonly computeEdgeEdgeGap: 'examples/foldGapEvaluators.ts'; readonly computePointPlaneGap: 'examples/foldGapEvaluators.ts'; + readonly enforceSpd: 'examples/foldSpd.ts'; }; readonly performance: { readonly debounce: 'examples/requestDedup.ts'; @@ -3444,6 +3445,18 @@ export function computeEdgeEdgeGap(edgeA: readonly [Point3D, Point3D], edgeB: re export interface PointPlaneGapResult { gap: number; projectedPoint: Point3D; normal: Vector3D } export function computePointPlaneGap(point: Point3D, planePoint: Point3D, planeNormal: Vector3D): PointPlaneGapResult; +/** + * Enforce SPD on a symmetric 3x3 matrix via diagonal shifts. + * Use for: Fold Hessian regularisation and solver stability. + * Import: physics/fold/spd.ts + */ +export interface SpdEnforcementOptions { + epsilon?: number; + maxIterations?: number; + initialShift?: number; +} +export function enforceSpd(matrix: Matrix3x3, options?: SpdEnforcementOptions): Matrix3x3; + export type FoldConstraintType = | 'cubic-barrier' | 'contact-barrier' diff --git a/examples/foldSpd.ts b/examples/foldSpd.ts new file mode 100644 index 0000000..d16615a --- /dev/null +++ b/examples/foldSpd.ts @@ -0,0 +1,11 @@ +import { enforceSpd } from '../src/index.js'; +import type { Matrix3x3 } from '../src/types.js'; + +const matrix: Matrix3x3 = [ + [0.0, 0.2, 0.1], + [0.2, -0.1, 0.3], + [0.1, 0.3, 0.5], +]; + +const result = enforceSpd(matrix); +console.log(result); diff --git a/src/index.ts b/src/index.ts index 6c3cdd1..07e8a1d 100644 --- a/src/index.ts +++ b/src/index.ts @@ -126,6 +126,7 @@ export const examples = { computePointTriangleGap: 'examples/foldGapEvaluators.ts', computeEdgeEdgeGap: 'examples/foldGapEvaluators.ts', computePointPlaneGap: 'examples/foldGapEvaluators.ts', + enforceSpd: 'examples/foldSpd.ts', }, performance: { debounce: 'examples/requestDedup.ts', @@ -1211,6 +1212,7 @@ export { computePointTriangleGap, computeEdgeEdgeGap, computePointPlaneGap, + enforceSpd, } from './physics/fold/index.js'; export type { @@ -1239,6 +1241,7 @@ export type { PointTriangleGapResult, EdgeEdgeGapResult, PointPlaneGapResult, + SpdEnforcementOptions, } from './physics/fold/index.js'; // ============================================================================ diff --git a/src/physics/fold/index.ts b/src/physics/fold/index.ts index c1d0d68..0749ce8 100644 --- a/src/physics/fold/index.ts +++ b/src/physics/fold/index.ts @@ -8,3 +8,4 @@ export * from './strainBarrier.js'; export * from './frictionPotential.js'; export * from './matrixAssembly.js'; export * from './gapEvaluators.js'; +export * from './spd.js'; diff --git a/src/physics/fold/spd.ts b/src/physics/fold/spd.ts new file mode 100644 index 0000000..c0c2aff --- /dev/null +++ b/src/physics/fold/spd.ts @@ -0,0 +1,76 @@ +import type { Matrix3x3 } from '../../types.js'; + +export interface SpdEnforcementOptions { + epsilon?: number; + maxIterations?: number; + initialShift?: number; +} + +export function enforceSpd(matrix: Matrix3x3, options: SpdEnforcementOptions = {}): Matrix3x3 { + validateMatrix(matrix); + + const epsilon = options.epsilon ?? 1e-8; + const maxIterations = options.maxIterations ?? 12; + const base = symmetrise(matrix); + let shift = Math.max(options.initialShift ?? 0, 0); + let current = shift > 0 ? addShift(base, shift) : base; + + for (let iteration = 0; iteration < maxIterations; iteration += 1) { + if (isPositiveDefinite(current)) { + return current; + } + + const minDiag = Math.min(base[0][0], base[1][1], base[2][2]); + const requiredShift = minDiag > 0 ? shift : Math.max(-minDiag + epsilon, shift); + shift = Math.max(shift > 0 ? shift * 2 : epsilon, requiredShift); + current = addShift(base, shift); + } + + return addShift(base, shift > 0 ? shift : epsilon); +} + +function validateMatrix(matrix: Matrix3x3 | undefined): asserts matrix is Matrix3x3 { + if (!Array.isArray(matrix) || matrix.length !== 3) { + throw new TypeError('Matrix must be a 3x3 array.'); + } + matrix.forEach((row) => { + if (!Array.isArray(row) || row.length !== 3 || row.some((value) => !Number.isFinite(value))) { + throw new TypeError('Matrix rows must contain three finite numbers.'); + } + }); +} + +function symmetrise(matrix: Matrix3x3): Matrix3x3 { + return matrix.map((row, i) => + row.map((_, j) => ((matrix[i]?.[j] ?? 0) + (matrix[j]?.[i] ?? 0)) / 2) + ) as Matrix3x3; +} + +function addShift(matrix: Matrix3x3, shift: number): Matrix3x3 { + const shifted: Matrix3x3 = matrix.map((row) => [...row]) as Matrix3x3; + for (let i = 0; i < 3; i += 1) { + shifted[i][i] = (shifted[i][i] ?? 0) + shift; + } + return shifted; +} + +function isPositiveDefinite(matrix: Matrix3x3): boolean { + const cholesky = Array.from({ length: 3 }, () => [0, 0, 0]); + for (let i = 0; i < 3; i += 1) { + for (let j = 0; j <= i; j += 1) { + let sum = matrix[i][j]; + for (let k = 0; k < j; k += 1) { + sum -= cholesky[i][k] * cholesky[j][k]; + } + if (i === j) { + if (sum <= 0) { + return false; + } + cholesky[i][j] = Math.sqrt(sum); + } else { + cholesky[i][j] = sum / cholesky[j][j]; + } + } + } + return true; +} diff --git a/tests/index.test.ts b/tests/index.test.ts index d61e7f8..8fcff8a 100644 --- a/tests/index.test.ts +++ b/tests/index.test.ts @@ -69,6 +69,7 @@ describe('package entry point', () => { expect(examples.physics.computePointTriangleGap).toBe('examples/foldGapEvaluators.ts'); expect(examples.physics.computeEdgeEdgeGap).toBe('examples/foldGapEvaluators.ts'); expect(examples.physics.computePointPlaneGap).toBe('examples/foldGapEvaluators.ts'); + expect(examples.physics.enforceSpd).toBe('examples/foldSpd.ts'); }); it('provides strong typing for example categories and names', () => { @@ -221,6 +222,7 @@ describe('package entry point', () => { | 'computePointTriangleGap' | 'computeEdgeEdgeGap' | 'computePointPlaneGap' + | 'enforceSpd' >(); expectTypeOf>().toEqualTypeOf< diff --git a/tests/spd.test.ts b/tests/spd.test.ts new file mode 100644 index 0000000..96011f2 --- /dev/null +++ b/tests/spd.test.ts @@ -0,0 +1,58 @@ +import { describe, expect, it } from 'vitest'; + +import { enforceSpd } from '../src/physics/fold/spd.js'; +import type { Matrix3x3 } from '../src/types.js'; + +const isPositiveDefinite = (matrix: Matrix3x3): boolean => { + const n = matrix.length; + const cholesky: number[][] = []; + for (let i = 0; i < n; i += 1) { + const row: number[] = Array.from({ length: n }, () => 0); + cholesky.push(row); + } + for (let i = 0; i < n; i += 1) { + for (let j = 0; j <= i; j += 1) { + let sum = matrix[i][j]; + for (let k = 0; k < j; k += 1) { + sum -= cholesky[i][k] * cholesky[j][k]; + } + if (i === j) { + if (sum <= 0) return false; + cholesky[i][j] = Math.sqrt(sum); + } else { + cholesky[i][j] = sum / cholesky[j][j]; + } + } + } + return true; +}; + +describe('enforceSpd', () => { + it('returns SPD matrix for indefinite input', () => { + const matrix: Matrix3x3 = [ + [0.0, 0.2, 0.1], + [0.2, -0.5, 0.3], + [0.1, 0.3, 0.5], + ]; + + const result = enforceSpd(matrix); + expect(isPositiveDefinite(result)).toBe(true); + }); + + it('returns original matrix when already SPD', () => { + const matrix: Matrix3x3 = [ + [2, 0, 0], + [0, 3, 0], + [0, 0, 4], + ]; + + const result = enforceSpd(matrix); + expect(result).toEqual(matrix); + expect(result).not.toBe(matrix); + expect(matrix).toEqual([ + [2, 0, 0], + [0, 3, 0], + [0, 0, 4], + ]); + }); +});