brepjs 18.79.0 → 18.81.0
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- package/dist/brepjs.cjs +157 -12
- package/dist/brepjs.js +156 -13
- package/dist/index.d.ts +1 -0
- package/dist/kernel/solverAdapter.d.ts +5 -3
- package/dist/operations/ikFns.d.ts +58 -0
- package/dist/operations.cjs +3 -1
- package/dist/operations.d.ts +1 -0
- package/dist/operations.js +2 -2
- package/dist/{threadFns-Cra0yHSF.js → threadFns-D_CxgFXg.js} +298 -5
- package/dist/{threadFns-B7a1EVpS.cjs → threadFns-ubwexS-v.cjs} +309 -4
- package/package.json +1 -1
package/dist/brepjs.cjs
CHANGED
|
@@ -17,7 +17,7 @@ const require_arrayAccess = require("./arrayAccess-e4H9cBfh.cjs");
|
|
|
17
17
|
const require_surfaceBuilders = require("./surfaceBuilders-B8aVZamB.cjs");
|
|
18
18
|
const require_primitiveFns = require("./primitiveFns-DEBQdEkG.cjs");
|
|
19
19
|
const require_healingFns = require("./healingFns--PtL9j2K.cjs");
|
|
20
|
-
const require_threadFns = require("./threadFns-
|
|
20
|
+
const require_threadFns = require("./threadFns-ubwexS-v.cjs");
|
|
21
21
|
const require_blueprintSketcher = require("./blueprintSketcher-BJPBKhF3.cjs");
|
|
22
22
|
const require_helpers = require("./helpers-B8mE35Fm.cjs");
|
|
23
23
|
const require_drawFns = require("./drawFns-DTpCthM5.cjs");
|
|
@@ -2286,10 +2286,122 @@ function solveAngle(ref, dep, angleRad) {
|
|
|
2286
2286
|
rotation: require_threadFns.quatFromAxisAngle(Math.hypot(c[0], c[1], c[2]) < 1e-9 ? anyPerpendicular(nDep) : c, phi - angleRad)
|
|
2287
2287
|
};
|
|
2288
2288
|
}
|
|
2289
|
-
/**
|
|
2289
|
+
/**
|
|
2290
|
+
* Point-point coincident/distance: place the dependent point on the reference
|
|
2291
|
+
* point (`extra` = 0) or at `extra` along the original separation direction.
|
|
2292
|
+
* If the points already coincide the direction is arbitrary (+X).
|
|
2293
|
+
*/
|
|
2294
|
+
function solvePointPair(refOrigin, depOrigin, extra) {
|
|
2295
|
+
const sep = sub(depOrigin, refOrigin);
|
|
2296
|
+
const len = Math.hypot(sep[0], sep[1], sep[2]);
|
|
2297
|
+
return {
|
|
2298
|
+
position: sub(add$1(refOrigin, scale$2(len < 1e-9 ? [
|
|
2299
|
+
1,
|
|
2300
|
+
0,
|
|
2301
|
+
0
|
|
2302
|
+
] : scale$2(sep, 1 / len), extra)), depOrigin),
|
|
2303
|
+
rotation: IDENTITY_ROTATION
|
|
2304
|
+
};
|
|
2305
|
+
}
|
|
2306
|
+
/**
|
|
2307
|
+
* Move the dependent plane (normal `depNormal`, at the origin) so its signed
|
|
2308
|
+
* distance from the reference point equals `extra` (the mirror of `solvePlanePair`
|
|
2309
|
+
* for a point reference and a plane dependent).
|
|
2310
|
+
*/
|
|
2311
|
+
function solvePlaneToPoint(depNormal, refOrigin, depOrigin, extra) {
|
|
2312
|
+
const n = normalize(depNormal);
|
|
2313
|
+
return {
|
|
2314
|
+
position: scale$2(n, dot(n, sub(refOrigin, depOrigin)) - extra),
|
|
2315
|
+
rotation: IDENTITY_ROTATION
|
|
2316
|
+
};
|
|
2317
|
+
}
|
|
2318
|
+
/**
|
|
2319
|
+
* Reference axis, dependent point: drop the point onto the axis line
|
|
2320
|
+
* (`extra` = 0) or place it at radial distance `extra` from the line, keeping
|
|
2321
|
+
* its along-axis position. A point already on the axis gets an arbitrary radial.
|
|
2322
|
+
*/
|
|
2323
|
+
function solveAxisToPoint(ref, depOrigin, extra) {
|
|
2324
|
+
const d = normalize(ref.direction ?? [
|
|
2325
|
+
0,
|
|
2326
|
+
0,
|
|
2327
|
+
1
|
|
2328
|
+
]);
|
|
2329
|
+
const foot = add$1(ref.origin, scale$2(d, dot(d, sub(depOrigin, ref.origin))));
|
|
2330
|
+
if (extra === 0) return {
|
|
2331
|
+
position: sub(foot, depOrigin),
|
|
2332
|
+
rotation: IDENTITY_ROTATION
|
|
2333
|
+
};
|
|
2334
|
+
const radial = sub(depOrigin, foot);
|
|
2335
|
+
const rlen = Math.hypot(radial[0], radial[1], radial[2]);
|
|
2336
|
+
return {
|
|
2337
|
+
position: sub(add$1(foot, scale$2(rlen < 1e-9 ? anyPerpendicular(d) : scale$2(radial, 1 / rlen), extra)), depOrigin),
|
|
2338
|
+
rotation: IDENTITY_ROTATION
|
|
2339
|
+
};
|
|
2340
|
+
}
|
|
2341
|
+
/**
|
|
2342
|
+
* Reference point, dependent axis: translate the axis line so it passes through
|
|
2343
|
+
* the point (`extra` = 0) or lies at perpendicular distance `extra` from it.
|
|
2344
|
+
* Translation is purely perpendicular to the axis, so the line's direction and
|
|
2345
|
+
* along-axis parameterization are preserved.
|
|
2346
|
+
*/
|
|
2347
|
+
function solvePointToAxis(refOrigin, dep, extra) {
|
|
2348
|
+
const d = normalize(dep.direction ?? [
|
|
2349
|
+
0,
|
|
2350
|
+
0,
|
|
2351
|
+
1
|
|
2352
|
+
]);
|
|
2353
|
+
const w = sub(dep.origin, refOrigin);
|
|
2354
|
+
const perp = sub(w, scale$2(d, dot(d, w)));
|
|
2355
|
+
const plen = Math.hypot(perp[0], perp[1], perp[2]);
|
|
2356
|
+
if (extra === 0) return {
|
|
2357
|
+
position: scale$2(perp, -1),
|
|
2358
|
+
rotation: IDENTITY_ROTATION
|
|
2359
|
+
};
|
|
2360
|
+
return {
|
|
2361
|
+
position: sub(scale$2(plen < 1e-9 ? anyPerpendicular(d) : scale$2(perp, 1 / plen), extra), perp),
|
|
2362
|
+
rotation: IDENTITY_ROTATION
|
|
2363
|
+
};
|
|
2364
|
+
}
|
|
2365
|
+
/**
|
|
2366
|
+
* Axis-axis distance: align the dependent axis parallel to the reference, then
|
|
2367
|
+
* offset it to perpendicular distance `extra` (parallel pin-and-spacer). With
|
|
2368
|
+
* `extra` = 0 the axes become collinear, matching `concentric`.
|
|
2369
|
+
*/
|
|
2370
|
+
function solveAxisAxisDistance(ref, dep, extra) {
|
|
2371
|
+
const dRef = normalize(ref.direction ?? [
|
|
2372
|
+
0,
|
|
2373
|
+
0,
|
|
2374
|
+
1
|
|
2375
|
+
]);
|
|
2376
|
+
const rotation = require_threadFns.quatFromTo(normalize(dep.direction ?? [
|
|
2377
|
+
0,
|
|
2378
|
+
0,
|
|
2379
|
+
1
|
|
2380
|
+
]), dRef);
|
|
2381
|
+
const w = sub(require_threadFns.quatRotate(rotation, dep.origin), ref.origin);
|
|
2382
|
+
const perp = sub(w, scale$2(dRef, dot(dRef, w)));
|
|
2383
|
+
const plen = Math.hypot(perp[0], perp[1], perp[2]);
|
|
2384
|
+
return {
|
|
2385
|
+
position: sub(scale$2(plen < 1e-9 ? anyPerpendicular(dRef) : scale$2(perp, 1 / plen), extra), perp),
|
|
2386
|
+
rotation
|
|
2387
|
+
};
|
|
2388
|
+
}
|
|
2389
|
+
/**
|
|
2390
|
+
* Supported entity-type pairs for the translational mates (`coincident` /
|
|
2391
|
+
* `distance`), keyed `${entityA}-${entityB}`. Both orders are listed where the
|
|
2392
|
+
* solver handles them, so a user need not pre-order the entities.
|
|
2393
|
+
*/
|
|
2394
|
+
var TRANSLATIONAL_PAIRS = new Set([
|
|
2395
|
+
"plane-plane",
|
|
2396
|
+
"plane-point",
|
|
2397
|
+
"point-plane",
|
|
2398
|
+
"point-point",
|
|
2399
|
+
"axis-axis",
|
|
2400
|
+
"axis-point",
|
|
2401
|
+
"point-axis"
|
|
2402
|
+
]);
|
|
2403
|
+
/** Entity types the orientation/axis mates require of (entityA, entityB). */
|
|
2290
2404
|
var REQUIRED_ENTITIES = {
|
|
2291
|
-
coincident: "plane",
|
|
2292
|
-
distance: "plane",
|
|
2293
2405
|
angle: "plane",
|
|
2294
2406
|
concentric: "axis"
|
|
2295
2407
|
};
|
|
@@ -2299,21 +2411,51 @@ var POSITIONING_TYPES = new Set([
|
|
|
2299
2411
|
"angle",
|
|
2300
2412
|
"concentric"
|
|
2301
2413
|
]);
|
|
2414
|
+
/** Whether a positioning mate's entity pair is solvable. */
|
|
2415
|
+
function isSupportedPair(type, a, b) {
|
|
2416
|
+
const required = REQUIRED_ENTITIES[type];
|
|
2417
|
+
if (required) return a === required && b === required;
|
|
2418
|
+
return TRANSLATIONAL_PAIRS.has(`${a}-${b}`);
|
|
2419
|
+
}
|
|
2420
|
+
/**
|
|
2421
|
+
* Solve a `coincident` (extra = 0) or `distance` (extra = value) mate for any
|
|
2422
|
+
* supported entity-type pair. `ref` is already in world space; the dependent is
|
|
2423
|
+
* at the origin. Returns null only for an unsupported pair (filtered out
|
|
2424
|
+
* upstream by `isSupportedPair`).
|
|
2425
|
+
*/
|
|
2426
|
+
function solveTranslational(ref, dep, extra) {
|
|
2427
|
+
switch (`${ref.type}-${dep.type}`) {
|
|
2428
|
+
case "plane-plane":
|
|
2429
|
+
case "plane-point": return solvePlanePair(ref, dep, extra);
|
|
2430
|
+
case "point-plane": return solvePlaneToPoint(dep.normal ?? [
|
|
2431
|
+
0,
|
|
2432
|
+
0,
|
|
2433
|
+
1
|
|
2434
|
+
], ref.origin, dep.origin, extra);
|
|
2435
|
+
case "point-point": return solvePointPair(ref.origin, dep.origin, extra);
|
|
2436
|
+
case "axis-axis": return extra === 0 ? solveConcentric(ref, dep) : solveAxisAxisDistance(ref, dep, extra);
|
|
2437
|
+
case "axis-point": return solveAxisToPoint(ref, dep.origin, extra);
|
|
2438
|
+
case "point-axis": return solvePointToAxis(ref.origin, dep, extra);
|
|
2439
|
+
default: return null;
|
|
2440
|
+
}
|
|
2441
|
+
}
|
|
2302
2442
|
/** Dispatch a positioning mate to its solver. `ref` is already in world space. */
|
|
2303
2443
|
function solveMate(c, ref, dep) {
|
|
2304
2444
|
switch (c.type) {
|
|
2305
2445
|
case "concentric": return solveConcentric(ref, dep);
|
|
2306
2446
|
case "angle": return solveAngle(ref, dep, (c.value ?? 0) * Math.PI / 180);
|
|
2307
|
-
case "distance": return solvePlanePair(ref, dep, c.value ?? 0);
|
|
2308
|
-
default: return solvePlanePair(ref, dep, 0);
|
|
2447
|
+
case "distance": return solveTranslational(ref, dep, c.value ?? 0) ?? solvePlanePair(ref, dep, c.value ?? 0);
|
|
2448
|
+
default: return solveTranslational(ref, dep, 0) ?? solvePlanePair(ref, dep, 0);
|
|
2309
2449
|
}
|
|
2310
2450
|
}
|
|
2311
2451
|
/**
|
|
2312
2452
|
* Solve assembly constraints analytically.
|
|
2313
2453
|
*
|
|
2314
|
-
* Handles: fixed,
|
|
2315
|
-
*
|
|
2316
|
-
*
|
|
2454
|
+
* Handles: fixed, concentric (axis-axis), angle (plane-plane orientation), and
|
|
2455
|
+
* coincident/distance for any supported entity-type pair (plane-plane,
|
|
2456
|
+
* plane-point, point-point, axis-axis, axis-point, and both point orders — see
|
|
2457
|
+
* `TRANSLATIONAL_PAIRS`). For a positioning mate, entityA is the reference and
|
|
2458
|
+
* entityB the dependent. Chain roots (nodes never positioned by a
|
|
2317
2459
|
* mate) and explicit `fixed` nodes anchor at the origin; constraints then resolve
|
|
2318
2460
|
* in topological order — each places its dependent against the reference's solved
|
|
2319
2461
|
* pose (rotation included), so multi-body chains compose. Returns
|
|
@@ -2340,9 +2482,10 @@ function solveConstraints(nodes, constraints) {
|
|
|
2340
2482
|
const pending = [];
|
|
2341
2483
|
for (const c of positioning) {
|
|
2342
2484
|
if (!c.entityA || !c.entityB) continue;
|
|
2343
|
-
const
|
|
2344
|
-
|
|
2345
|
-
|
|
2485
|
+
const a = c.entityA.entity.type;
|
|
2486
|
+
const b = c.entityB.entity.type;
|
|
2487
|
+
if (!isSupportedPair(c.type, a, b)) {
|
|
2488
|
+
unsupported.push(`${c.type}(${a}-${b})`);
|
|
2346
2489
|
continue;
|
|
2347
2490
|
}
|
|
2348
2491
|
pending.push(c);
|
|
@@ -6599,6 +6742,7 @@ exports.intersect2D = require_boolean2D.intersect2D;
|
|
|
6599
6742
|
exports.intersectBlueprints = require_boolean2D.intersectBlueprints;
|
|
6600
6743
|
exports.intersectWithEvolution = require_healingFns.intersectWithEvolution;
|
|
6601
6744
|
exports.invalidateShapeCache = require_topologyQueryFns.invalidateShapeCache;
|
|
6745
|
+
exports.inverseKinematics = require_threadFns.inverseKinematics;
|
|
6602
6746
|
Object.defineProperty(exports, "io", {
|
|
6603
6747
|
enumerable: true,
|
|
6604
6748
|
get: function() {
|
|
@@ -6650,6 +6794,7 @@ exports.iterSolids = require_topologyQueryFns.iterSolids;
|
|
|
6650
6794
|
exports.iterTopo = require_faceFns.iterTopo;
|
|
6651
6795
|
exports.iterVertices = require_topologyQueryFns.iterVertices;
|
|
6652
6796
|
exports.iterWires = require_topologyQueryFns.iterWires;
|
|
6797
|
+
exports.jointTrajectory = require_threadFns.jointTrajectory;
|
|
6653
6798
|
exports.jointTransform = require_threadFns.jointTransform;
|
|
6654
6799
|
exports.kernelCall = require_topologyQueryFns.kernelCall;
|
|
6655
6800
|
exports.kernelCallRaw = require_topologyQueryFns.kernelCallRaw;
|
package/dist/brepjs.js
CHANGED
|
@@ -15,7 +15,7 @@ import { n as getAtOrThrow, r as lastOrThrow, t as firstOrThrow } from "./arrayA
|
|
|
15
15
|
import { _ as makeThreePointArc, d as makeCircle, h as makeLine, l as makeBSplineInterpolation, n as fill, r as makeFace, s as assembleWire } from "./surfaceBuilders-hTXdNCYm.js";
|
|
16
16
|
import { A as cutAll, C as threePointArc, D as wireLoop, E as wire, F as sectionToFace$1, I as slice$1, L as split$1, M as fuseAll, N as intersect$2, O as booleanPipeline, P as section$1, S as tangentArc, T as vertex, _ as polygon, a as circle, b as sphere$1, c as cylinder, d as ellipsoid, f as face, g as offsetFace, h as line, i as bsplineApprox, j as fuse$2, k as cut$2, l as ellipse, m as helix, n as bezier, o as compound, p as filledFace, r as box, s as cone, t as addHoles, u as ellipseArc, v as sewShells, w as torus$1, x as subFace, y as solid } from "./primitiveFns-BSKbI4Kl.js";
|
|
17
17
|
import { A as edgesOfFace, C as shellWithEvolution, D as getNurbsCurveData, E as fuseAllBisect, F as chamferDistAngle, I as toBufferGeometryData, L as toGroupedBufferGeometryData, M as sharedEdges, N as verticesOfEdge, O as getNurbsSurfaceData, P as wiresOfFace, R as toLODGeometryData, S as intersectWithEvolution, T as cutAllBisect, _ as positionOnCurve, a as healFace, b as filletWithEvolution, c as isValid$1, d as draft$1, f as fillet$1, g as variableFillet, h as thicken$1, i as heal$1, j as facesOfEdge, k as adjacentFaces, l as solidFromShell, m as shell$1, n as fixSelfIntersection, o as healSolid, p as offset$1, r as fixShape, s as healWire, t as autoHeal, u as chamfer$1, v as chamferWithEvolution, w as checkBoolean, x as fuseWithEvolution, y as cutWithEvolution, z as toLineGeometryData } from "./healingFns-Bm-NdBj_.js";
|
|
18
|
-
import { A as
|
|
18
|
+
import { A as quatFromAxisAngle, B as walkAssembly, C as mechanismDOF, D as setJointValue, E as revoluteJoint, F as countNodes, G as createAssembly, I as createAssemblyNode, L as findNode, M as quatRotate, N as addChild, O as setJointValues, P as collectShapes, R as removeChild, S as jointTransform, T as prismaticJoint, U as linearPattern, V as circularPattern, W as exportAssemblySTEP, _ as inverseKinematics, a as deserializeHistory, b as cylindricalJoint, c as modifyStep, d as replayFrom, f as replayHistory, g as undoLast, h as stepsFrom, i as createRegistry, j as quatFromTo, k as sphericalJoint, l as registerOperation, m as stepCount, n as addStep, o as findStep, p as serializeHistory, r as createHistory, s as getShape, t as thread, u as registerShape, v as jointTrajectory, w as planarJoint, x as forwardKinematics, y as addJoint, z as updateNode } from "./threadFns-D_CxgFXg.js";
|
|
19
19
|
import { n as BaseSketcher2d, r as organiseBlueprints, t as BlueprintSketcher } from "./blueprintSketcher-EYqKWQh0.js";
|
|
20
20
|
import { a as createTypedFinder, i as wireFinder, n as edgeFinder, r as faceFinder, t as getSingleFace } from "./helpers-CD8EMZ5l.js";
|
|
21
21
|
import { A as sketchEllipse, D as makeBaseBox, E as deserializeDrawing, F as sketchRectangle, I as sketchRoundedRectangle, L as FaceSketcher, M as sketchHelix, N as sketchParametricFunction, O as polysideInnerRadius, P as sketchPolysides, R as Sketcher, S as drawText, _ as drawPolysides, a as drawingIntersect, b as drawSingleCircle, c as rotateDrawing, d as drawFaceOutline, f as drawProjection, g as drawPointsInterpolation, h as drawParametricFunction, i as drawingFuse, j as sketchFaceOffset, k as sketchCircle, l as scaleDrawing, m as drawEllipse, n as drawingCut, o as drawingToSketchOnPlane, p as drawCircle, r as drawingFillet, s as mirrorDrawing, t as drawingChamfer, u as translateDrawing, v as drawRectangle, w as draw, x as drawSingleEllipse, y as drawRoundedRectangle } from "./drawFns-D_LwehLr.js";
|
|
@@ -2297,10 +2297,122 @@ function solveAngle(ref, dep, angleRad) {
|
|
|
2297
2297
|
rotation: quatFromAxisAngle(Math.hypot(c[0], c[1], c[2]) < 1e-9 ? anyPerpendicular(nDep) : c, phi - angleRad)
|
|
2298
2298
|
};
|
|
2299
2299
|
}
|
|
2300
|
-
/**
|
|
2300
|
+
/**
|
|
2301
|
+
* Point-point coincident/distance: place the dependent point on the reference
|
|
2302
|
+
* point (`extra` = 0) or at `extra` along the original separation direction.
|
|
2303
|
+
* If the points already coincide the direction is arbitrary (+X).
|
|
2304
|
+
*/
|
|
2305
|
+
function solvePointPair(refOrigin, depOrigin, extra) {
|
|
2306
|
+
const sep = sub(depOrigin, refOrigin);
|
|
2307
|
+
const len = Math.hypot(sep[0], sep[1], sep[2]);
|
|
2308
|
+
return {
|
|
2309
|
+
position: sub(add$1(refOrigin, scale$2(len < 1e-9 ? [
|
|
2310
|
+
1,
|
|
2311
|
+
0,
|
|
2312
|
+
0
|
|
2313
|
+
] : scale$2(sep, 1 / len), extra)), depOrigin),
|
|
2314
|
+
rotation: IDENTITY_ROTATION
|
|
2315
|
+
};
|
|
2316
|
+
}
|
|
2317
|
+
/**
|
|
2318
|
+
* Move the dependent plane (normal `depNormal`, at the origin) so its signed
|
|
2319
|
+
* distance from the reference point equals `extra` (the mirror of `solvePlanePair`
|
|
2320
|
+
* for a point reference and a plane dependent).
|
|
2321
|
+
*/
|
|
2322
|
+
function solvePlaneToPoint(depNormal, refOrigin, depOrigin, extra) {
|
|
2323
|
+
const n = normalize(depNormal);
|
|
2324
|
+
return {
|
|
2325
|
+
position: scale$2(n, dot(n, sub(refOrigin, depOrigin)) - extra),
|
|
2326
|
+
rotation: IDENTITY_ROTATION
|
|
2327
|
+
};
|
|
2328
|
+
}
|
|
2329
|
+
/**
|
|
2330
|
+
* Reference axis, dependent point: drop the point onto the axis line
|
|
2331
|
+
* (`extra` = 0) or place it at radial distance `extra` from the line, keeping
|
|
2332
|
+
* its along-axis position. A point already on the axis gets an arbitrary radial.
|
|
2333
|
+
*/
|
|
2334
|
+
function solveAxisToPoint(ref, depOrigin, extra) {
|
|
2335
|
+
const d = normalize(ref.direction ?? [
|
|
2336
|
+
0,
|
|
2337
|
+
0,
|
|
2338
|
+
1
|
|
2339
|
+
]);
|
|
2340
|
+
const foot = add$1(ref.origin, scale$2(d, dot(d, sub(depOrigin, ref.origin))));
|
|
2341
|
+
if (extra === 0) return {
|
|
2342
|
+
position: sub(foot, depOrigin),
|
|
2343
|
+
rotation: IDENTITY_ROTATION
|
|
2344
|
+
};
|
|
2345
|
+
const radial = sub(depOrigin, foot);
|
|
2346
|
+
const rlen = Math.hypot(radial[0], radial[1], radial[2]);
|
|
2347
|
+
return {
|
|
2348
|
+
position: sub(add$1(foot, scale$2(rlen < 1e-9 ? anyPerpendicular(d) : scale$2(radial, 1 / rlen), extra)), depOrigin),
|
|
2349
|
+
rotation: IDENTITY_ROTATION
|
|
2350
|
+
};
|
|
2351
|
+
}
|
|
2352
|
+
/**
|
|
2353
|
+
* Reference point, dependent axis: translate the axis line so it passes through
|
|
2354
|
+
* the point (`extra` = 0) or lies at perpendicular distance `extra` from it.
|
|
2355
|
+
* Translation is purely perpendicular to the axis, so the line's direction and
|
|
2356
|
+
* along-axis parameterization are preserved.
|
|
2357
|
+
*/
|
|
2358
|
+
function solvePointToAxis(refOrigin, dep, extra) {
|
|
2359
|
+
const d = normalize(dep.direction ?? [
|
|
2360
|
+
0,
|
|
2361
|
+
0,
|
|
2362
|
+
1
|
|
2363
|
+
]);
|
|
2364
|
+
const w = sub(dep.origin, refOrigin);
|
|
2365
|
+
const perp = sub(w, scale$2(d, dot(d, w)));
|
|
2366
|
+
const plen = Math.hypot(perp[0], perp[1], perp[2]);
|
|
2367
|
+
if (extra === 0) return {
|
|
2368
|
+
position: scale$2(perp, -1),
|
|
2369
|
+
rotation: IDENTITY_ROTATION
|
|
2370
|
+
};
|
|
2371
|
+
return {
|
|
2372
|
+
position: sub(scale$2(plen < 1e-9 ? anyPerpendicular(d) : scale$2(perp, 1 / plen), extra), perp),
|
|
2373
|
+
rotation: IDENTITY_ROTATION
|
|
2374
|
+
};
|
|
2375
|
+
}
|
|
2376
|
+
/**
|
|
2377
|
+
* Axis-axis distance: align the dependent axis parallel to the reference, then
|
|
2378
|
+
* offset it to perpendicular distance `extra` (parallel pin-and-spacer). With
|
|
2379
|
+
* `extra` = 0 the axes become collinear, matching `concentric`.
|
|
2380
|
+
*/
|
|
2381
|
+
function solveAxisAxisDistance(ref, dep, extra) {
|
|
2382
|
+
const dRef = normalize(ref.direction ?? [
|
|
2383
|
+
0,
|
|
2384
|
+
0,
|
|
2385
|
+
1
|
|
2386
|
+
]);
|
|
2387
|
+
const rotation = quatFromTo(normalize(dep.direction ?? [
|
|
2388
|
+
0,
|
|
2389
|
+
0,
|
|
2390
|
+
1
|
|
2391
|
+
]), dRef);
|
|
2392
|
+
const w = sub(quatRotate(rotation, dep.origin), ref.origin);
|
|
2393
|
+
const perp = sub(w, scale$2(dRef, dot(dRef, w)));
|
|
2394
|
+
const plen = Math.hypot(perp[0], perp[1], perp[2]);
|
|
2395
|
+
return {
|
|
2396
|
+
position: sub(scale$2(plen < 1e-9 ? anyPerpendicular(dRef) : scale$2(perp, 1 / plen), extra), perp),
|
|
2397
|
+
rotation
|
|
2398
|
+
};
|
|
2399
|
+
}
|
|
2400
|
+
/**
|
|
2401
|
+
* Supported entity-type pairs for the translational mates (`coincident` /
|
|
2402
|
+
* `distance`), keyed `${entityA}-${entityB}`. Both orders are listed where the
|
|
2403
|
+
* solver handles them, so a user need not pre-order the entities.
|
|
2404
|
+
*/
|
|
2405
|
+
var TRANSLATIONAL_PAIRS = new Set([
|
|
2406
|
+
"plane-plane",
|
|
2407
|
+
"plane-point",
|
|
2408
|
+
"point-plane",
|
|
2409
|
+
"point-point",
|
|
2410
|
+
"axis-axis",
|
|
2411
|
+
"axis-point",
|
|
2412
|
+
"point-axis"
|
|
2413
|
+
]);
|
|
2414
|
+
/** Entity types the orientation/axis mates require of (entityA, entityB). */
|
|
2301
2415
|
var REQUIRED_ENTITIES = {
|
|
2302
|
-
coincident: "plane",
|
|
2303
|
-
distance: "plane",
|
|
2304
2416
|
angle: "plane",
|
|
2305
2417
|
concentric: "axis"
|
|
2306
2418
|
};
|
|
@@ -2310,21 +2422,51 @@ var POSITIONING_TYPES = new Set([
|
|
|
2310
2422
|
"angle",
|
|
2311
2423
|
"concentric"
|
|
2312
2424
|
]);
|
|
2425
|
+
/** Whether a positioning mate's entity pair is solvable. */
|
|
2426
|
+
function isSupportedPair(type, a, b) {
|
|
2427
|
+
const required = REQUIRED_ENTITIES[type];
|
|
2428
|
+
if (required) return a === required && b === required;
|
|
2429
|
+
return TRANSLATIONAL_PAIRS.has(`${a}-${b}`);
|
|
2430
|
+
}
|
|
2431
|
+
/**
|
|
2432
|
+
* Solve a `coincident` (extra = 0) or `distance` (extra = value) mate for any
|
|
2433
|
+
* supported entity-type pair. `ref` is already in world space; the dependent is
|
|
2434
|
+
* at the origin. Returns null only for an unsupported pair (filtered out
|
|
2435
|
+
* upstream by `isSupportedPair`).
|
|
2436
|
+
*/
|
|
2437
|
+
function solveTranslational(ref, dep, extra) {
|
|
2438
|
+
switch (`${ref.type}-${dep.type}`) {
|
|
2439
|
+
case "plane-plane":
|
|
2440
|
+
case "plane-point": return solvePlanePair(ref, dep, extra);
|
|
2441
|
+
case "point-plane": return solvePlaneToPoint(dep.normal ?? [
|
|
2442
|
+
0,
|
|
2443
|
+
0,
|
|
2444
|
+
1
|
|
2445
|
+
], ref.origin, dep.origin, extra);
|
|
2446
|
+
case "point-point": return solvePointPair(ref.origin, dep.origin, extra);
|
|
2447
|
+
case "axis-axis": return extra === 0 ? solveConcentric(ref, dep) : solveAxisAxisDistance(ref, dep, extra);
|
|
2448
|
+
case "axis-point": return solveAxisToPoint(ref, dep.origin, extra);
|
|
2449
|
+
case "point-axis": return solvePointToAxis(ref.origin, dep, extra);
|
|
2450
|
+
default: return null;
|
|
2451
|
+
}
|
|
2452
|
+
}
|
|
2313
2453
|
/** Dispatch a positioning mate to its solver. `ref` is already in world space. */
|
|
2314
2454
|
function solveMate(c, ref, dep) {
|
|
2315
2455
|
switch (c.type) {
|
|
2316
2456
|
case "concentric": return solveConcentric(ref, dep);
|
|
2317
2457
|
case "angle": return solveAngle(ref, dep, (c.value ?? 0) * Math.PI / 180);
|
|
2318
|
-
case "distance": return solvePlanePair(ref, dep, c.value ?? 0);
|
|
2319
|
-
default: return solvePlanePair(ref, dep, 0);
|
|
2458
|
+
case "distance": return solveTranslational(ref, dep, c.value ?? 0) ?? solvePlanePair(ref, dep, c.value ?? 0);
|
|
2459
|
+
default: return solveTranslational(ref, dep, 0) ?? solvePlanePair(ref, dep, 0);
|
|
2320
2460
|
}
|
|
2321
2461
|
}
|
|
2322
2462
|
/**
|
|
2323
2463
|
* Solve assembly constraints analytically.
|
|
2324
2464
|
*
|
|
2325
|
-
* Handles: fixed,
|
|
2326
|
-
*
|
|
2327
|
-
*
|
|
2465
|
+
* Handles: fixed, concentric (axis-axis), angle (plane-plane orientation), and
|
|
2466
|
+
* coincident/distance for any supported entity-type pair (plane-plane,
|
|
2467
|
+
* plane-point, point-point, axis-axis, axis-point, and both point orders — see
|
|
2468
|
+
* `TRANSLATIONAL_PAIRS`). For a positioning mate, entityA is the reference and
|
|
2469
|
+
* entityB the dependent. Chain roots (nodes never positioned by a
|
|
2328
2470
|
* mate) and explicit `fixed` nodes anchor at the origin; constraints then resolve
|
|
2329
2471
|
* in topological order — each places its dependent against the reference's solved
|
|
2330
2472
|
* pose (rotation included), so multi-body chains compose. Returns
|
|
@@ -2351,9 +2493,10 @@ function solveConstraints(nodes, constraints) {
|
|
|
2351
2493
|
const pending = [];
|
|
2352
2494
|
for (const c of positioning) {
|
|
2353
2495
|
if (!c.entityA || !c.entityB) continue;
|
|
2354
|
-
const
|
|
2355
|
-
|
|
2356
|
-
|
|
2496
|
+
const a = c.entityA.entity.type;
|
|
2497
|
+
const b = c.entityB.entity.type;
|
|
2498
|
+
if (!isSupportedPair(c.type, a, b)) {
|
|
2499
|
+
unsupported.push(`${c.type}(${a}-${b})`);
|
|
2357
2500
|
continue;
|
|
2358
2501
|
}
|
|
2359
2502
|
pending.push(c);
|
|
@@ -6311,4 +6454,4 @@ var csg_exports = /* @__PURE__ */ __exportAll({
|
|
|
6311
6454
|
withEvaluator: () => withEvaluator
|
|
6312
6455
|
});
|
|
6313
6456
|
//#endregion
|
|
6314
|
-
export { BaseSketcher2d, BlueprintSketcher, BrepBugError, BrepErrorCode, BrepWrapperError, BrepkitAdapter, CompoundSketch, DEFAULT_CAPABILITIES, DEG2RAD, DisposalScope, EXACT_BREP_CAPABILITIES, FaceSketcher, HASH_CODE_MAX, OK, OcctWasmAdapter, RAD2DEG, Sketch, Sketcher, Sketches, addChild, addHoles, addJoint, addMate, addStep, adjacentFaces, all, andThen, applyGlue, applyMatrix, approximateCurve, as2D, as3D, asTopo, assignRoles, autoHeal, bezier, blueprintToDXF, booleanPipeline, booleans_exports as booleans, boss, box, bsplineApprox, bug, cameraFromPlane, cameraLookAt, captureHint, cast, castShape, castShape3D, chamfer, chamferDistAngle as chamferDistAngleShape, chamferWithEvolution, checkAllInterferences, checkBoolean, checkInterference, circle, circularPattern, classifyPointOnFace, clearMeshCache, clone, closedWire, collect, collectShapes, colorFaces, colorShape, complexExtrude, composeTransforms, compound, compoundSketchExtrude, compoundSketchFace, compoundSketchLoft, compoundSketchRevolve, computationError, computeStraightSkeleton, cone, construction_exports as construction, convexHull, cornerFinder, countNodes, createAssembly, createAssemblyNode, createBlueprint, createCamera, createCompound, createCompoundBlueprint, createDistanceQuery, createEdge, createFace, createHandle, createHistory, createKernelHandle, createMeshCache, createNamedPlane, createOperationRegistry, createPlane, createRef, createRegistry, createShell, createSolid, createTaskQueue, createVertex, createWire, createWorkerClient, createWorkerHandler, csg_exports as csg, currentQuality, curve2dBoundingBox, curve2dDistanceFrom, curve2dFirstPoint, curve2dIsOnCurve, curve2dLastPoint, curve2dParameter, curve2dSplitAt, curve2dTangentAt, curveAxis, curveEndPoint, curveIsClosed, curveIsPeriodic, curveLength, curvePeriod, curvePointAt, curveStartPoint, curveTangentAt, cut, cut2D, cutAll, cutAllBisect, cutBlueprints, cutWithEvolution, cylinder, cylindricalJoint, defaultScorer, dequeueTask, describe, deserializeDrawing, deserializeHistory, fromBREP as deserializeShape, downcast, draft, draw, drawCircle, drawEllipse, drawFaceOutline, drawParametricFunction, drawPointsInterpolation, drawPolysides, drawProjection, drawRectangle, drawRoundedRectangle, drawSingleCircle, drawSingleEllipse, drawText, drawingChamfer, drawingCut, drawingFillet, drawingFuse, drawingIntersect, drawingToSketchOnPlane, drill, edgeFinder, edgesOfFace, ellipse, ellipseArc, ellipsoid, enqueueTask, err, exportAssemblySTEP, exportDXF, exportGlb, exportGltf, exportIGES, exportOBJ, exportSTEP, exportSTEPConfigured, exportSTL, exportThreeMF, extrude, extrudeAll, face, faceAxis, faceCenter, faceFinder, faceGeomType, faceOrientation, facesOfEdge, fieldBoolean, fieldContour, fieldOffset, fieldReinit, fieldShell, fill, filledFace, fillet, filletWithEvolution, findFacesByTag, findNode, findStep, fixSelfIntersection, fixShape, flatMap, flatten, flipFaceOrientation, flipOrientation, fontMetrics, forwardKinematics, fromBREP$1 as fromBREP, fromKernelDir, fromKernelPnt, fromKernelVec, fromNullable, fuse, fuse2D, fuseAll, fuseAllBisect, fuseBlueprints, fuseWithEvolution, gearGeometry, getActiveVoxelId, getBounds, getBounds2D, getCompSolids, getCurveType, getDisposalStats, getEdges, getFaceColor, getFaceOrigins, getFaceTags, getFaces, getFont, getHashCode, getShape as getHistoryShape, getKernel, getKernelCapabilities, getKernelTier, getNurbsCurveData, getNurbsSurfaceData, getOrientation, getOrientation2D, getPerformanceStats, getShapeColor, getShapeKind, getShells, getSingleFace, getSolids, getSurfaceType, getTagMetadata, getVertices, getVoxel, getWires, guidedSweep, heal, healFace, healSolid, healWire, helix, hull, importDXF, importGLB, importIGES, importOBJ, importSTEP, importSTL, importSVG, importSVGPathD, importThreeMF, init, initFromManifold, initFromOC, initVoxel, innerWires, interpolateCurve, intersect, intersect2D, intersectBlueprints, intersectWithEvolution, invalidateShapeCache, ioNs_exports as io, ioError, is2D, is3D, isChamferRadius, isClosedWire, isCompSolid, isCompound, isDisposeRequest, isEdge, isEmpty, isEqualShape, isErr, isErrorResponse, isFace, isFilletRadius, isInitRequest, isInside2D, isLive, isManifoldShell, isNumber, isOk, isOperationRequest, isOrientedFace, isPlanarFace, isPlanarWire, isProjectionPlane, isEmpty$1 as isQueueEmpty, isSameShape, isShape1D, isShape3D, isShell, isSolid, isSuccessResponse, isValid, isValidSolid, isVertex, isWire, iterCompSolids, iterEdges, iterFaces, iterShells, iterSolids, iterTopo, iterVertices, iterWires, jointTransform, kernelCall, kernelCallRaw, kernelCallScoped, kernelError, latticeInfill, latticeInfillShape, line, linearPattern, loadFont, loft, loftAll, makeBaseBox, makeExternalGear, makeInternalGear, makePlane, makePlanetaryGear, makeProjectedEdges, manifoldShell, map, mapBoth, mapErr, match, measureArea, measureCurvatureAt, measureCurvatureAtMid, measureDistance, measureDistanceProps, measureLength, measureLinearProps, measureSurfaceProps, measureVolume, measureVolumeProps, measurement_exports as measurement, mechanismDOF, mesh, meshEdges, meshMultiLOD, minkowski, mirror, mirror2D, mirrorDrawing, mirrorJoin, modifiers_exports as modifiers, modifyStep, moduleInitError, multiSectionSweep, normalAt, offset, offsetFace, offsetMesh, offsetShape, offsetWire2D, ok, or, orElse, organiseBlueprints, orientedFace, outerWire, patterns_exports as patterns, pendingCount, pipeline, pivotPlane, planarFace, planarJoint, planarWire, planetPlacements, pocket, pointOnSurface, pointsInside, polygon, polyhedron, polysideInnerRadius, polysidesBlueprint, positionOnCurve, prewarm, primitives_exports as primitives, prismaticJoint, projectEdges, projectPointOnFace, query_exports as query, queryError, rectangularPattern, registerHandler, registerKernel, registerKernelTier, registerOperation, registerShape, registerVoxel, rejectAll, removeChild, removeHolesFromFace, repairMesh, replayFrom, replayHistory, resetDisposalStats, resetPerformanceStats, resize, resolve, resolve3D, resolveDirection, resolvePlane, resolveRef, reverseCurve, revoluteJoint, revolve, roof, rotate, rotate2D, rotateDrawing, roundedRectangleBlueprint, scale, scale2D, scaleDrawing, box$1 as sdfBox, capsule as sdfCapsule, cone$1 as sdfCone, cylinder$1 as sdfCylinder, fieldAxialRamp as sdfFieldAxialRamp, fieldClamp as sdfFieldClamp, fieldConst as sdfFieldConst, fieldFromSdf as sdfFieldFromSdf, fieldRadialRamp as sdfFieldRadialRamp, lattice as sdfLattice, plane as sdfPlane, roundedBox as sdfRoundedBox, sphere as sdfSphere, strutLattice as sdfStrutLattice, sweep as sdfSweep, torus as sdfTorus, section, sectionToFace, serializeHistory, setJointValue, setJointValues, setShapeOrigin, setTagMetadata, sewShells, shape, shapeToMeshInput, shapeType, sharedEdges, shell, shellMesh, shellShape, shellWithEvolution, simplify, sketchCircle, sketchEllipse, sketchExtrude, sketchFace, sketchFaceOffset, sketchHelix, sketchLoft, sketchOnFace2D, sketchOnPlane2D, sketchParametricFunction, sketchPolysides, sketchRectangle, sketchRevolve, sketchRoundedRectangle, sketchSweep, sketchText, sketchWires, sketcherStateError, slice, solid, solidFromShell, solveAssembly, sphere$1 as sphere, sphericalJoint, split, stepCount, stepsFrom, stretch2D, subFace, supportExtrude, supportsConstraintSketch, supportsProjection, surfaceFromGrid, surfaceFromImage, sweep$1 as sweep, tagFaces, tangentArc, tap, tapErr, textBlueprints, textMetrics, thicken, thread, threePointArc, toBREP, toBufferGeometryData, toGroupedBufferGeometryData, toKernelVec, toLODGeometryData, toLineGeometryData, toSVGPathD, toVec2, toVec3, torus$1 as torus, tpmsLattice, transformCopy, transforms_exports as transforms, translate, translate2D, translateDrawing, translatePlane, tryCatch, tryCatchAsync, twistExtrude, typeCastError, undoLast, unsupportedError, unwrap, unwrapErr, unwrapOr, unwrapOrElse, updateNode, updateRoles, uvBounds, uvCoordinates, validSolid, validatePlanetary, validationError, variableFillet, vecAdd, vecAngle, vecCross, vecDistance, vecDot, vecEquals, vecIsZero, vecLength, vecLengthSq, vecNegate, vecNormalize, vecProjectToPlane, vecRepr, vecRotate, vecScale, vecSub, vertex, vertexFinder, vertexPosition, verticesOfEdge, voxelBoolean, voxelBooleanField, voxelBooleanFieldShapes, voxelBooleanShapes, voxelField, voxelFieldFromShape, walkAssembly, windingNumbers, wire, wireFinder, wireLoop, wiresOfFace, withKernel, withKernelDir, withKernelPnt, withKernelVec, withQuality, withScope, withScopeResult, withScopeResultAsync, withTier, zip as zipResults };
|
|
6457
|
+
export { BaseSketcher2d, BlueprintSketcher, BrepBugError, BrepErrorCode, BrepWrapperError, BrepkitAdapter, CompoundSketch, DEFAULT_CAPABILITIES, DEG2RAD, DisposalScope, EXACT_BREP_CAPABILITIES, FaceSketcher, HASH_CODE_MAX, OK, OcctWasmAdapter, RAD2DEG, Sketch, Sketcher, Sketches, addChild, addHoles, addJoint, addMate, addStep, adjacentFaces, all, andThen, applyGlue, applyMatrix, approximateCurve, as2D, as3D, asTopo, assignRoles, autoHeal, bezier, blueprintToDXF, booleanPipeline, booleans_exports as booleans, boss, box, bsplineApprox, bug, cameraFromPlane, cameraLookAt, captureHint, cast, castShape, castShape3D, chamfer, chamferDistAngle as chamferDistAngleShape, chamferWithEvolution, checkAllInterferences, checkBoolean, checkInterference, circle, circularPattern, classifyPointOnFace, clearMeshCache, clone, closedWire, collect, collectShapes, colorFaces, colorShape, complexExtrude, composeTransforms, compound, compoundSketchExtrude, compoundSketchFace, compoundSketchLoft, compoundSketchRevolve, computationError, computeStraightSkeleton, cone, construction_exports as construction, convexHull, cornerFinder, countNodes, createAssembly, createAssemblyNode, createBlueprint, createCamera, createCompound, createCompoundBlueprint, createDistanceQuery, createEdge, createFace, createHandle, createHistory, createKernelHandle, createMeshCache, createNamedPlane, createOperationRegistry, createPlane, createRef, createRegistry, createShell, createSolid, createTaskQueue, createVertex, createWire, createWorkerClient, createWorkerHandler, csg_exports as csg, currentQuality, curve2dBoundingBox, curve2dDistanceFrom, curve2dFirstPoint, curve2dIsOnCurve, curve2dLastPoint, curve2dParameter, curve2dSplitAt, curve2dTangentAt, curveAxis, curveEndPoint, curveIsClosed, curveIsPeriodic, curveLength, curvePeriod, curvePointAt, curveStartPoint, curveTangentAt, cut, cut2D, cutAll, cutAllBisect, cutBlueprints, cutWithEvolution, cylinder, cylindricalJoint, defaultScorer, dequeueTask, describe, deserializeDrawing, deserializeHistory, fromBREP as deserializeShape, downcast, draft, draw, drawCircle, drawEllipse, drawFaceOutline, drawParametricFunction, drawPointsInterpolation, drawPolysides, drawProjection, drawRectangle, drawRoundedRectangle, drawSingleCircle, drawSingleEllipse, drawText, drawingChamfer, drawingCut, drawingFillet, drawingFuse, drawingIntersect, drawingToSketchOnPlane, drill, edgeFinder, edgesOfFace, ellipse, ellipseArc, ellipsoid, enqueueTask, err, exportAssemblySTEP, exportDXF, exportGlb, exportGltf, exportIGES, exportOBJ, exportSTEP, exportSTEPConfigured, exportSTL, exportThreeMF, extrude, extrudeAll, face, faceAxis, faceCenter, faceFinder, faceGeomType, faceOrientation, facesOfEdge, fieldBoolean, fieldContour, fieldOffset, fieldReinit, fieldShell, fill, filledFace, fillet, filletWithEvolution, findFacesByTag, findNode, findStep, fixSelfIntersection, fixShape, flatMap, flatten, flipFaceOrientation, flipOrientation, fontMetrics, forwardKinematics, fromBREP$1 as fromBREP, fromKernelDir, fromKernelPnt, fromKernelVec, fromNullable, fuse, fuse2D, fuseAll, fuseAllBisect, fuseBlueprints, fuseWithEvolution, gearGeometry, getActiveVoxelId, getBounds, getBounds2D, getCompSolids, getCurveType, getDisposalStats, getEdges, getFaceColor, getFaceOrigins, getFaceTags, getFaces, getFont, getHashCode, getShape as getHistoryShape, getKernel, getKernelCapabilities, getKernelTier, getNurbsCurveData, getNurbsSurfaceData, getOrientation, getOrientation2D, getPerformanceStats, getShapeColor, getShapeKind, getShells, getSingleFace, getSolids, getSurfaceType, getTagMetadata, getVertices, getVoxel, getWires, guidedSweep, heal, healFace, healSolid, healWire, helix, hull, importDXF, importGLB, importIGES, importOBJ, importSTEP, importSTL, importSVG, importSVGPathD, importThreeMF, init, initFromManifold, initFromOC, initVoxel, innerWires, interpolateCurve, intersect, intersect2D, intersectBlueprints, intersectWithEvolution, invalidateShapeCache, inverseKinematics, ioNs_exports as io, ioError, is2D, is3D, isChamferRadius, isClosedWire, isCompSolid, isCompound, isDisposeRequest, isEdge, isEmpty, isEqualShape, isErr, isErrorResponse, isFace, isFilletRadius, isInitRequest, isInside2D, isLive, isManifoldShell, isNumber, isOk, isOperationRequest, isOrientedFace, isPlanarFace, isPlanarWire, isProjectionPlane, isEmpty$1 as isQueueEmpty, isSameShape, isShape1D, isShape3D, isShell, isSolid, isSuccessResponse, isValid, isValidSolid, isVertex, isWire, iterCompSolids, iterEdges, iterFaces, iterShells, iterSolids, iterTopo, iterVertices, iterWires, jointTrajectory, jointTransform, kernelCall, kernelCallRaw, kernelCallScoped, kernelError, latticeInfill, latticeInfillShape, line, linearPattern, loadFont, loft, loftAll, makeBaseBox, makeExternalGear, makeInternalGear, makePlane, makePlanetaryGear, makeProjectedEdges, manifoldShell, map, mapBoth, mapErr, match, measureArea, measureCurvatureAt, measureCurvatureAtMid, measureDistance, measureDistanceProps, measureLength, measureLinearProps, measureSurfaceProps, measureVolume, measureVolumeProps, measurement_exports as measurement, mechanismDOF, mesh, meshEdges, meshMultiLOD, minkowski, mirror, mirror2D, mirrorDrawing, mirrorJoin, modifiers_exports as modifiers, modifyStep, moduleInitError, multiSectionSweep, normalAt, offset, offsetFace, offsetMesh, offsetShape, offsetWire2D, ok, or, orElse, organiseBlueprints, orientedFace, outerWire, patterns_exports as patterns, pendingCount, pipeline, pivotPlane, planarFace, planarJoint, planarWire, planetPlacements, pocket, pointOnSurface, pointsInside, polygon, polyhedron, polysideInnerRadius, polysidesBlueprint, positionOnCurve, prewarm, primitives_exports as primitives, prismaticJoint, projectEdges, projectPointOnFace, query_exports as query, queryError, rectangularPattern, registerHandler, registerKernel, registerKernelTier, registerOperation, registerShape, registerVoxel, rejectAll, removeChild, removeHolesFromFace, repairMesh, replayFrom, replayHistory, resetDisposalStats, resetPerformanceStats, resize, resolve, resolve3D, resolveDirection, resolvePlane, resolveRef, reverseCurve, revoluteJoint, revolve, roof, rotate, rotate2D, rotateDrawing, roundedRectangleBlueprint, scale, scale2D, scaleDrawing, box$1 as sdfBox, capsule as sdfCapsule, cone$1 as sdfCone, cylinder$1 as sdfCylinder, fieldAxialRamp as sdfFieldAxialRamp, fieldClamp as sdfFieldClamp, fieldConst as sdfFieldConst, fieldFromSdf as sdfFieldFromSdf, fieldRadialRamp as sdfFieldRadialRamp, lattice as sdfLattice, plane as sdfPlane, roundedBox as sdfRoundedBox, sphere as sdfSphere, strutLattice as sdfStrutLattice, sweep as sdfSweep, torus as sdfTorus, section, sectionToFace, serializeHistory, setJointValue, setJointValues, setShapeOrigin, setTagMetadata, sewShells, shape, shapeToMeshInput, shapeType, sharedEdges, shell, shellMesh, shellShape, shellWithEvolution, simplify, sketchCircle, sketchEllipse, sketchExtrude, sketchFace, sketchFaceOffset, sketchHelix, sketchLoft, sketchOnFace2D, sketchOnPlane2D, sketchParametricFunction, sketchPolysides, sketchRectangle, sketchRevolve, sketchRoundedRectangle, sketchSweep, sketchText, sketchWires, sketcherStateError, slice, solid, solidFromShell, solveAssembly, sphere$1 as sphere, sphericalJoint, split, stepCount, stepsFrom, stretch2D, subFace, supportExtrude, supportsConstraintSketch, supportsProjection, surfaceFromGrid, surfaceFromImage, sweep$1 as sweep, tagFaces, tangentArc, tap, tapErr, textBlueprints, textMetrics, thicken, thread, threePointArc, toBREP, toBufferGeometryData, toGroupedBufferGeometryData, toKernelVec, toLODGeometryData, toLineGeometryData, toSVGPathD, toVec2, toVec3, torus$1 as torus, tpmsLattice, transformCopy, transforms_exports as transforms, translate, translate2D, translateDrawing, translatePlane, tryCatch, tryCatchAsync, twistExtrude, typeCastError, undoLast, unsupportedError, unwrap, unwrapErr, unwrapOr, unwrapOrElse, updateNode, updateRoles, uvBounds, uvCoordinates, validSolid, validatePlanetary, validationError, variableFillet, vecAdd, vecAngle, vecCross, vecDistance, vecDot, vecEquals, vecIsZero, vecLength, vecLengthSq, vecNegate, vecNormalize, vecProjectToPlane, vecRepr, vecRotate, vecScale, vecSub, vertex, vertexFinder, vertexPosition, verticesOfEdge, voxelBoolean, voxelBooleanField, voxelBooleanFieldShapes, voxelBooleanShapes, voxelField, voxelFieldFromShape, walkAssembly, windingNumbers, wire, wireFinder, wireLoop, wiresOfFace, withKernel, withKernelDir, withKernelPnt, withKernelVec, withQuality, withScope, withScopeResult, withScopeResultAsync, withTier, zip as zipResults };
|
package/dist/index.d.ts
CHANGED
|
@@ -122,6 +122,7 @@ export { linearPattern, circularPattern } from './operations/patternFns.js';
|
|
|
122
122
|
export { createAssemblyNode, addChild, removeChild, updateNode, findNode, walkAssembly, countNodes, collectShapes, type AssemblyNode, type AssemblyNodeOptions, } from './operations/assemblyFns.js';
|
|
123
123
|
export { addMate, solveAssembly, type MateConstraint, type MateEntity, type AssemblySolveResult, } from './operations/mateFns.js';
|
|
124
124
|
export { revoluteJoint, prismaticJoint, cylindricalJoint, planarJoint, sphericalJoint, setJointValue, setJointValues, jointTransform, addJoint, forwardKinematics, mechanismDOF, type Joint, type JointDOF, type JointAxis, type JointType, type JointPose, type JointOptions, type CylindricalOptions, type PlanarOptions, type SphericalOptions, } from './operations/jointFns.js';
|
|
125
|
+
export { inverseKinematics, jointTrajectory, type IKTarget, type IKOptions, type IKResult, type TrajectorySample, } from './operations/ikFns.js';
|
|
125
126
|
export { createHistory, addStep, undoLast, findStep, getShape as getHistoryShape, stepCount, stepsFrom, registerShape, createRegistry, registerOperation, replayHistory, replayFrom, modifyStep, serializeHistory, deserializeHistory, type OperationStep, type ModelHistory, type SerializedHistory, type OperationFn, type OperationRegistry as HistoryOperationRegistry, } from './operations/historyFns.js';
|
|
126
127
|
export { measureVolume, measureArea, measureLength, measureDistance, measureDistanceProps, createDistanceQuery, measureVolumeProps, measureSurfaceProps, measureLinearProps, type PhysicalProps, type VolumeProps, type SurfaceProps, type LinearProps, type DistanceProps, measureCurvatureAt, measureCurvatureAtMid, type CurvatureResult, } from './measurement/measureFns.js';
|
|
127
128
|
export { checkInterference, checkAllInterferences, type InterferenceResult, type InterferencePair, } from './measurement/interferenceFns.js';
|
|
@@ -34,9 +34,11 @@ export interface SolverResult {
|
|
|
34
34
|
/**
|
|
35
35
|
* Solve assembly constraints analytically.
|
|
36
36
|
*
|
|
37
|
-
* Handles: fixed,
|
|
38
|
-
*
|
|
39
|
-
*
|
|
37
|
+
* Handles: fixed, concentric (axis-axis), angle (plane-plane orientation), and
|
|
38
|
+
* coincident/distance for any supported entity-type pair (plane-plane,
|
|
39
|
+
* plane-point, point-point, axis-axis, axis-point, and both point orders — see
|
|
40
|
+
* `TRANSLATIONAL_PAIRS`). For a positioning mate, entityA is the reference and
|
|
41
|
+
* entityB the dependent. Chain roots (nodes never positioned by a
|
|
40
42
|
* mate) and explicit `fixed` nodes anchor at the origin; constraints then resolve
|
|
41
43
|
* in topological order — each places its dependent against the reference's solved
|
|
42
44
|
* pose (rotation included), so multi-body chains compose. Returns
|
|
@@ -0,0 +1,58 @@
|
|
|
1
|
+
import { Vec3 } from '../core/types.js';
|
|
2
|
+
import { AssemblyNode } from './assemblyFns.js';
|
|
3
|
+
import { JointPose } from './jointFns.js';
|
|
4
|
+
type Quat = readonly [number, number, number, number];
|
|
5
|
+
/** A target for the end-effector: a world position, optionally an orientation. */
|
|
6
|
+
export interface IKTarget {
|
|
7
|
+
readonly position: Vec3;
|
|
8
|
+
/** Target orientation `[w, x, y, z]`. Omit for position-only IK. */
|
|
9
|
+
readonly rotation?: Quat;
|
|
10
|
+
}
|
|
11
|
+
export interface IKOptions {
|
|
12
|
+
/** Maximum solver iterations. Default 200. */
|
|
13
|
+
maxIterations?: number;
|
|
14
|
+
/** Convergence threshold on the residual norm. Default 1e-5. */
|
|
15
|
+
tolerance?: number;
|
|
16
|
+
/** Damping factor λ for the least-squares step. Default 0.05. */
|
|
17
|
+
damping?: number;
|
|
18
|
+
/** Initial joint values, keyed by child node (number or per-DOF array). */
|
|
19
|
+
seed?: Readonly<Record<string, number | readonly number[]>>;
|
|
20
|
+
/** Local point on the end-effector node to drive to the target. Default origin. */
|
|
21
|
+
tip?: Vec3;
|
|
22
|
+
}
|
|
23
|
+
export interface IKResult {
|
|
24
|
+
/** Solved joint values, keyed by child node, one entry per DOF. */
|
|
25
|
+
readonly values: Record<string, number[]>;
|
|
26
|
+
readonly converged: boolean;
|
|
27
|
+
readonly iterations: number;
|
|
28
|
+
/** Final residual norm (position, plus orientation when targeted). */
|
|
29
|
+
readonly error: number;
|
|
30
|
+
}
|
|
31
|
+
/**
|
|
32
|
+
* Solve for the joint values that place `endEffector` (offset by `tip`) at
|
|
33
|
+
* `target`, by damped-least-squares descent on a numerical Jacobian. Joint
|
|
34
|
+
* ranges are honored: every iterate is clamped to each DOF's `[min, max]`.
|
|
35
|
+
*
|
|
36
|
+
* Returns the solved per-DOF values keyed by child node (ready to pass to
|
|
37
|
+
* `forwardKinematics`), whether it converged, the iteration count, and the final
|
|
38
|
+
* residual norm. An end-effector with no driving joints, or an unreachable
|
|
39
|
+
* target, returns `converged: false` with the best configuration found.
|
|
40
|
+
*/
|
|
41
|
+
export declare function inverseKinematics(assembly: AssemblyNode, endEffector: string, target: IKTarget, options?: IKOptions): IKResult;
|
|
42
|
+
export interface TrajectorySample {
|
|
43
|
+
/** Normalized path parameter in `[0, 1]`. */
|
|
44
|
+
readonly t: number;
|
|
45
|
+
/** Interpolated joint values at this step, keyed by child node. */
|
|
46
|
+
readonly values: Record<string, number[]>;
|
|
47
|
+
/** Forward-kinematics world poses for every node at this step. */
|
|
48
|
+
readonly poses: Map<string, JointPose>;
|
|
49
|
+
}
|
|
50
|
+
/**
|
|
51
|
+
* Sample a straight-line path in joint space from `from` to `to` over `steps`
|
|
52
|
+
* segments, yielding `steps + 1` samples (inclusive of both endpoints). Each
|
|
53
|
+
* sample carries the interpolated per-DOF values (clamped to range) and the
|
|
54
|
+
* forward-kinematics poses of every node. Joints absent from `from`/`to` hold
|
|
55
|
+
* their stored value at both ends.
|
|
56
|
+
*/
|
|
57
|
+
export declare function jointTrajectory(assembly: AssemblyNode, from: Readonly<Record<string, number | readonly number[]>>, to: Readonly<Record<string, number | readonly number[]>>, steps: number): TrajectorySample[];
|
|
58
|
+
export {};
|
package/dist/operations.cjs
CHANGED
|
@@ -1,5 +1,5 @@
|
|
|
1
1
|
Object.defineProperty(exports, Symbol.toStringTag, { value: "Module" });
|
|
2
|
-
const require_threadFns = require("./threadFns-
|
|
2
|
+
const require_threadFns = require("./threadFns-ubwexS-v.cjs");
|
|
3
3
|
const require_loftFns = require("./loftFns-DiSsI2PM.cjs");
|
|
4
4
|
exports.addChild = require_threadFns.addChild;
|
|
5
5
|
exports.addJoint = require_threadFns.addJoint;
|
|
@@ -19,6 +19,8 @@ exports.findStep = require_threadFns.findStep;
|
|
|
19
19
|
exports.forwardKinematics = require_threadFns.forwardKinematics;
|
|
20
20
|
exports.getHistoryShape = require_threadFns.getShape;
|
|
21
21
|
exports.gridPattern = require_threadFns.gridPattern;
|
|
22
|
+
exports.inverseKinematics = require_threadFns.inverseKinematics;
|
|
23
|
+
exports.jointTrajectory = require_threadFns.jointTrajectory;
|
|
22
24
|
exports.jointTransform = require_threadFns.jointTransform;
|
|
23
25
|
exports.linearPattern = require_threadFns.linearPattern;
|
|
24
26
|
exports.mechanismDOF = require_threadFns.mechanismDOF;
|
package/dist/operations.d.ts
CHANGED
|
@@ -11,6 +11,7 @@ export { thread, type ThreadOptions } from './operations/threadFns.js';
|
|
|
11
11
|
export { linearPattern, circularPattern, gridPattern } from './operations/patternFns.js';
|
|
12
12
|
export { createAssemblyNode, addChild, removeChild, updateNode, findNode, walkAssembly, countNodes, collectShapes, type AssemblyNode, type AssemblyNodeOptions, } from './operations/assemblyFns.js';
|
|
13
13
|
export { revoluteJoint, prismaticJoint, cylindricalJoint, planarJoint, sphericalJoint, setJointValue, setJointValues, jointTransform, addJoint, forwardKinematics, mechanismDOF, type Joint, type JointDOF, type JointAxis, type JointType, type JointPose, type JointOptions, type CylindricalOptions, type PlanarOptions, type SphericalOptions, } from './operations/jointFns.js';
|
|
14
|
+
export { inverseKinematics, jointTrajectory, type IKTarget, type IKOptions, type IKResult, type TrajectorySample, } from './operations/ikFns.js';
|
|
14
15
|
export { exportAssemblySTEP, type ShapeOptions, type SupportedUnit, } from './operations/exporterFns.js';
|
|
15
16
|
export { createHistory, addStep, undoLast, findStep, getShape as getHistoryShape, stepCount, stepsFrom, registerShape, createRegistry, registerOperation, replayHistory, replayFrom, modifyStep, type OperationStep, type ModelHistory, type OperationFn, type OperationRegistry as HistoryOperationRegistry, } from './operations/historyFns.js';
|
|
16
17
|
export { type AssemblyExporter, createAssembly } from './operations/exporters.js';
|
package/dist/operations.js
CHANGED
|
@@ -1,3 +1,3 @@
|
|
|
1
|
-
import { B as
|
|
1
|
+
import { B as walkAssembly, C as mechanismDOF, D as setJointValue, E as revoluteJoint, F as countNodes, G as createAssembly, H as gridPattern, I as createAssemblyNode, L as findNode, N as addChild, O as setJointValues, P as collectShapes, R as removeChild, S as jointTransform, T as prismaticJoint, U as linearPattern, V as circularPattern, W as exportAssemblySTEP, _ as inverseKinematics, b as cylindricalJoint, c as modifyStep, d as replayFrom, f as replayHistory, g as undoLast, h as stepsFrom, i as createRegistry, k as sphericalJoint, l as registerOperation, m as stepCount, n as addStep, o as findStep, r as createHistory, s as getShape, t as thread, u as registerShape, v as jointTrajectory, w as planarJoint, x as forwardKinematics, y as addJoint, z as updateNode } from "./threadFns-D_CxgFXg.js";
|
|
2
2
|
import { d as twistExtrude, l as supportExtrude, o as complexExtrude, u as sweep } from "./loftFns-CsHOwded.js";
|
|
3
|
-
export { addChild, addJoint, addStep, circularPattern, collectShapes, complexExtrude, countNodes, createAssembly, createAssemblyNode, createHistory, createRegistry, cylindricalJoint, exportAssemblySTEP, findNode, findStep, forwardKinematics, getShape as getHistoryShape, gridPattern, jointTransform, linearPattern, mechanismDOF, modifyStep, planarJoint, prismaticJoint, registerOperation, registerShape, removeChild, replayFrom, replayHistory, revoluteJoint, setJointValue, setJointValues, sphericalJoint, stepCount, stepsFrom, supportExtrude, sweep, thread, twistExtrude, undoLast, updateNode, walkAssembly };
|
|
3
|
+
export { addChild, addJoint, addStep, circularPattern, collectShapes, complexExtrude, countNodes, createAssembly, createAssemblyNode, createHistory, createRegistry, cylindricalJoint, exportAssemblySTEP, findNode, findStep, forwardKinematics, getShape as getHistoryShape, gridPattern, inverseKinematics, jointTrajectory, jointTransform, linearPattern, mechanismDOF, modifyStep, planarJoint, prismaticJoint, registerOperation, registerShape, removeChild, replayFrom, replayHistory, revoluteJoint, setJointValue, setJointValues, sphericalJoint, stepCount, stepsFrom, supportExtrude, sweep, thread, twistExtrude, undoLast, updateNode, walkAssembly };
|
|
@@ -565,7 +565,7 @@ function jointTransform(joint, value = joint.value) {
|
|
|
565
565
|
const overrides = Array.isArray(value) ? value : void 0;
|
|
566
566
|
const primary = overrides ? void 0 : value;
|
|
567
567
|
const origin = joint.axis.origin;
|
|
568
|
-
let pose = IDENTITY_POSE;
|
|
568
|
+
let pose = IDENTITY_POSE$1;
|
|
569
569
|
for (let i = 0; i < joint.dofs.length; i++) {
|
|
570
570
|
const dof = joint.dofs[i];
|
|
571
571
|
if (!dof) continue;
|
|
@@ -582,7 +582,7 @@ function addJoint(assembly, joint) {
|
|
|
582
582
|
joints: [...existing, joint]
|
|
583
583
|
};
|
|
584
584
|
}
|
|
585
|
-
var IDENTITY_POSE = {
|
|
585
|
+
var IDENTITY_POSE$1 = {
|
|
586
586
|
position: [
|
|
587
587
|
0,
|
|
588
588
|
0,
|
|
@@ -639,7 +639,7 @@ function forwardKinematics(assembly, jointValues = {}) {
|
|
|
639
639
|
names.add(j.child);
|
|
640
640
|
}
|
|
641
641
|
const poses = /* @__PURE__ */ new Map();
|
|
642
|
-
for (const name of names) if (!byChild.has(name)) poses.set(name, IDENTITY_POSE);
|
|
642
|
+
for (const name of names) if (!byChild.has(name)) poses.set(name, IDENTITY_POSE$1);
|
|
643
643
|
const pending = [...joints];
|
|
644
644
|
let progress = true;
|
|
645
645
|
while (progress && pending.length > 0) {
|
|
@@ -656,7 +656,7 @@ function forwardKinematics(assembly, jointValues = {}) {
|
|
|
656
656
|
poses.set(j.child, composePose(parentPose, jointTransform(j, value)));
|
|
657
657
|
}
|
|
658
658
|
}
|
|
659
|
-
for (const j of pending) if (!poses.has(j.child)) poses.set(j.child, IDENTITY_POSE);
|
|
659
|
+
for (const j of pending) if (!poses.has(j.child)) poses.set(j.child, IDENTITY_POSE$1);
|
|
660
660
|
return poses;
|
|
661
661
|
}
|
|
662
662
|
/**
|
|
@@ -669,6 +669,299 @@ function mechanismDOF(assembly) {
|
|
|
669
669
|
return collectJoints(assembly).reduce((sum, j) => sum + j.dofs.length, 0);
|
|
670
670
|
}
|
|
671
671
|
//#endregion
|
|
672
|
+
//#region src/operations/ikFns.ts
|
|
673
|
+
var IDENTITY_POSE = {
|
|
674
|
+
position: [
|
|
675
|
+
0,
|
|
676
|
+
0,
|
|
677
|
+
0
|
|
678
|
+
],
|
|
679
|
+
rotation: [
|
|
680
|
+
1,
|
|
681
|
+
0,
|
|
682
|
+
0,
|
|
683
|
+
0
|
|
684
|
+
]
|
|
685
|
+
};
|
|
686
|
+
function applyPose(pose, p) {
|
|
687
|
+
const r = quatRotate(pose.rotation, p);
|
|
688
|
+
return [
|
|
689
|
+
r[0] + pose.position[0],
|
|
690
|
+
r[1] + pose.position[1],
|
|
691
|
+
r[2] + pose.position[2]
|
|
692
|
+
];
|
|
693
|
+
}
|
|
694
|
+
function quatConjugate(q) {
|
|
695
|
+
return [
|
|
696
|
+
q[0],
|
|
697
|
+
-q[1],
|
|
698
|
+
-q[2],
|
|
699
|
+
-q[3]
|
|
700
|
+
];
|
|
701
|
+
}
|
|
702
|
+
/**
|
|
703
|
+
* The rotation vector (axis · angle) taking orientation `from` to `to`, i.e. the
|
|
704
|
+
* angular error that drives `from` toward `to`. Returns the zero vector when the
|
|
705
|
+
* orientations coincide.
|
|
706
|
+
*/
|
|
707
|
+
function rotationError(from, to) {
|
|
708
|
+
let [w, x, y, z] = quatMultiply(to, quatConjugate(from));
|
|
709
|
+
const norm = Math.hypot(w, x, y, z) || 1;
|
|
710
|
+
w /= norm;
|
|
711
|
+
x /= norm;
|
|
712
|
+
y /= norm;
|
|
713
|
+
z /= norm;
|
|
714
|
+
if (w < 0) {
|
|
715
|
+
w = -w;
|
|
716
|
+
x = -x;
|
|
717
|
+
y = -y;
|
|
718
|
+
z = -z;
|
|
719
|
+
}
|
|
720
|
+
const s = Math.hypot(x, y, z);
|
|
721
|
+
if (s < 1e-12) return [
|
|
722
|
+
0,
|
|
723
|
+
0,
|
|
724
|
+
0
|
|
725
|
+
];
|
|
726
|
+
const k = 2 * Math.atan2(s, w) / s;
|
|
727
|
+
return [
|
|
728
|
+
x * k,
|
|
729
|
+
y * k,
|
|
730
|
+
z * k
|
|
731
|
+
];
|
|
732
|
+
}
|
|
733
|
+
/** Joints from the root down to `endEffector`, in root→leaf order. */
|
|
734
|
+
function chainTo(assembly, endEffector) {
|
|
735
|
+
const joints = [];
|
|
736
|
+
walkAssembly(assembly, (n) => {
|
|
737
|
+
if (n.joints) joints.push(...n.joints);
|
|
738
|
+
});
|
|
739
|
+
const byChild = /* @__PURE__ */ new Map();
|
|
740
|
+
for (const j of joints) byChild.set(j.child, j);
|
|
741
|
+
const chain = [];
|
|
742
|
+
const seen = /* @__PURE__ */ new Set();
|
|
743
|
+
let cur = endEffector;
|
|
744
|
+
while (cur && byChild.has(cur) && !seen.has(cur)) {
|
|
745
|
+
seen.add(cur);
|
|
746
|
+
const j = byChild.get(cur);
|
|
747
|
+
if (!j) break;
|
|
748
|
+
chain.push(j);
|
|
749
|
+
cur = j.parent;
|
|
750
|
+
}
|
|
751
|
+
return chain.reverse();
|
|
752
|
+
}
|
|
753
|
+
/** Read a Float64Array element as a definite number (dense matrices are full). */
|
|
754
|
+
function el(a, i) {
|
|
755
|
+
return a[i] ?? 0;
|
|
756
|
+
}
|
|
757
|
+
/** Solve `A x = b` for an `n×n` system by Gauss-Jordan with partial pivoting. */
|
|
758
|
+
function solveLinear(A, b, n) {
|
|
759
|
+
const w = n + 1;
|
|
760
|
+
const M = new Float64Array(n * w);
|
|
761
|
+
for (let r = 0; r < n; r++) {
|
|
762
|
+
for (let c = 0; c < n; c++) M[r * w + c] = el(A, r * n + c);
|
|
763
|
+
M[r * w + n] = el(b, r);
|
|
764
|
+
}
|
|
765
|
+
for (let col = 0; col < n; col++) {
|
|
766
|
+
let piv = col;
|
|
767
|
+
for (let r = col + 1; r < n; r++) if (Math.abs(el(M, r * w + col)) > Math.abs(el(M, piv * w + col))) piv = r;
|
|
768
|
+
if (Math.abs(el(M, piv * w + col)) < 1e-12) return null;
|
|
769
|
+
if (piv !== col) for (let k = col; k < w; k++) {
|
|
770
|
+
const tmp = el(M, col * w + k);
|
|
771
|
+
M[col * w + k] = el(M, piv * w + k);
|
|
772
|
+
M[piv * w + k] = tmp;
|
|
773
|
+
}
|
|
774
|
+
const d = el(M, col * w + col);
|
|
775
|
+
for (let k = col; k < w; k++) M[col * w + k] = el(M, col * w + k) / d;
|
|
776
|
+
for (let r = 0; r < n; r++) {
|
|
777
|
+
if (r === col) continue;
|
|
778
|
+
const f = el(M, r * w + col);
|
|
779
|
+
if (f === 0) continue;
|
|
780
|
+
for (let k = col; k < w; k++) M[r * w + k] = el(M, r * w + k) - f * el(M, col * w + k);
|
|
781
|
+
}
|
|
782
|
+
}
|
|
783
|
+
const x = new Float64Array(n);
|
|
784
|
+
for (let r = 0; r < n; r++) x[r] = el(M, r * w + n);
|
|
785
|
+
return x;
|
|
786
|
+
}
|
|
787
|
+
/** Flatten a chain's DOFs into a parameter vector with bounds, applying the seed. */
|
|
788
|
+
function flattenChain(chain, seed) {
|
|
789
|
+
const segments = [];
|
|
790
|
+
const q = [];
|
|
791
|
+
const lo = [];
|
|
792
|
+
const hi = [];
|
|
793
|
+
for (const j of chain) {
|
|
794
|
+
const s = seed?.[j.child];
|
|
795
|
+
segments.push({
|
|
796
|
+
child: j.child,
|
|
797
|
+
count: j.dofs.length
|
|
798
|
+
});
|
|
799
|
+
j.dofs.forEach((dof, i) => {
|
|
800
|
+
const v = (Array.isArray(s) ? s[i] : i === 0 ? s : void 0) ?? dof.value;
|
|
801
|
+
q.push(Math.min(dof.max, Math.max(dof.min, v)));
|
|
802
|
+
lo.push(dof.min);
|
|
803
|
+
hi.push(dof.max);
|
|
804
|
+
});
|
|
805
|
+
}
|
|
806
|
+
return {
|
|
807
|
+
segments,
|
|
808
|
+
q: Float64Array.from(q),
|
|
809
|
+
lo: Float64Array.from(lo),
|
|
810
|
+
hi: Float64Array.from(hi)
|
|
811
|
+
};
|
|
812
|
+
}
|
|
813
|
+
/** Slice a parameter vector back into per-joint value arrays keyed by child. */
|
|
814
|
+
function overridesOf(segments, q) {
|
|
815
|
+
const out = {};
|
|
816
|
+
let i = 0;
|
|
817
|
+
for (const seg of segments) {
|
|
818
|
+
const vals = [];
|
|
819
|
+
for (let k = 0; k < seg.count; k++) vals.push(el(q, i++));
|
|
820
|
+
out[seg.child] = vals;
|
|
821
|
+
}
|
|
822
|
+
return out;
|
|
823
|
+
}
|
|
824
|
+
/** Residual twist `e` (target − current) for a pose; returns its norm. */
|
|
825
|
+
function residualTwist(out, pose, target, tip, m) {
|
|
826
|
+
const pos = applyPose(pose, tip);
|
|
827
|
+
out[0] = target.position[0] - pos[0];
|
|
828
|
+
out[1] = target.position[1] - pos[1];
|
|
829
|
+
out[2] = target.position[2] - pos[2];
|
|
830
|
+
if (m === 6 && target.rotation) {
|
|
831
|
+
const r = rotationError(pose.rotation, target.rotation);
|
|
832
|
+
out[3] = r[0];
|
|
833
|
+
out[4] = r[1];
|
|
834
|
+
out[5] = r[2];
|
|
835
|
+
}
|
|
836
|
+
let s = 0;
|
|
837
|
+
for (let i = 0; i < m; i++) s += el(out, i) ** 2;
|
|
838
|
+
return Math.sqrt(s);
|
|
839
|
+
}
|
|
840
|
+
/** Finite-difference Jacobian: column `j` is the end-effector twist from δq[j]. */
|
|
841
|
+
function fillJacobian(J, q, n, m, base, tip, eps, tipPose) {
|
|
842
|
+
const basePos = applyPose(base, tip);
|
|
843
|
+
for (let j = 0; j < n; j++) {
|
|
844
|
+
const saved = el(q, j);
|
|
845
|
+
q[j] = saved + eps;
|
|
846
|
+
const p2 = tipPose(q);
|
|
847
|
+
q[j] = saved;
|
|
848
|
+
const pos2 = applyPose(p2, tip);
|
|
849
|
+
J[j] = (pos2[0] - basePos[0]) / eps;
|
|
850
|
+
J[n + j] = (pos2[1] - basePos[1]) / eps;
|
|
851
|
+
J[2 * n + j] = (pos2[2] - basePos[2]) / eps;
|
|
852
|
+
if (m === 6) {
|
|
853
|
+
const dr = rotationError(base.rotation, p2.rotation);
|
|
854
|
+
J[3 * n + j] = dr[0] / eps;
|
|
855
|
+
J[4 * n + j] = dr[1] / eps;
|
|
856
|
+
J[5 * n + j] = dr[2] / eps;
|
|
857
|
+
}
|
|
858
|
+
}
|
|
859
|
+
}
|
|
860
|
+
/** One damped-least-squares step: `Δq = Jᵀ(JJᵀ + λ²I)⁻¹ e`. */
|
|
861
|
+
function dlsStep(J, e, n, m, lambda) {
|
|
862
|
+
const A = new Float64Array(m * m);
|
|
863
|
+
const lam2 = lambda * lambda;
|
|
864
|
+
for (let r = 0; r < m; r++) for (let c = 0; c < m; c++) {
|
|
865
|
+
let s = 0;
|
|
866
|
+
for (let k = 0; k < n; k++) s += el(J, r * n + k) * el(J, c * n + k);
|
|
867
|
+
A[r * m + c] = s + (r === c ? lam2 : 0);
|
|
868
|
+
}
|
|
869
|
+
const y = solveLinear(A, e, m);
|
|
870
|
+
if (!y) return null;
|
|
871
|
+
const dq = new Float64Array(n);
|
|
872
|
+
for (let j = 0; j < n; j++) {
|
|
873
|
+
let v = 0;
|
|
874
|
+
for (let r = 0; r < m; r++) v += el(J, r * n + j) * el(y, r);
|
|
875
|
+
dq[j] = v;
|
|
876
|
+
}
|
|
877
|
+
return dq;
|
|
878
|
+
}
|
|
879
|
+
/**
|
|
880
|
+
* Solve for the joint values that place `endEffector` (offset by `tip`) at
|
|
881
|
+
* `target`, by damped-least-squares descent on a numerical Jacobian. Joint
|
|
882
|
+
* ranges are honored: every iterate is clamped to each DOF's `[min, max]`.
|
|
883
|
+
*
|
|
884
|
+
* Returns the solved per-DOF values keyed by child node (ready to pass to
|
|
885
|
+
* `forwardKinematics`), whether it converged, the iteration count, and the final
|
|
886
|
+
* residual norm. An end-effector with no driving joints, or an unreachable
|
|
887
|
+
* target, returns `converged: false` with the best configuration found.
|
|
888
|
+
*/
|
|
889
|
+
function inverseKinematics(assembly, endEffector, target, options = {}) {
|
|
890
|
+
const maxIterations = options.maxIterations ?? 200;
|
|
891
|
+
const tolerance = options.tolerance ?? 1e-5;
|
|
892
|
+
const lambda = options.damping ?? .05;
|
|
893
|
+
const tip = options.tip ?? [
|
|
894
|
+
0,
|
|
895
|
+
0,
|
|
896
|
+
0
|
|
897
|
+
];
|
|
898
|
+
const m = target.rotation !== void 0 ? 6 : 3;
|
|
899
|
+
const eps = 1e-6;
|
|
900
|
+
const { segments, q, lo, hi } = flattenChain(chainTo(assembly, endEffector), options.seed);
|
|
901
|
+
const n = q.length;
|
|
902
|
+
const tipPose = (state) => forwardKinematics(assembly, overridesOf(segments, state)).get(endEffector) ?? IDENTITY_POSE;
|
|
903
|
+
const e = new Float64Array(m);
|
|
904
|
+
const J = new Float64Array(m * n);
|
|
905
|
+
let pose = tipPose(q);
|
|
906
|
+
let err = residualTwist(e, pose, target, tip, m);
|
|
907
|
+
let iter = 0;
|
|
908
|
+
for (; iter < maxIterations && n > 0 && err > tolerance; iter++) {
|
|
909
|
+
fillJacobian(J, q, n, m, pose, tip, eps, tipPose);
|
|
910
|
+
const dq = dlsStep(J, e, n, m, lambda);
|
|
911
|
+
if (!dq) break;
|
|
912
|
+
for (let j = 0; j < n; j++) {
|
|
913
|
+
const next = el(q, j) + el(dq, j);
|
|
914
|
+
q[j] = Math.min(el(hi, j), Math.max(el(lo, j), next));
|
|
915
|
+
}
|
|
916
|
+
pose = tipPose(q);
|
|
917
|
+
err = residualTwist(e, pose, target, tip, m);
|
|
918
|
+
}
|
|
919
|
+
return {
|
|
920
|
+
values: overridesOf(segments, q),
|
|
921
|
+
converged: err <= tolerance,
|
|
922
|
+
iterations: iter,
|
|
923
|
+
error: err
|
|
924
|
+
};
|
|
925
|
+
}
|
|
926
|
+
/** Resolve a value spec (number, array, or absent) to a per-DOF array. */
|
|
927
|
+
function valuesOf(joint, spec) {
|
|
928
|
+
return joint.dofs.map((dof, i) => {
|
|
929
|
+
const v = (Array.isArray(spec) ? spec[i] : i === 0 ? spec : void 0) ?? dof.value;
|
|
930
|
+
return Math.min(dof.max, Math.max(dof.min, v));
|
|
931
|
+
});
|
|
932
|
+
}
|
|
933
|
+
/**
|
|
934
|
+
* Sample a straight-line path in joint space from `from` to `to` over `steps`
|
|
935
|
+
* segments, yielding `steps + 1` samples (inclusive of both endpoints). Each
|
|
936
|
+
* sample carries the interpolated per-DOF values (clamped to range) and the
|
|
937
|
+
* forward-kinematics poses of every node. Joints absent from `from`/`to` hold
|
|
938
|
+
* their stored value at both ends.
|
|
939
|
+
*/
|
|
940
|
+
function jointTrajectory(assembly, from, to, steps) {
|
|
941
|
+
const joints = [];
|
|
942
|
+
walkAssembly(assembly, (n) => {
|
|
943
|
+
if (n.joints) joints.push(...n.joints);
|
|
944
|
+
});
|
|
945
|
+
const ends = joints.map((j) => ({
|
|
946
|
+
child: j.child,
|
|
947
|
+
a: valuesOf(j, from[j.child]),
|
|
948
|
+
b: valuesOf(j, to[j.child])
|
|
949
|
+
}));
|
|
950
|
+
const count = Math.max(1, Math.floor(steps));
|
|
951
|
+
const samples = [];
|
|
952
|
+
for (let s = 0; s <= count; s++) {
|
|
953
|
+
const t = s / count;
|
|
954
|
+
const values = {};
|
|
955
|
+
for (const end of ends) values[end.child] = end.a.map((a, i) => a + ((end.b[i] ?? a) - a) * t);
|
|
956
|
+
samples.push({
|
|
957
|
+
t,
|
|
958
|
+
values,
|
|
959
|
+
poses: forwardKinematics(assembly, values)
|
|
960
|
+
});
|
|
961
|
+
}
|
|
962
|
+
return samples;
|
|
963
|
+
}
|
|
964
|
+
//#endregion
|
|
672
965
|
//#region src/operations/historyFns.ts
|
|
673
966
|
/** Create a new empty history. */
|
|
674
967
|
function createHistory() {
|
|
@@ -936,4 +1229,4 @@ function thread(options) {
|
|
|
936
1229
|
}
|
|
937
1230
|
}
|
|
938
1231
|
//#endregion
|
|
939
|
-
export {
|
|
1232
|
+
export { quatFromAxisAngle as A, walkAssembly as B, mechanismDOF as C, setJointValue as D, revoluteJoint as E, countNodes as F, createAssembly as G, gridPattern as H, createAssemblyNode as I, findNode as L, quatRotate as M, addChild as N, setJointValues as O, collectShapes as P, removeChild as R, jointTransform as S, prismaticJoint as T, linearPattern as U, circularPattern as V, exportAssemblySTEP as W, inverseKinematics as _, deserializeHistory as a, cylindricalJoint as b, modifyStep as c, replayFrom as d, replayHistory as f, undoLast as g, stepsFrom as h, createRegistry as i, quatFromTo as j, sphericalJoint as k, registerOperation as l, stepCount as m, addStep as n, findStep as o, serializeHistory as p, createHistory as r, getShape as s, thread as t, registerShape as u, jointTrajectory as v, planarJoint as w, forwardKinematics as x, addJoint as y, updateNode as z };
|
|
@@ -565,7 +565,7 @@ function jointTransform(joint, value = joint.value) {
|
|
|
565
565
|
const overrides = Array.isArray(value) ? value : void 0;
|
|
566
566
|
const primary = overrides ? void 0 : value;
|
|
567
567
|
const origin = joint.axis.origin;
|
|
568
|
-
let pose = IDENTITY_POSE;
|
|
568
|
+
let pose = IDENTITY_POSE$1;
|
|
569
569
|
for (let i = 0; i < joint.dofs.length; i++) {
|
|
570
570
|
const dof = joint.dofs[i];
|
|
571
571
|
if (!dof) continue;
|
|
@@ -582,7 +582,7 @@ function addJoint(assembly, joint) {
|
|
|
582
582
|
joints: [...existing, joint]
|
|
583
583
|
};
|
|
584
584
|
}
|
|
585
|
-
var IDENTITY_POSE = {
|
|
585
|
+
var IDENTITY_POSE$1 = {
|
|
586
586
|
position: [
|
|
587
587
|
0,
|
|
588
588
|
0,
|
|
@@ -639,7 +639,7 @@ function forwardKinematics(assembly, jointValues = {}) {
|
|
|
639
639
|
names.add(j.child);
|
|
640
640
|
}
|
|
641
641
|
const poses = /* @__PURE__ */ new Map();
|
|
642
|
-
for (const name of names) if (!byChild.has(name)) poses.set(name, IDENTITY_POSE);
|
|
642
|
+
for (const name of names) if (!byChild.has(name)) poses.set(name, IDENTITY_POSE$1);
|
|
643
643
|
const pending = [...joints];
|
|
644
644
|
let progress = true;
|
|
645
645
|
while (progress && pending.length > 0) {
|
|
@@ -656,7 +656,7 @@ function forwardKinematics(assembly, jointValues = {}) {
|
|
|
656
656
|
poses.set(j.child, composePose(parentPose, jointTransform(j, value)));
|
|
657
657
|
}
|
|
658
658
|
}
|
|
659
|
-
for (const j of pending) if (!poses.has(j.child)) poses.set(j.child, IDENTITY_POSE);
|
|
659
|
+
for (const j of pending) if (!poses.has(j.child)) poses.set(j.child, IDENTITY_POSE$1);
|
|
660
660
|
return poses;
|
|
661
661
|
}
|
|
662
662
|
/**
|
|
@@ -669,6 +669,299 @@ function mechanismDOF(assembly) {
|
|
|
669
669
|
return collectJoints(assembly).reduce((sum, j) => sum + j.dofs.length, 0);
|
|
670
670
|
}
|
|
671
671
|
//#endregion
|
|
672
|
+
//#region src/operations/ikFns.ts
|
|
673
|
+
var IDENTITY_POSE = {
|
|
674
|
+
position: [
|
|
675
|
+
0,
|
|
676
|
+
0,
|
|
677
|
+
0
|
|
678
|
+
],
|
|
679
|
+
rotation: [
|
|
680
|
+
1,
|
|
681
|
+
0,
|
|
682
|
+
0,
|
|
683
|
+
0
|
|
684
|
+
]
|
|
685
|
+
};
|
|
686
|
+
function applyPose(pose, p) {
|
|
687
|
+
const r = quatRotate(pose.rotation, p);
|
|
688
|
+
return [
|
|
689
|
+
r[0] + pose.position[0],
|
|
690
|
+
r[1] + pose.position[1],
|
|
691
|
+
r[2] + pose.position[2]
|
|
692
|
+
];
|
|
693
|
+
}
|
|
694
|
+
function quatConjugate(q) {
|
|
695
|
+
return [
|
|
696
|
+
q[0],
|
|
697
|
+
-q[1],
|
|
698
|
+
-q[2],
|
|
699
|
+
-q[3]
|
|
700
|
+
];
|
|
701
|
+
}
|
|
702
|
+
/**
|
|
703
|
+
* The rotation vector (axis · angle) taking orientation `from` to `to`, i.e. the
|
|
704
|
+
* angular error that drives `from` toward `to`. Returns the zero vector when the
|
|
705
|
+
* orientations coincide.
|
|
706
|
+
*/
|
|
707
|
+
function rotationError(from, to) {
|
|
708
|
+
let [w, x, y, z] = quatMultiply(to, quatConjugate(from));
|
|
709
|
+
const norm = Math.hypot(w, x, y, z) || 1;
|
|
710
|
+
w /= norm;
|
|
711
|
+
x /= norm;
|
|
712
|
+
y /= norm;
|
|
713
|
+
z /= norm;
|
|
714
|
+
if (w < 0) {
|
|
715
|
+
w = -w;
|
|
716
|
+
x = -x;
|
|
717
|
+
y = -y;
|
|
718
|
+
z = -z;
|
|
719
|
+
}
|
|
720
|
+
const s = Math.hypot(x, y, z);
|
|
721
|
+
if (s < 1e-12) return [
|
|
722
|
+
0,
|
|
723
|
+
0,
|
|
724
|
+
0
|
|
725
|
+
];
|
|
726
|
+
const k = 2 * Math.atan2(s, w) / s;
|
|
727
|
+
return [
|
|
728
|
+
x * k,
|
|
729
|
+
y * k,
|
|
730
|
+
z * k
|
|
731
|
+
];
|
|
732
|
+
}
|
|
733
|
+
/** Joints from the root down to `endEffector`, in root→leaf order. */
|
|
734
|
+
function chainTo(assembly, endEffector) {
|
|
735
|
+
const joints = [];
|
|
736
|
+
walkAssembly(assembly, (n) => {
|
|
737
|
+
if (n.joints) joints.push(...n.joints);
|
|
738
|
+
});
|
|
739
|
+
const byChild = /* @__PURE__ */ new Map();
|
|
740
|
+
for (const j of joints) byChild.set(j.child, j);
|
|
741
|
+
const chain = [];
|
|
742
|
+
const seen = /* @__PURE__ */ new Set();
|
|
743
|
+
let cur = endEffector;
|
|
744
|
+
while (cur && byChild.has(cur) && !seen.has(cur)) {
|
|
745
|
+
seen.add(cur);
|
|
746
|
+
const j = byChild.get(cur);
|
|
747
|
+
if (!j) break;
|
|
748
|
+
chain.push(j);
|
|
749
|
+
cur = j.parent;
|
|
750
|
+
}
|
|
751
|
+
return chain.reverse();
|
|
752
|
+
}
|
|
753
|
+
/** Read a Float64Array element as a definite number (dense matrices are full). */
|
|
754
|
+
function el(a, i) {
|
|
755
|
+
return a[i] ?? 0;
|
|
756
|
+
}
|
|
757
|
+
/** Solve `A x = b` for an `n×n` system by Gauss-Jordan with partial pivoting. */
|
|
758
|
+
function solveLinear(A, b, n) {
|
|
759
|
+
const w = n + 1;
|
|
760
|
+
const M = new Float64Array(n * w);
|
|
761
|
+
for (let r = 0; r < n; r++) {
|
|
762
|
+
for (let c = 0; c < n; c++) M[r * w + c] = el(A, r * n + c);
|
|
763
|
+
M[r * w + n] = el(b, r);
|
|
764
|
+
}
|
|
765
|
+
for (let col = 0; col < n; col++) {
|
|
766
|
+
let piv = col;
|
|
767
|
+
for (let r = col + 1; r < n; r++) if (Math.abs(el(M, r * w + col)) > Math.abs(el(M, piv * w + col))) piv = r;
|
|
768
|
+
if (Math.abs(el(M, piv * w + col)) < 1e-12) return null;
|
|
769
|
+
if (piv !== col) for (let k = col; k < w; k++) {
|
|
770
|
+
const tmp = el(M, col * w + k);
|
|
771
|
+
M[col * w + k] = el(M, piv * w + k);
|
|
772
|
+
M[piv * w + k] = tmp;
|
|
773
|
+
}
|
|
774
|
+
const d = el(M, col * w + col);
|
|
775
|
+
for (let k = col; k < w; k++) M[col * w + k] = el(M, col * w + k) / d;
|
|
776
|
+
for (let r = 0; r < n; r++) {
|
|
777
|
+
if (r === col) continue;
|
|
778
|
+
const f = el(M, r * w + col);
|
|
779
|
+
if (f === 0) continue;
|
|
780
|
+
for (let k = col; k < w; k++) M[r * w + k] = el(M, r * w + k) - f * el(M, col * w + k);
|
|
781
|
+
}
|
|
782
|
+
}
|
|
783
|
+
const x = new Float64Array(n);
|
|
784
|
+
for (let r = 0; r < n; r++) x[r] = el(M, r * w + n);
|
|
785
|
+
return x;
|
|
786
|
+
}
|
|
787
|
+
/** Flatten a chain's DOFs into a parameter vector with bounds, applying the seed. */
|
|
788
|
+
function flattenChain(chain, seed) {
|
|
789
|
+
const segments = [];
|
|
790
|
+
const q = [];
|
|
791
|
+
const lo = [];
|
|
792
|
+
const hi = [];
|
|
793
|
+
for (const j of chain) {
|
|
794
|
+
const s = seed?.[j.child];
|
|
795
|
+
segments.push({
|
|
796
|
+
child: j.child,
|
|
797
|
+
count: j.dofs.length
|
|
798
|
+
});
|
|
799
|
+
j.dofs.forEach((dof, i) => {
|
|
800
|
+
const v = (Array.isArray(s) ? s[i] : i === 0 ? s : void 0) ?? dof.value;
|
|
801
|
+
q.push(Math.min(dof.max, Math.max(dof.min, v)));
|
|
802
|
+
lo.push(dof.min);
|
|
803
|
+
hi.push(dof.max);
|
|
804
|
+
});
|
|
805
|
+
}
|
|
806
|
+
return {
|
|
807
|
+
segments,
|
|
808
|
+
q: Float64Array.from(q),
|
|
809
|
+
lo: Float64Array.from(lo),
|
|
810
|
+
hi: Float64Array.from(hi)
|
|
811
|
+
};
|
|
812
|
+
}
|
|
813
|
+
/** Slice a parameter vector back into per-joint value arrays keyed by child. */
|
|
814
|
+
function overridesOf(segments, q) {
|
|
815
|
+
const out = {};
|
|
816
|
+
let i = 0;
|
|
817
|
+
for (const seg of segments) {
|
|
818
|
+
const vals = [];
|
|
819
|
+
for (let k = 0; k < seg.count; k++) vals.push(el(q, i++));
|
|
820
|
+
out[seg.child] = vals;
|
|
821
|
+
}
|
|
822
|
+
return out;
|
|
823
|
+
}
|
|
824
|
+
/** Residual twist `e` (target − current) for a pose; returns its norm. */
|
|
825
|
+
function residualTwist(out, pose, target, tip, m) {
|
|
826
|
+
const pos = applyPose(pose, tip);
|
|
827
|
+
out[0] = target.position[0] - pos[0];
|
|
828
|
+
out[1] = target.position[1] - pos[1];
|
|
829
|
+
out[2] = target.position[2] - pos[2];
|
|
830
|
+
if (m === 6 && target.rotation) {
|
|
831
|
+
const r = rotationError(pose.rotation, target.rotation);
|
|
832
|
+
out[3] = r[0];
|
|
833
|
+
out[4] = r[1];
|
|
834
|
+
out[5] = r[2];
|
|
835
|
+
}
|
|
836
|
+
let s = 0;
|
|
837
|
+
for (let i = 0; i < m; i++) s += el(out, i) ** 2;
|
|
838
|
+
return Math.sqrt(s);
|
|
839
|
+
}
|
|
840
|
+
/** Finite-difference Jacobian: column `j` is the end-effector twist from δq[j]. */
|
|
841
|
+
function fillJacobian(J, q, n, m, base, tip, eps, tipPose) {
|
|
842
|
+
const basePos = applyPose(base, tip);
|
|
843
|
+
for (let j = 0; j < n; j++) {
|
|
844
|
+
const saved = el(q, j);
|
|
845
|
+
q[j] = saved + eps;
|
|
846
|
+
const p2 = tipPose(q);
|
|
847
|
+
q[j] = saved;
|
|
848
|
+
const pos2 = applyPose(p2, tip);
|
|
849
|
+
J[j] = (pos2[0] - basePos[0]) / eps;
|
|
850
|
+
J[n + j] = (pos2[1] - basePos[1]) / eps;
|
|
851
|
+
J[2 * n + j] = (pos2[2] - basePos[2]) / eps;
|
|
852
|
+
if (m === 6) {
|
|
853
|
+
const dr = rotationError(base.rotation, p2.rotation);
|
|
854
|
+
J[3 * n + j] = dr[0] / eps;
|
|
855
|
+
J[4 * n + j] = dr[1] / eps;
|
|
856
|
+
J[5 * n + j] = dr[2] / eps;
|
|
857
|
+
}
|
|
858
|
+
}
|
|
859
|
+
}
|
|
860
|
+
/** One damped-least-squares step: `Δq = Jᵀ(JJᵀ + λ²I)⁻¹ e`. */
|
|
861
|
+
function dlsStep(J, e, n, m, lambda) {
|
|
862
|
+
const A = new Float64Array(m * m);
|
|
863
|
+
const lam2 = lambda * lambda;
|
|
864
|
+
for (let r = 0; r < m; r++) for (let c = 0; c < m; c++) {
|
|
865
|
+
let s = 0;
|
|
866
|
+
for (let k = 0; k < n; k++) s += el(J, r * n + k) * el(J, c * n + k);
|
|
867
|
+
A[r * m + c] = s + (r === c ? lam2 : 0);
|
|
868
|
+
}
|
|
869
|
+
const y = solveLinear(A, e, m);
|
|
870
|
+
if (!y) return null;
|
|
871
|
+
const dq = new Float64Array(n);
|
|
872
|
+
for (let j = 0; j < n; j++) {
|
|
873
|
+
let v = 0;
|
|
874
|
+
for (let r = 0; r < m; r++) v += el(J, r * n + j) * el(y, r);
|
|
875
|
+
dq[j] = v;
|
|
876
|
+
}
|
|
877
|
+
return dq;
|
|
878
|
+
}
|
|
879
|
+
/**
|
|
880
|
+
* Solve for the joint values that place `endEffector` (offset by `tip`) at
|
|
881
|
+
* `target`, by damped-least-squares descent on a numerical Jacobian. Joint
|
|
882
|
+
* ranges are honored: every iterate is clamped to each DOF's `[min, max]`.
|
|
883
|
+
*
|
|
884
|
+
* Returns the solved per-DOF values keyed by child node (ready to pass to
|
|
885
|
+
* `forwardKinematics`), whether it converged, the iteration count, and the final
|
|
886
|
+
* residual norm. An end-effector with no driving joints, or an unreachable
|
|
887
|
+
* target, returns `converged: false` with the best configuration found.
|
|
888
|
+
*/
|
|
889
|
+
function inverseKinematics(assembly, endEffector, target, options = {}) {
|
|
890
|
+
const maxIterations = options.maxIterations ?? 200;
|
|
891
|
+
const tolerance = options.tolerance ?? 1e-5;
|
|
892
|
+
const lambda = options.damping ?? .05;
|
|
893
|
+
const tip = options.tip ?? [
|
|
894
|
+
0,
|
|
895
|
+
0,
|
|
896
|
+
0
|
|
897
|
+
];
|
|
898
|
+
const m = target.rotation !== void 0 ? 6 : 3;
|
|
899
|
+
const eps = 1e-6;
|
|
900
|
+
const { segments, q, lo, hi } = flattenChain(chainTo(assembly, endEffector), options.seed);
|
|
901
|
+
const n = q.length;
|
|
902
|
+
const tipPose = (state) => forwardKinematics(assembly, overridesOf(segments, state)).get(endEffector) ?? IDENTITY_POSE;
|
|
903
|
+
const e = new Float64Array(m);
|
|
904
|
+
const J = new Float64Array(m * n);
|
|
905
|
+
let pose = tipPose(q);
|
|
906
|
+
let err = residualTwist(e, pose, target, tip, m);
|
|
907
|
+
let iter = 0;
|
|
908
|
+
for (; iter < maxIterations && n > 0 && err > tolerance; iter++) {
|
|
909
|
+
fillJacobian(J, q, n, m, pose, tip, eps, tipPose);
|
|
910
|
+
const dq = dlsStep(J, e, n, m, lambda);
|
|
911
|
+
if (!dq) break;
|
|
912
|
+
for (let j = 0; j < n; j++) {
|
|
913
|
+
const next = el(q, j) + el(dq, j);
|
|
914
|
+
q[j] = Math.min(el(hi, j), Math.max(el(lo, j), next));
|
|
915
|
+
}
|
|
916
|
+
pose = tipPose(q);
|
|
917
|
+
err = residualTwist(e, pose, target, tip, m);
|
|
918
|
+
}
|
|
919
|
+
return {
|
|
920
|
+
values: overridesOf(segments, q),
|
|
921
|
+
converged: err <= tolerance,
|
|
922
|
+
iterations: iter,
|
|
923
|
+
error: err
|
|
924
|
+
};
|
|
925
|
+
}
|
|
926
|
+
/** Resolve a value spec (number, array, or absent) to a per-DOF array. */
|
|
927
|
+
function valuesOf(joint, spec) {
|
|
928
|
+
return joint.dofs.map((dof, i) => {
|
|
929
|
+
const v = (Array.isArray(spec) ? spec[i] : i === 0 ? spec : void 0) ?? dof.value;
|
|
930
|
+
return Math.min(dof.max, Math.max(dof.min, v));
|
|
931
|
+
});
|
|
932
|
+
}
|
|
933
|
+
/**
|
|
934
|
+
* Sample a straight-line path in joint space from `from` to `to` over `steps`
|
|
935
|
+
* segments, yielding `steps + 1` samples (inclusive of both endpoints). Each
|
|
936
|
+
* sample carries the interpolated per-DOF values (clamped to range) and the
|
|
937
|
+
* forward-kinematics poses of every node. Joints absent from `from`/`to` hold
|
|
938
|
+
* their stored value at both ends.
|
|
939
|
+
*/
|
|
940
|
+
function jointTrajectory(assembly, from, to, steps) {
|
|
941
|
+
const joints = [];
|
|
942
|
+
walkAssembly(assembly, (n) => {
|
|
943
|
+
if (n.joints) joints.push(...n.joints);
|
|
944
|
+
});
|
|
945
|
+
const ends = joints.map((j) => ({
|
|
946
|
+
child: j.child,
|
|
947
|
+
a: valuesOf(j, from[j.child]),
|
|
948
|
+
b: valuesOf(j, to[j.child])
|
|
949
|
+
}));
|
|
950
|
+
const count = Math.max(1, Math.floor(steps));
|
|
951
|
+
const samples = [];
|
|
952
|
+
for (let s = 0; s <= count; s++) {
|
|
953
|
+
const t = s / count;
|
|
954
|
+
const values = {};
|
|
955
|
+
for (const end of ends) values[end.child] = end.a.map((a, i) => a + ((end.b[i] ?? a) - a) * t);
|
|
956
|
+
samples.push({
|
|
957
|
+
t,
|
|
958
|
+
values,
|
|
959
|
+
poses: forwardKinematics(assembly, values)
|
|
960
|
+
});
|
|
961
|
+
}
|
|
962
|
+
return samples;
|
|
963
|
+
}
|
|
964
|
+
//#endregion
|
|
672
965
|
//#region src/operations/historyFns.ts
|
|
673
966
|
/** Create a new empty history. */
|
|
674
967
|
function createHistory() {
|
|
@@ -1044,6 +1337,18 @@ Object.defineProperty(exports, "gridPattern", {
|
|
|
1044
1337
|
return gridPattern;
|
|
1045
1338
|
}
|
|
1046
1339
|
});
|
|
1340
|
+
Object.defineProperty(exports, "inverseKinematics", {
|
|
1341
|
+
enumerable: true,
|
|
1342
|
+
get: function() {
|
|
1343
|
+
return inverseKinematics;
|
|
1344
|
+
}
|
|
1345
|
+
});
|
|
1346
|
+
Object.defineProperty(exports, "jointTrajectory", {
|
|
1347
|
+
enumerable: true,
|
|
1348
|
+
get: function() {
|
|
1349
|
+
return jointTrajectory;
|
|
1350
|
+
}
|
|
1351
|
+
});
|
|
1047
1352
|
Object.defineProperty(exports, "jointTransform", {
|
|
1048
1353
|
enumerable: true,
|
|
1049
1354
|
get: function() {
|