|
| 1 | +import type { Vector3D, Matrix3x3 } from '../../types.js'; |
| 2 | +import type { |
| 3 | + FoldComputationContext, |
| 4 | + FoldConstraint, |
| 5 | + FoldConstraintEvaluation, |
| 6 | + FoldConstraintState, |
| 7 | +} from './types.js'; |
| 8 | +import { createCubicBarrier } from './cubicBarrier.js'; |
| 9 | +import { computeFrozenStiffness } from './stiffness.js'; |
| 10 | + |
| 11 | +export interface WallBarrierOptions { |
| 12 | + id?: string; |
| 13 | + stiffnessOverride?: number; |
| 14 | + maxGap?: number; |
| 15 | + normal?: Vector3D; |
| 16 | + planePoint?: Vector3D; |
| 17 | +} |
| 18 | + |
| 19 | +const ZERO_GRADIENT: Vector3D = { x: 0, y: 0, z: 0 }; |
| 20 | +const ZERO_HESSIAN: Matrix3x3 = [ |
| 21 | + [0, 0, 0], |
| 22 | + [0, 0, 0], |
| 23 | + [0, 0, 0], |
| 24 | +]; |
| 25 | + |
| 26 | +export function createWallBarrier(options: WallBarrierOptions = {}): FoldConstraint { |
| 27 | + const baseBarrier = createCubicBarrier({ |
| 28 | + id: options.id, |
| 29 | + maxGap: options.maxGap, |
| 30 | + direction: options.normal, |
| 31 | + }); |
| 32 | + |
| 33 | + return { |
| 34 | + type: 'wall-barrier', |
| 35 | + id: options.id, |
| 36 | + enabled: true, |
| 37 | + evaluate(state: FoldConstraintState, context: FoldComputationContext): FoldConstraintEvaluation { |
| 38 | + const wallNormal = normalise(options.normal ?? state.direction); |
| 39 | + if (!wallNormal) { |
| 40 | + return { energy: 0, gradient: ZERO_GRADIENT, hessian: ZERO_HESSIAN }; |
| 41 | + } |
| 42 | + |
| 43 | + const adjustedGap = adjustGap(state, wallNormal, options.planePoint); |
| 44 | + |
| 45 | + const stiffness = options.stiffnessOverride ?? |
| 46 | + computeFrozenStiffness( |
| 47 | + { |
| 48 | + gap: adjustedGap, |
| 49 | + effectiveMass: state.effectiveMass ?? 0, |
| 50 | + direction: wallNormal, |
| 51 | + hessian: (state.metadata?.hessian as Matrix3x3 | undefined) ?? ZERO_HESSIAN, |
| 52 | + }, |
| 53 | + { min: 0 } |
| 54 | + ); |
| 55 | + |
| 56 | + if (stiffness <= 0) { |
| 57 | + return { energy: 0, gradient: ZERO_GRADIENT, hessian: ZERO_HESSIAN }; |
| 58 | + } |
| 59 | + |
| 60 | + const evaluation = baseBarrier.evaluate( |
| 61 | + { |
| 62 | + ...state, |
| 63 | + gap: adjustedGap, |
| 64 | + direction: wallNormal, |
| 65 | + stiffness, |
| 66 | + maxGap: options.maxGap ?? state.maxGap, |
| 67 | + }, |
| 68 | + context |
| 69 | + ); |
| 70 | + |
| 71 | + return evaluation; |
| 72 | + }, |
| 73 | + }; |
| 74 | +} |
| 75 | + |
| 76 | +function adjustGap( |
| 77 | + state: FoldConstraintState, |
| 78 | + normal: Vector3D, |
| 79 | + planePoint: Vector3D | undefined |
| 80 | +): number { |
| 81 | + if (!planePoint) { |
| 82 | + return state.gap; |
| 83 | + } |
| 84 | + |
| 85 | + const position = state.metadata?.position as Vector3D | undefined; |
| 86 | + if (!position) { |
| 87 | + return state.gap; |
| 88 | + } |
| 89 | + |
| 90 | + const offsetVector = { |
| 91 | + x: position.x - planePoint.x, |
| 92 | + y: position.y - planePoint.y, |
| 93 | + z: position.z - planePoint.z, |
| 94 | + }; |
| 95 | + const signedDistance = dot(offsetVector, normal); |
| 96 | + return Math.min(state.gap, signedDistance); |
| 97 | +} |
| 98 | + |
| 99 | +function normalise(vector: Vector3D | undefined): Vector3D | null { |
| 100 | + if (!vector) return null; |
| 101 | + const length = Math.hypot(vector.x, vector.y, vector.z); |
| 102 | + if (length === 0) return null; |
| 103 | + return { |
| 104 | + x: vector.x / length, |
| 105 | + y: vector.y / length, |
| 106 | + z: vector.z / length, |
| 107 | + }; |
| 108 | +} |
| 109 | + |
| 110 | +function dot(a: Vector3D, b: Vector3D): number { |
| 111 | + return a.x * b.x + a.y * b.y + a.z * b.z; |
| 112 | +} |
0 commit comments