From f2f297b45aed710d59ef0b1ba28060535fa88f11 Mon Sep 17 00:00:00 2001 From: Francois Date: Sat, 18 Oct 2025 20:09:41 +0900 Subject: [PATCH] feat(physics): add contact matrix assembly --- ROADMAP.md | 2 +- docs/index.d.ts | 13 +++ examples/foldMatrixAssembly.ts | 35 ++++++++ package.json | 2 +- src/index.ts | 7 ++ src/physics/fold/index.ts | 1 + src/physics/fold/matrixAssembly.ts | 140 +++++++++++++++++++++++++++++ tests/index.test.ts | 2 + tests/matrixAssembly.test.ts | 77 ++++++++++++++++ 9 files changed, 277 insertions(+), 2 deletions(-) create mode 100644 examples/foldMatrixAssembly.ts create mode 100644 src/physics/fold/matrixAssembly.ts create mode 100644 tests/matrixAssembly.test.ts diff --git a/ROADMAP.md b/ROADMAP.md index 70e52a7..d8cdd4d 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -132,7 +132,7 @@ - [ ] Linear solver pipeline (PCG with 3x3 block-Jacobi preconditioner) - **Contact and friction infrastructure** - [x] Friction potential tied to contact force magnitude - - [ ] Matrix assembly with cached contact index tables + - [x] Matrix assembly with cached contact index tables - [ ] Gap evaluators for point/triangle, edge/edge, and wall constraints - [ ] SPD enforcement pass for elasticity Hessian blocks diff --git a/docs/index.d.ts b/docs/index.d.ts index 630c62f..142f345 100644 --- a/docs/index.d.ts +++ b/docs/index.d.ts @@ -124,6 +124,7 @@ export const examples: { readonly createWallBarrier: 'examples/foldWallBarrier.ts'; readonly createStrainBarrier: 'examples/foldStrainBarrier.ts'; readonly createFrictionPotential: 'examples/foldFriction.ts'; + readonly assembleContactMatrix: 'examples/foldMatrixAssembly.ts'; }; readonly performance: { readonly debounce: 'examples/requestDedup.ts'; @@ -3404,6 +3405,18 @@ export interface FrictionOptions { } export function createFrictionPotential(options?: FrictionOptions): FoldConstraint; +/** + * Assemble contact matrices with cached index tables. + * Use for: efficient Fold barrier matrix assembly with deterministic block reuse. + * Import: physics/fold/matrixAssembly.ts + */ +export interface ContactBlock { constraintId: string; matrix: Matrix3x3 } +export interface ContactAssemblyInput { contactId: string; blocks: ReadonlyArray } +export interface MatrixAssemblyOptions { size?: number; symmetry?: boolean } +export interface CachedAssembly { readonly contactId: string; readonly baseIndices: number[] } +export interface MatrixAssemblyResult { matrix: number[][]; cache: CachedAssembly[] } +export function assembleContactMatrix(entries: ReadonlyArray, options?: MatrixAssemblyOptions): MatrixAssemblyResult; + export type FoldConstraintType = | 'cubic-barrier' | 'contact-barrier' diff --git a/examples/foldMatrixAssembly.ts b/examples/foldMatrixAssembly.ts new file mode 100644 index 0000000..d0e721c --- /dev/null +++ b/examples/foldMatrixAssembly.ts @@ -0,0 +1,35 @@ +import { assembleContactMatrix } from '../src/index.js'; + +const result = assembleContactMatrix( + [ + { + contactId: 'contact-1', + blocks: [ + { + constraintId: 'c1', + matrix: [ + [1, 0, 0], + [0, 1, 0], + [0, 0, 1], + ], + }, + ], + }, + { + contactId: 'contact-2', + blocks: [ + { + constraintId: 'c2', + matrix: [ + [2, 0, 0], + [0, 2, 0], + [0, 0, 2], + ], + }, + ], + }, + ], + { size: 9 } +); + +console.log(result.matrix.length, result.cache.length); diff --git a/package.json b/package.json index f9d18df..f7ff836 100644 --- a/package.json +++ b/package.json @@ -62,7 +62,7 @@ { "name": "bundle", "path": "dist/index.js", - "limit": "46 KB" + "limit": "48 KB" } ] } diff --git a/src/index.ts b/src/index.ts index 9a7ee83..5013529 100644 --- a/src/index.ts +++ b/src/index.ts @@ -122,6 +122,7 @@ export const examples = { createWallBarrier: 'examples/foldWallBarrier.ts', createStrainBarrier: 'examples/foldStrainBarrier.ts', createFrictionPotential: 'examples/foldFriction.ts', + assembleContactMatrix: 'examples/foldMatrixAssembly.ts', }, performance: { debounce: 'examples/requestDedup.ts', @@ -1203,6 +1204,7 @@ export { createWallBarrier, createStrainBarrier, createFrictionPotential, + assembleContactMatrix, } from './physics/fold/index.js'; export type { @@ -1223,6 +1225,11 @@ export type { WallBarrierOptions, StrainBarrierOptions, FrictionOptions, + ContactAssemblyInput, + ContactBlock, + MatrixAssemblyOptions, + MatrixAssemblyResult, + CachedAssembly, } from './physics/fold/index.js'; // ============================================================================ diff --git a/src/physics/fold/index.ts b/src/physics/fold/index.ts index 24db815..b793575 100644 --- a/src/physics/fold/index.ts +++ b/src/physics/fold/index.ts @@ -6,3 +6,4 @@ export * from './pinBarrier.js'; export * from './wallBarrier.js'; export * from './strainBarrier.js'; export * from './frictionPotential.js'; +export * from './matrixAssembly.js'; diff --git a/src/physics/fold/matrixAssembly.ts b/src/physics/fold/matrixAssembly.ts new file mode 100644 index 0000000..e765422 --- /dev/null +++ b/src/physics/fold/matrixAssembly.ts @@ -0,0 +1,140 @@ +import type { Matrix3x3 } from '../../types.js'; + +export interface ContactBlock { + constraintId: string; + matrix: Matrix3x3; +} + +export interface ContactAssemblyInput { + contactId: string; + blocks: ReadonlyArray; +} + +export interface CachedAssembly { + readonly contactId: string; + readonly baseIndices: number[]; +} + +export interface MatrixAssemblyOptions { + size?: number; + symmetry?: boolean; +} + +export interface MatrixAssemblyResult { + matrix: number[][]; + cache: CachedAssembly[]; +} + +export function assembleContactMatrix( + entries: ReadonlyArray, + options: MatrixAssemblyOptions = {} +): MatrixAssemblyResult { + const allocator = new IndexAllocator(options.size); + const symmetry = options.symmetry ?? true; + const cache: CachedAssembly[] = []; + + for (const entry of entries) { + if (!entry || typeof entry.contactId !== 'string') { + continue; + } + + const baseIndices: number[] = []; + for (const block of entry.blocks ?? []) { + const baseIndex = allocator.allocate(block); + baseIndices.push(baseIndex); + insertBlock(allocator.matrix, block.matrix, baseIndex, symmetry); + } + + cache.push({ contactId: entry.contactId, baseIndices }); + } + + const matrix = allocator.finalise(); + return { matrix, cache }; +} + +class IndexAllocator { + public readonly matrix: number[][]; + private readonly assignments = new Map(); + private cursor = 0; + private readonly size: number; + + constructor(sizeOverride?: number) { + this.size = sizeOverride ?? 0; + this.matrix = createZeroMatrix(this.size); + } + + allocate(block: ContactBlock): number { + if (!block || typeof block.constraintId !== 'string') { + throw new TypeError('Each block must have a constraintId.'); + } + const existing = this.assignments.get(block.constraintId); + const blockSize = block.matrix.length; + + if (existing) { + if (existing.size !== blockSize) { + throw new Error(`Mismatched block size for constraint ${block.constraintId}.`); + } + return existing.index; + } + + this.ensureCapacity(this.cursor + blockSize); + const index = this.cursor; + this.assignments.set(block.constraintId, { index, size: blockSize }); + this.cursor += blockSize; + return index; + } + + private ensureCapacity(required: number): void { + if (required <= this.matrix.length) { + return; + } + const newSize = Math.max(required, this.matrix.length * 2 || required); + resizeMatrix(this.matrix, newSize); + } + + finalise(): number[][] { + const targetSize = this.size || this.cursor; + const finalSize = Math.max(targetSize, this.cursor); + resizeMatrix(this.matrix, finalSize); + return this.matrix.slice(0, finalSize).map((row) => row.slice(0, finalSize)); + } +} + +function insertBlock(matrix: number[][], block: Matrix3x3, baseIndex: number, symmetry: boolean): void { + for (let i = 0; i < block.length; i += 1) { + const row = block[i]; + if (!row) continue; + const rowIndex = baseIndex + i; + const targetRow = matrix[rowIndex]; + if (!targetRow) continue; + + for (let j = 0; j < row.length; j += 1) { + const value = row[j] ?? 0; + if (value === 0) continue; + const colIndex = baseIndex + j; + const targetColRow = matrix[colIndex]; + targetRow[colIndex] = (targetRow[colIndex] ?? 0) + value; + if (symmetry && rowIndex !== colIndex && targetColRow) { + targetColRow[rowIndex] = (targetColRow[rowIndex] ?? 0) + value; + } + } + } +} + +function createZeroMatrix(size: number): number[][] { + return Array.from({ length: size }, () => Array.from({ length: size }, () => 0)); +} + +function resizeMatrix(matrix: number[][], newSize: number): void { + const current = matrix.length; + for (let i = 0; i < current; i += 1) { + const row = matrix[i]; + row.length = newSize; + for (let j = current; j < newSize; j += 1) { + row[j] = 0; + } + } + for (let i = current; i < newSize; i += 1) { + matrix.push(Array.from({ length: newSize }, () => 0)); + } +} diff --git a/tests/index.test.ts b/tests/index.test.ts index f1b38e9..2a2e74d 100644 --- a/tests/index.test.ts +++ b/tests/index.test.ts @@ -65,6 +65,7 @@ describe('package entry point', () => { expect(examples.physics.createWallBarrier).toBe('examples/foldWallBarrier.ts'); expect(examples.physics.createStrainBarrier).toBe('examples/foldStrainBarrier.ts'); expect(examples.physics.createFrictionPotential).toBe('examples/foldFriction.ts'); + expect(examples.physics.assembleContactMatrix).toBe('examples/foldMatrixAssembly.ts'); }); it('provides strong typing for example categories and names', () => { @@ -213,6 +214,7 @@ describe('package entry point', () => { | 'createWallBarrier' | 'createStrainBarrier' | 'createFrictionPotential' + | 'assembleContactMatrix' >(); expectTypeOf>().toEqualTypeOf< diff --git a/tests/matrixAssembly.test.ts b/tests/matrixAssembly.test.ts new file mode 100644 index 0000000..5a6c7c9 --- /dev/null +++ b/tests/matrixAssembly.test.ts @@ -0,0 +1,77 @@ +import { describe, expect, it } from 'vitest'; + +import { assembleContactMatrix } from '../src/physics/fold/matrixAssembly.js'; + +describe('assembleContactMatrix', () => { + it('allocates sequential blocks and caches indices', () => { + const result = assembleContactMatrix( + [ + { + contactId: 'contact-1', + blocks: [ + { + constraintId: 'c1', + matrix: [ + [1, 0, 0], + [0, 1, 0], + [0, 0, 1], + ], + }, + { + constraintId: 'c2', + matrix: [ + [2, 0, 0], + [0, 2, 0], + [0, 0, 2], + ], + }, + ], + }, + ], + { size: 12 } + ); + + expect(result.matrix.length).toBe(12); + expect(result.cache).toHaveLength(1); + expect(result.cache[0]?.baseIndices).toHaveLength(2); + expect(result.matrix[0]?.[0]).toBe(1); + expect(result.matrix[3]?.[3]).toBe(2); + expect(result.matrix[3]?.[0]).toBe(0); + }); + + it('reuses cached indices for repeated constraints', () => { + const result = assembleContactMatrix( + [ + { + contactId: 'contact-1', + blocks: [ + { + constraintId: 'shared', + matrix: [ + [1, 0, 0], + [0, 1, 0], + [0, 0, 1], + ], + }, + ], + }, + { + contactId: 'contact-2', + blocks: [ + { + constraintId: 'shared', + matrix: [ + [0.5, 0, 0], + [0, 0.5, 0], + [0, 0, 0.5], + ], + }, + ], + }, + ] + ); + + expect(result.matrix.length).toBeGreaterThan(0); + expect(result.cache[0]?.baseIndices[0]).toBe(result.cache[1]?.baseIndices[0]); + }); +});