|
| 1 | +import type { Matrix3x3 } from '../../types.js'; |
| 2 | + |
| 3 | +export interface ContactBlock { |
| 4 | + constraintId: string; |
| 5 | + matrix: Matrix3x3; |
| 6 | +} |
| 7 | + |
| 8 | +export interface ContactAssemblyInput { |
| 9 | + contactId: string; |
| 10 | + blocks: ReadonlyArray<ContactBlock>; |
| 11 | +} |
| 12 | + |
| 13 | +export interface CachedAssembly { |
| 14 | + readonly contactId: string; |
| 15 | + readonly baseIndices: number[]; |
| 16 | +} |
| 17 | + |
| 18 | +export interface MatrixAssemblyOptions { |
| 19 | + size?: number; |
| 20 | + symmetry?: boolean; |
| 21 | +} |
| 22 | + |
| 23 | +export interface MatrixAssemblyResult { |
| 24 | + matrix: number[][]; |
| 25 | + cache: CachedAssembly[]; |
| 26 | +} |
| 27 | + |
| 28 | +export function assembleContactMatrix( |
| 29 | + entries: ReadonlyArray<ContactAssemblyInput>, |
| 30 | + options: MatrixAssemblyOptions = {} |
| 31 | +): MatrixAssemblyResult { |
| 32 | + const allocator = new IndexAllocator(options.size); |
| 33 | + const symmetry = options.symmetry ?? true; |
| 34 | + const cache: CachedAssembly[] = []; |
| 35 | + |
| 36 | + for (const entry of entries) { |
| 37 | + if (!entry || typeof entry.contactId !== 'string') { |
| 38 | + continue; |
| 39 | + } |
| 40 | + |
| 41 | + const baseIndices: number[] = []; |
| 42 | + for (const block of entry.blocks ?? []) { |
| 43 | + const baseIndex = allocator.allocate(block); |
| 44 | + baseIndices.push(baseIndex); |
| 45 | + insertBlock(allocator.matrix, block.matrix, baseIndex, symmetry); |
| 46 | + } |
| 47 | + |
| 48 | + cache.push({ contactId: entry.contactId, baseIndices }); |
| 49 | + } |
| 50 | + |
| 51 | + const matrix = allocator.finalise(); |
| 52 | + return { matrix, cache }; |
| 53 | +} |
| 54 | + |
| 55 | +class IndexAllocator { |
| 56 | + public readonly matrix: number[][]; |
| 57 | + private readonly assignments = new Map<string, { index: number; size: number }>(); |
| 58 | + private cursor = 0; |
| 59 | + private readonly size: number; |
| 60 | + |
| 61 | + constructor(sizeOverride?: number) { |
| 62 | + this.size = sizeOverride ?? 0; |
| 63 | + this.matrix = createZeroMatrix(this.size); |
| 64 | + } |
| 65 | + |
| 66 | + allocate(block: ContactBlock): number { |
| 67 | + if (!block || typeof block.constraintId !== 'string') { |
| 68 | + throw new TypeError('Each block must have a constraintId.'); |
| 69 | + } |
| 70 | + const existing = this.assignments.get(block.constraintId); |
| 71 | + const blockSize = block.matrix.length; |
| 72 | + |
| 73 | + if (existing) { |
| 74 | + if (existing.size !== blockSize) { |
| 75 | + throw new Error(`Mismatched block size for constraint ${block.constraintId}.`); |
| 76 | + } |
| 77 | + return existing.index; |
| 78 | + } |
| 79 | + |
| 80 | + this.ensureCapacity(this.cursor + blockSize); |
| 81 | + const index = this.cursor; |
| 82 | + this.assignments.set(block.constraintId, { index, size: blockSize }); |
| 83 | + this.cursor += blockSize; |
| 84 | + return index; |
| 85 | + } |
| 86 | + |
| 87 | + private ensureCapacity(required: number): void { |
| 88 | + if (required <= this.matrix.length) { |
| 89 | + return; |
| 90 | + } |
| 91 | + const newSize = Math.max(required, this.matrix.length * 2 || required); |
| 92 | + resizeMatrix(this.matrix, newSize); |
| 93 | + } |
| 94 | + |
| 95 | + finalise(): number[][] { |
| 96 | + const targetSize = this.size || this.cursor; |
| 97 | + const finalSize = Math.max(targetSize, this.cursor); |
| 98 | + resizeMatrix(this.matrix, finalSize); |
| 99 | + return this.matrix.slice(0, finalSize).map((row) => row.slice(0, finalSize)); |
| 100 | + } |
| 101 | +} |
| 102 | + |
| 103 | +function insertBlock(matrix: number[][], block: Matrix3x3, baseIndex: number, symmetry: boolean): void { |
| 104 | + for (let i = 0; i < block.length; i += 1) { |
| 105 | + const row = block[i]; |
| 106 | + if (!row) continue; |
| 107 | + const rowIndex = baseIndex + i; |
| 108 | + const targetRow = matrix[rowIndex]; |
| 109 | + if (!targetRow) continue; |
| 110 | + |
| 111 | + for (let j = 0; j < row.length; j += 1) { |
| 112 | + const value = row[j] ?? 0; |
| 113 | + if (value === 0) continue; |
| 114 | + const colIndex = baseIndex + j; |
| 115 | + const targetColRow = matrix[colIndex]; |
| 116 | + targetRow[colIndex] = (targetRow[colIndex] ?? 0) + value; |
| 117 | + if (symmetry && rowIndex !== colIndex && targetColRow) { |
| 118 | + targetColRow[rowIndex] = (targetColRow[rowIndex] ?? 0) + value; |
| 119 | + } |
| 120 | + } |
| 121 | + } |
| 122 | +} |
| 123 | + |
| 124 | +function createZeroMatrix(size: number): number[][] { |
| 125 | + return Array.from({ length: size }, () => Array.from({ length: size }, () => 0)); |
| 126 | +} |
| 127 | + |
| 128 | +function resizeMatrix(matrix: number[][], newSize: number): void { |
| 129 | + const current = matrix.length; |
| 130 | + for (let i = 0; i < current; i += 1) { |
| 131 | + const row = matrix[i]; |
| 132 | + row.length = newSize; |
| 133 | + for (let j = current; j < newSize; j += 1) { |
| 134 | + row[j] = 0; |
| 135 | + } |
| 136 | + } |
| 137 | + for (let i = current; i < newSize; i += 1) { |
| 138 | + matrix.push(Array.from({ length: newSize }, () => 0)); |
| 139 | + } |
| 140 | +} |
0 commit comments