From bf47272758a772e1cb3501d94b0cee6b65217328 Mon Sep 17 00:00:00 2001 From: Francois Date: Sun, 19 Oct 2025 18:58:32 +0900 Subject: [PATCH] feat(physics): add gap evaluators --- ROADMAP.md | 2 +- docs/index.d.ts | 27 ++++ examples/foldGapEvaluators.ts | 33 +++++ src/index.ts | 9 ++ src/physics/fold/gapEvaluators.ts | 207 ++++++++++++++++++++++++++++++ src/physics/fold/index.ts | 1 + tests/gapEvaluators.test.ts | 51 ++++++++ tests/index.test.ts | 6 + 8 files changed, 335 insertions(+), 1 deletion(-) create mode 100644 examples/foldGapEvaluators.ts create mode 100644 src/physics/fold/gapEvaluators.ts create mode 100644 tests/gapEvaluators.test.ts diff --git a/ROADMAP.md b/ROADMAP.md index d8cdd4d..90342fa 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -133,7 +133,7 @@ - **Contact and friction infrastructure** - [x] Friction potential tied to contact force magnitude - [x] Matrix assembly with cached contact index tables - - [ ] Gap evaluators for point/triangle, edge/edge, and wall constraints + - [x] Gap evaluators for point/triangle, edge/edge, and wall constraints - [ ] 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 142f345..927d9f5 100644 --- a/docs/index.d.ts +++ b/docs/index.d.ts @@ -125,6 +125,9 @@ export const examples: { readonly createStrainBarrier: 'examples/foldStrainBarrier.ts'; readonly createFrictionPotential: 'examples/foldFriction.ts'; readonly assembleContactMatrix: 'examples/foldMatrixAssembly.ts'; + readonly computePointTriangleGap: 'examples/foldGapEvaluators.ts'; + readonly computeEdgeEdgeGap: 'examples/foldGapEvaluators.ts'; + readonly computePointPlaneGap: 'examples/foldGapEvaluators.ts'; }; readonly performance: { readonly debounce: 'examples/requestDedup.ts'; @@ -3417,6 +3420,30 @@ export interface CachedAssembly { readonly contactId: string; readonly baseIndic export interface MatrixAssemblyResult { matrix: number[][]; cache: CachedAssembly[] } export function assembleContactMatrix(entries: ReadonlyArray, options?: MatrixAssemblyOptions): MatrixAssemblyResult; +/** + * Point-triangle gap with closest point and normal. + * Use for: character contacts, Fold barrier estimation. + * Import: physics/fold/gapEvaluators.ts + */ +export interface PointTriangleGapResult { gap: number; closestPoint: Point3D; normal: Vector3D } +export function computePointTriangleGap(point: Point3D, triangle: readonly [Point3D, Point3D, Point3D]): PointTriangleGapResult; + +/** + * Edge-edge gap with closest points. + * Use for: edge-edge collision in Fold contacts. + * Import: physics/fold/gapEvaluators.ts + */ +export interface EdgeEdgeGapResult { gap: number; closestPointA: Point3D; closestPointB: Point3D; normal: Vector3D } +export function computeEdgeEdgeGap(edgeA: readonly [Point3D, Point3D], edgeB: readonly [Point3D, Point3D]): EdgeEdgeGapResult; + +/** + * Point-plane gap helper with projection. + * Use for: wall contacts, Fold barrier plane constraints. + * Import: physics/fold/gapEvaluators.ts + */ +export interface PointPlaneGapResult { gap: number; projectedPoint: Point3D; normal: Vector3D } +export function computePointPlaneGap(point: Point3D, planePoint: Point3D, planeNormal: Vector3D): PointPlaneGapResult; + export type FoldConstraintType = | 'cubic-barrier' | 'contact-barrier' diff --git a/examples/foldGapEvaluators.ts b/examples/foldGapEvaluators.ts new file mode 100644 index 0000000..96a434c --- /dev/null +++ b/examples/foldGapEvaluators.ts @@ -0,0 +1,33 @@ +import { computePointTriangleGap, computeEdgeEdgeGap, computePointPlaneGap } from '../src/index.js'; + +const pointTriangle = computePointTriangleGap( + { x: 0.1, y: 0.2, z: 0.3 }, + [ + { x: 0, y: 0, z: 0 }, + { x: 1, y: 0, z: 0 }, + { x: 0, y: 1, z: 0 }, + ] +); + +console.log('point-triangle gap', pointTriangle.gap); + +const edgeEdge = computeEdgeEdgeGap( + [ + { x: 0, y: 0, z: 0 }, + { x: 1, y: 0, z: 0 }, + ], + [ + { x: 0, y: 0, z: 1 }, + { x: 1, y: 0, z: 1 }, + ] +); + +console.log('edge-edge gap', edgeEdge.gap); + +const pointPlane = computePointPlaneGap( + { x: 0, y: 0.5, z: 0 }, + { x: 0, y: 0, z: 0 }, + { x: 0, y: 1, z: 0 } +); + +console.log('point-plane gap', pointPlane.gap); diff --git a/src/index.ts b/src/index.ts index 5013529..6c3cdd1 100644 --- a/src/index.ts +++ b/src/index.ts @@ -123,6 +123,9 @@ export const examples = { createStrainBarrier: 'examples/foldStrainBarrier.ts', createFrictionPotential: 'examples/foldFriction.ts', assembleContactMatrix: 'examples/foldMatrixAssembly.ts', + computePointTriangleGap: 'examples/foldGapEvaluators.ts', + computeEdgeEdgeGap: 'examples/foldGapEvaluators.ts', + computePointPlaneGap: 'examples/foldGapEvaluators.ts', }, performance: { debounce: 'examples/requestDedup.ts', @@ -1205,6 +1208,9 @@ export { createStrainBarrier, createFrictionPotential, assembleContactMatrix, + computePointTriangleGap, + computeEdgeEdgeGap, + computePointPlaneGap, } from './physics/fold/index.js'; export type { @@ -1230,6 +1236,9 @@ export type { MatrixAssemblyOptions, MatrixAssemblyResult, CachedAssembly, + PointTriangleGapResult, + EdgeEdgeGapResult, + PointPlaneGapResult, } from './physics/fold/index.js'; // ============================================================================ diff --git a/src/physics/fold/gapEvaluators.ts b/src/physics/fold/gapEvaluators.ts new file mode 100644 index 0000000..d2aa7fe --- /dev/null +++ b/src/physics/fold/gapEvaluators.ts @@ -0,0 +1,207 @@ +import type { Point3D, Vector3D } from '../../types.js'; + +export interface PointTriangleGapResult { + gap: number; + closestPoint: Point3D; + normal: Vector3D; +} + +export interface EdgeEdgeGapResult { + gap: number; + closestPointA: Point3D; + closestPointB: Point3D; + normal: Vector3D; +} + +export interface PointPlaneGapResult { + gap: number; + projectedPoint: Point3D; + normal: Vector3D; +} + +export function computePointTriangleGap(point: Point3D, triangle: readonly [Point3D, Point3D, Point3D]): PointTriangleGapResult { + validatePoint(point); + triangle.forEach(validatePoint); + + const [a, b, c] = triangle; + const ab = subtract(b, a); + const ac = subtract(c, a); + const ap = subtract(point, a); + const normal = normalise(cross(ab, ac)) ?? { x: 0, y: 0, z: 1 }; + + const projection = dot(ap, normal); + const projectedPoint = subtract(point, scale(normal, projection)); + const barycentric = barycentricCoordinates(projectedPoint, a, b, c); + + let closestPoint: Point3D; + if (isInsideBarycentric(barycentric)) { + closestPoint = projectedPoint; + } else { + closestPoint = closestPointOnEdges(projectedPoint, triangle); + } + + const gap = projection; + return { gap, closestPoint, normal }; +} + +export function computeEdgeEdgeGap(edgeA: readonly [Point3D, Point3D], edgeB: readonly [Point3D, Point3D]): EdgeEdgeGapResult { + edgeA.forEach(validatePoint); + edgeB.forEach(validatePoint); + + const [p1, q1] = edgeA; + const [p2, q2] = edgeB; + const d1 = subtract(q1, p1); + const d2 = subtract(q2, p2); + const r = subtract(p1, p2); + const a = dot(d1, d1); + const e = dot(d2, d2); + const f = dot(d2, r); + let s: number; + let t: number; + + const EPS = 1e-8; + if (a <= EPS && e <= EPS) { + s = t = 0; + } else if (a <= EPS) { + s = 0; + t = clamp(f / e, 0, 1); + } else { + const c = dot(d1, r); + if (e <= EPS) { + t = 0; + s = clamp(-c / a, 0, 1); + } else { + const b = dot(d1, d2); + const denom = a * e - b * b; + if (denom !== 0) { + s = clamp((b * f - c * e) / denom, 0, 1); + } else { + s = 0; + } + t = (b * s + f) / e; + if (t < 0) { + t = 0; + s = clamp(-c / a, 0, 1); + } else if (t > 1) { + t = 1; + s = clamp((b - c) / a, 0, 1); + } + } + } + + const closestA = add(p1, scale(d1, s)); + const closestB = add(p2, scale(d2, t)); + const diff = subtract(closestB, closestA); + const normal = normalise(diff) ?? { x: 0, y: 0, z: 1 }; + const gap = dot(diff, normal); + return { gap, closestPointA: closestA, closestPointB: closestB, normal }; +} + +export function computePointPlaneGap(point: Point3D, planePoint: Point3D, planeNormal: Vector3D): PointPlaneGapResult { + validatePoint(point); + validatePoint(planePoint); + validateVector(planeNormal); + + const normal = normalise(planeNormal) ?? { x: 0, y: 0, z: 1 }; + const diff = subtract(point, planePoint); + const gap = dot(diff, normal); + const projectedPoint = subtract(point, scale(normal, gap)); + return { gap, projectedPoint, normal }; +} + +function validatePoint(point: Point3D | undefined): asserts point is Point3D { + if (!point || !Number.isFinite(point.x) || !Number.isFinite(point.y) || !Number.isFinite(point.z)) { + throw new TypeError('Point3D must contain finite x, y, z values.'); + } +} + +function validateVector(vector: Vector3D | undefined): asserts vector is Vector3D { + if (!vector || !Number.isFinite(vector.x) || !Number.isFinite(vector.y) || !Number.isFinite(vector.z)) { + throw new TypeError('Vector3D must contain finite x, y, z values.'); + } +} + +function subtract(a: Point3D, b: Point3D): Vector3D { + return { x: a.x - b.x, y: a.y - b.y, z: a.z - b.z }; +} + +function add(a: Point3D, b: Vector3D): Point3D { + return { x: a.x + b.x, y: a.y + b.y, z: a.z + b.z }; +} + +function scale(v: Vector3D, s: number): Vector3D { + return { x: v.x * s, y: v.y * s, z: v.z * s }; +} + +function dot(a: Vector3D, b: Vector3D): number { + return a.x * b.x + a.y * b.y + a.z * b.z; +} + +function cross(a: Vector3D, b: Vector3D): Vector3D { + return { + x: a.y * b.z - a.z * b.y, + y: a.z * b.x - a.x * b.z, + z: a.x * b.y - a.y * b.x, + }; +} + +function normalise(v: Vector3D): Vector3D | null { + const length = Math.hypot(v.x, v.y, v.z); + if (length === 0) return null; + return { x: v.x / length, y: v.y / length, z: v.z / length }; +} + +function barycentricCoordinates(p: Point3D, a: Point3D, b: Point3D, c: Point3D): { u: number; v: number; w: number } { + const v0 = subtract(b, a); + const v1 = subtract(c, a); + const v2 = subtract(p, a); + const d00 = dot(v0, v0); + const d01 = dot(v0, v1); + const d11 = dot(v1, v1); + const d20 = dot(v2, v0); + const d21 = dot(v2, v1); + const denom = d00 * d11 - d01 * d01; + if (denom === 0) { + return { u: -1, v: -1, w: -1 }; + } + const v = (d11 * d20 - d01 * d21) / denom; + const w = (d00 * d21 - d01 * d20) / denom; + const u = 1 - v - w; + return { u, v, w }; +} + +function isInsideBarycentric({ u, v, w }: { u: number; v: number; w: number }): boolean { + const EPS = -1e-8; + return u >= EPS && v >= EPS && w >= EPS; +} + +function closestPointOnEdges(point: Point3D, triangle: readonly [Point3D, Point3D, Point3D]): Point3D { + const edges: Array<[Point3D, Point3D]> = [ + [triangle[0], triangle[1]], + [triangle[1], triangle[2]], + [triangle[2], triangle[0]], + ]; + let closest = edges[0]?.[0] ?? point; + let minDistance = Number.POSITIVE_INFINITY; + for (const [start, end] of edges) { + const candidate = closestPointSegment(point, start, end); + const distance = Math.hypot(candidate.x - point.x, candidate.y - point.y, candidate.z - point.z); + if (distance < minDistance) { + minDistance = distance; + closest = candidate; + } + } + return closest; +} + +function closestPointSegment(point: Point3D, start: Point3D, end: Point3D): Point3D { + const ab = subtract(end, start); + const t = clamp(dot(subtract(point, start), ab) / dot(ab, ab), 0, 1); + return add(start, scale(ab, t)); +} + +function clamp(value: number, min: number, max: number): number { + if (value < min) return min; + if (value > max) return max; + return value; +} diff --git a/src/physics/fold/index.ts b/src/physics/fold/index.ts index b793575..c1d0d68 100644 --- a/src/physics/fold/index.ts +++ b/src/physics/fold/index.ts @@ -7,3 +7,4 @@ export * from './wallBarrier.js'; export * from './strainBarrier.js'; export * from './frictionPotential.js'; export * from './matrixAssembly.js'; +export * from './gapEvaluators.js'; diff --git a/tests/gapEvaluators.test.ts b/tests/gapEvaluators.test.ts new file mode 100644 index 0000000..3b1c8cd --- /dev/null +++ b/tests/gapEvaluators.test.ts @@ -0,0 +1,51 @@ +import { describe, expect, it } from 'vitest'; + +import { + computePointTriangleGap, + computeEdgeEdgeGap, + computePointPlaneGap, +} from '../src/physics/fold/gapEvaluators.js'; + +describe('gap evaluators', () => { + it('computes point-triangle gap with inside projection', () => { + const result = computePointTriangleGap( + { x: 0.1, y: 0.1, z: 0.5 }, + [ + { x: 0, y: 0, z: 0 }, + { x: 1, y: 0, z: 0 }, + { x: 0, y: 1, z: 0 }, + ] + ); + + expect(result.gap).toBeCloseTo(0.5, 6); + expect(result.normal.z).toBeCloseTo(1, 6); + }); + + it('computes edge-edge gap and provides closest points', () => { + const result = computeEdgeEdgeGap( + [ + { x: 0, y: 0, z: 0 }, + { x: 1, y: 0, z: 0 }, + ], + [ + { x: 0, y: 0, z: 1 }, + { x: 1, y: 0, z: 1 }, + ] + ); + + expect(result.gap).toBeCloseTo(1, 6); + expect(result.closestPointA.z).toBeCloseTo(0, 6); + expect(result.closestPointB.z).toBeCloseTo(1, 6); + }); + + it('computes point-plane gap with normalized normal', () => { + const result = computePointPlaneGap( + { x: 0, y: 2, z: 0 }, + { x: 0, y: 0, z: 0 }, + { x: 0, y: 2, z: 0 } + ); + + expect(result.gap).toBeCloseTo(2, 6); + expect(result.projectedPoint.y).toBeCloseTo(0, 6); + }); +}); diff --git a/tests/index.test.ts b/tests/index.test.ts index 2a2e74d..d61e7f8 100644 --- a/tests/index.test.ts +++ b/tests/index.test.ts @@ -66,6 +66,9 @@ describe('package entry point', () => { expect(examples.physics.createStrainBarrier).toBe('examples/foldStrainBarrier.ts'); expect(examples.physics.createFrictionPotential).toBe('examples/foldFriction.ts'); expect(examples.physics.assembleContactMatrix).toBe('examples/foldMatrixAssembly.ts'); + expect(examples.physics.computePointTriangleGap).toBe('examples/foldGapEvaluators.ts'); + expect(examples.physics.computeEdgeEdgeGap).toBe('examples/foldGapEvaluators.ts'); + expect(examples.physics.computePointPlaneGap).toBe('examples/foldGapEvaluators.ts'); }); it('provides strong typing for example categories and names', () => { @@ -215,6 +218,9 @@ describe('package entry point', () => { | 'createStrainBarrier' | 'createFrictionPotential' | 'assembleContactMatrix' + | 'computePointTriangleGap' + | 'computeEdgeEdgeGap' + | 'computePointPlaneGap' >(); expectTypeOf>().toEqualTypeOf<