feat(physics): add contact matrix assembly

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
 

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<ContactBlock> }
+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<ContactAssemblyInput>, options?: MatrixAssemblyOptions): MatrixAssemblyResult;
+
 export type FoldConstraintType =
   | 'cubic-barrier'
   | 'contact-barrier'

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);

package.json

@@ -62,7 +62,7 @@
     {
       "name": "bundle",
       "path": "dist/index.js",
-      "limit": "46 KB"
+      "limit": "48 KB"
     }
   ]
 }

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';
 
 // ============================================================================

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';

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<ContactBlock>;
+}
+
+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<ContactAssemblyInput>,
+  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<string, { index: number; size: number }>();
+  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));
+  }
+}

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<ExampleName<'ai'>>().toEqualTypeOf<

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]);
+  });
+});