Press n or j to go to the next uncovered block, b, p or k for the previous block.
| 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 | 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 6x 6x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 15x 15x 15x 15x 15x 15x 15x 15x 15x 15x 15x 15x 15x 15x 15x 15x 15x 15x 15x 15x 15x 15x 1x 1x 1x 15x 15x 15x 15x 15x 35x 35x 35x 35x 35x 35x 35x 35x 15x 15x 15x 35x 35x 15x 15x 15x 15x 15x 15x 15x 15x 15x 20x 20x 35x 35x 35x 35x 80x 80x 80x 80x 80x 80x 80x 80x 80x 80x 80x 80x 80x 80x 80x 80x 80x 80x 80x 80x 80x 80x 80x 20x 20x 20x 20x 20x 35x 80x 80x 80x 80x 20x 20x 20x 20x 20x 20x 35x 20x 20x 20x 20x 20x 20x 20x 20x 20x 20x 35x 80x 80x 20x 20x 20x 20x 20x 20x 20x 20x 20x 20x 20x 20x 20x 20x 20x 20x 20x 20x 20x 20x 20x 20x 20x 20x 20x 20x 20x 20x 20x 20x 20x 20x 20x 20x 20x 20x 20x 20x 20x 35x 35x 20x 15x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 15x 15x 15x 15x 15x 15x 15x 15x 15x 220x 220x 220x 15x 15x 1x 1x 1x 1x 19x 19x 37x 37x 19x 19x 1x 1x 250x 250x 500x 500x 250x 250x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 1x 4x 4x 4x 4x 4x 4x 4x 4x 4x 4x 4x 4x 1x 1x 1x | /**
* Inverse kinematics solver for Euclidean constructions.
*
* Given a construction recipe and a target position for any derived point,
* solves for the input (given) point positions that place the derived point
* at the target. Uses Levenberg-Marquardt with finite-difference Jacobian.
*
* Key properties:
* - Warm-starting: each frame starts from the previous solution, so convergence
* is typically 1-3 iterations during smooth drag.
* - Minimum displacement: the underdetermined system (2 equations, 4+ unknowns)
* naturally produces the solution closest to the current positions.
* - Graceful degradation: returns the best solution found even if not fully converged.
* - Branch tracking: warm-starting naturally follows the current geometric branch.
*/
import type { Pt, Ref, ConstructionRecipe, RecipeRegistry } from './recipe/types'
import { evaluateRecipe } from './recipe/evaluate'
// ── Public API ─────────────────────────────────────────────────────
/**
* A forward function that maps input positions to the target point's position.
* Returns null if the construction is invalid at the given inputs.
*/
export type ForwardFn = (inputPositions: Pt[]) => Pt | null
export interface InverseSolverOptions {
/** Max LM iterations per frame (default: 5) */
maxIterations?: number
/** Convergence tolerance in world units (default: 1e-4) */
tolerance?: number
/** Initial LM damping parameter (default: 1e-3) */
lambda?: number
/** Finite difference step size (default: 1e-6) */
epsilon?: number
/**
* When true, pre-compute parameter dependencies and skip finite-difference
* evaluations for parameters that don't affect the target point.
* Disabled by default — the Jacobian naturally produces near-zero columns
* for unrelated parameters, and LM leaves them unchanged.
*/
precomputeDependencies?: boolean
}
export interface InverseSolverResult {
/** Solved input positions (same length as recipe.inputSlots) */
inputPositions: Pt[]
/** Whether the solver converged within tolerance */
converged: boolean
/** Number of iterations used */
iterations: number
/** Final residual norm (distance from target) */
residualNorm: number
/** The LM lambda value — pass back as options.lambda for warm-starting */
lambda: number
}
/**
* Persistent solver state for warm-starting across frames.
* Create once per drag gesture, pass to each call.
*/
export interface InverseSolverState {
lambda: number
}
export function createSolverState(): InverseSolverState {
return { lambda: 1e-3 }
}
/**
* Generic inverse solver: given a forward function that maps input positions
* to a target point's position, solve for the input positions that place
* the target point at the desired position.
*
* @param forward - Maps input positions → target point position (null if invalid)
* @param currentInputPositions - Current positions of input (given) points
* @param targetPosition - Where the user wants the target point
* @param solverState - Persistent state for warm-starting lambda across frames
* @param options - Solver tuning parameters
*/
export function solveInverse(
forward: ForwardFn,
currentInputPositions: Pt[],
targetPosition: Pt,
solverState?: InverseSolverState,
options?: InverseSolverOptions
): InverseSolverResult {
const {
maxIterations = 5,
tolerance = 1e-4,
epsilon = 1e-6,
precomputeDependencies = false,
} = options ?? {}
let lambda = solverState?.lambda ?? options?.lambda ?? 1e-3
const n = currentInputPositions.length * 2 // Total parameters (x,y per input point)
// Flatten input positions to parameter vector [x0, y0, x1, y1, ...]
let params = flattenPositions(currentInputPositions)
// Optional: determine which parameters affect the target point
let activeParams: number[] | null = null
if (precomputeDependencies) {
activeParams = findActiveParametersGeneric(forward, currentInputPositions, epsilon)
if (activeParams.length === 0) {
return {
inputPositions: currentInputPositions,
converged: false,
iterations: 0,
residualNorm: Infinity,
lambda,
}
}
}
let bestParams = params
let bestResidualNorm = Infinity
for (let iter = 0; iter < maxIterations; iter++) {
const positions = unflattenPositions(params)
const currentPos = forward(positions)
if (!currentPos) break
const r: [number, number] = [currentPos.x - targetPosition.x, currentPos.y - targetPosition.y]
const residualNorm = Math.sqrt(r[0] * r[0] + r[1] * r[1])
if (residualNorm < bestResidualNorm) {
bestResidualNorm = residualNorm
bestParams = [...params]
}
if (residualNorm < tolerance) {
if (solverState) solverState.lambda = lambda
return {
inputPositions: unflattenPositions(params),
converged: true,
iterations: iter,
residualNorm,
lambda,
}
}
// Compute Jacobian via central finite differences
const paramIndices = activeParams ?? Array.from({ length: n }, (_, i) => i)
const J: [number[], number[]] = [new Array(n).fill(0), new Array(n).fill(0)]
for (const j of paramIndices) {
const paramsPlus = [...params]
const paramsMinus = [...params]
paramsPlus[j] += epsilon
paramsMinus[j] -= epsilon
const posPlus = forward(unflattenPositions(paramsPlus))
const posMinus = forward(unflattenPositions(paramsMinus))
// At a topology boundary one side of the central difference can fall
// off the feasibility manifold (forward returns null). Falling back to
// a one-sided difference preserves the gradient information from the
// valid side, which is what tells LM the direction *into* the
// feasible region. Zeroing the column instead (the previous behavior)
// discarded that signal — the resulting step would then point across
// the boundary, every backtracking alpha would land in null space,
// lambda would saturate, and the dragged point would freeze.
if (posPlus && posMinus) {
J[0][j] = (posPlus.x - posMinus.x) / (2 * epsilon)
J[1][j] = (posPlus.y - posMinus.y) / (2 * epsilon)
} else if (posPlus) {
J[0][j] = (posPlus.x - currentPos.x) / epsilon
J[1][j] = (posPlus.y - currentPos.y) / epsilon
} else if (posMinus) {
J[0][j] = (currentPos.x - posMinus.x) / epsilon
J[1][j] = (currentPos.y - posMinus.y) / epsilon
}
// else both sides null — column stays 0; this parameter is bracketed
// by infeasibility and can't safely move at the current epsilon.
}
// LM update: δ = Jᵀ (J Jᵀ + λI)⁻¹ (-r) — only a 2×2 invert
let jjt00 = 0,
jjt01 = 0,
jjt11 = 0
for (let j = 0; j < n; j++) {
jjt00 += J[0][j] * J[0][j]
jjt01 += J[0][j] * J[1][j]
jjt11 += J[1][j] * J[1][j]
}
jjt00 += lambda
jjt11 += lambda
const jjt10 = jjt01
const det = jjt00 * jjt11 - jjt01 * jjt10
if (Math.abs(det) < 1e-15) {
lambda *= 4
continue
}
const inv00 = jjt11 / det
const inv01 = -jjt01 / det
const inv10 = -jjt10 / det
const inv11 = jjt00 / det
const v0 = inv00 * -r[0] + inv01 * -r[1]
const v1 = inv10 * -r[0] + inv11 * -r[1]
const delta = new Array(n).fill(0)
for (let j = 0; j < n; j++) {
delta[j] = J[0][j] * v0 + J[1][j] * v1
}
// Backtracking line search: try the full LM step first, then shrink
// the step (alpha = 1, 1/2, 1/4, …) if the forward model returns null
// (topology boundary crossed) or the residual fails to decrease. This
// finds the largest feasible step in the LM-prescribed direction —
// crucial when the cursor is pulled into an infeasible region: instead
// of stalling at the previous params, the solver creeps to the boundary
// and lets subsequent frames slide along it.
let alpha = 1
let acceptedParams: number[] | null = null
let acceptedResidualNorm = Infinity
for (let bt = 0; bt < 8; bt++) {
const trialParams = params.map((p, i) => p + alpha * delta[i])
const trialPos = forward(unflattenPositions(trialParams))
if (trialPos) {
const dr0 = trialPos.x - targetPosition.x
const dr1 = trialPos.y - targetPosition.y
const trialResidualNorm = Math.sqrt(dr0 * dr0 + dr1 * dr1)
if (trialResidualNorm < residualNorm) {
acceptedParams = trialParams
acceptedResidualNorm = trialResidualNorm
break
}
}
alpha *= 0.5
}
if (acceptedParams) {
params = acceptedParams
// Smaller alpha means we had to back off — keep lambda where it is or
// increase it slightly so the next iteration proposes a more cautious
// direction. Full step (alpha = 1) lets us decay lambda as before.
if (alpha >= 1) {
lambda = Math.max(lambda * 0.5, 1e-8)
} else if (alpha < 0.25) {
lambda *= 2
}
// Track the accepted residual against bestResidualNorm immediately so
// the boundary-creeping pose is preserved if subsequent iters fail.
if (acceptedResidualNorm < bestResidualNorm) {
bestResidualNorm = acceptedResidualNorm
bestParams = [...params]
}
} else {
lambda *= 4
}
}
if (solverState) solverState.lambda = lambda
return {
inputPositions: unflattenPositions(bestParams),
converged: bestResidualNorm < tolerance,
iterations: maxIterations,
residualNorm: bestResidualNorm,
lambda,
}
}
/**
* Convenience wrapper: solve for input positions using a recipe as the forward model.
*
* @param recipe - The construction recipe (pure geometric definition)
* @param currentInputPositions - Current positions of input (given) points
* @param targetRef - Recipe ref of the point being dragged (e.g. 'C')
* @param targetPosition - Where the user wants that point
* @param registry - Recipe registry for resolving `apply` ops
* @param solverState - Persistent state for warm-starting lambda across frames
* @param options - Solver tuning parameters
*/
export function solveInverseKinematics(
recipe: ConstructionRecipe,
currentInputPositions: Pt[],
targetRef: Ref,
targetPosition: Pt,
registry: RecipeRegistry,
solverState?: InverseSolverState,
options?: InverseSolverOptions
): InverseSolverResult {
const forward: ForwardFn = (positions) => {
const trace = evaluateRecipe(recipe, positions, registry)
return trace?.pointMap.get(targetRef) ?? null
}
return solveInverse(forward, currentInputPositions, targetPosition, solverState, options)
}
// ── Helpers ────────────────────────────────────────────────────────
export function flattenPositions(positions: Pt[]): number[] {
const out: number[] = []
for (const p of positions) {
out.push(p.x, p.y)
}
return out
}
export function unflattenPositions(params: number[]): Pt[] {
const out: Pt[] = []
for (let i = 0; i < params.length; i += 2) {
out.push({ x: params[i], y: params[i + 1] })
}
return out
}
/**
* Generic version: determine which parameter indices affect the target point.
*/
function findActiveParametersGeneric(
forward: ForwardFn,
inputPositions: Pt[],
epsilon: number
): number[] {
const n = inputPositions.length * 2
const params = flattenPositions(inputPositions)
const active: number[] = []
const basePos = forward(inputPositions)
if (!basePos) return []
for (let j = 0; j < n; j++) {
const perturbed = [...params]
perturbed[j] += epsilon * 100
const pos = forward(unflattenPositions(perturbed))
if (!pos) {
active.push(j)
continue
}
const dx = pos.x - basePos.x
const dy = pos.y - basePos.y
if (Math.sqrt(dx * dx + dy * dy) > epsilon * 10) {
active.push(j)
}
}
return active
}
|