@code3d/core 0.0.1-alpha.4 → 0.0.1-alpha.6

package/src/library/relation-solver.ts

@@ -1,8 +1,10 @@
 import {
   axisRotation,
+  externalRotationFrame,
   solveBodies as solveBoundBodies,
   type BodyRelation as BoundRelation,
-  type BodyRotation,
+  type BodyAction,
+  solveTranslations,
 } from './bound-solver.js';
 import {
   alignmentResidual,
@@ -24,16 +26,14 @@ import {
   composeTransforms,
   identityRigidTransform,
   invertTransform,
-  origin,
   rotateVector,
   rotation,
-  translation,
   type Quaternion,
   type RigidTransform,
   type Vec3,
 } from './spatial.js';
 
-export {axisRotation, type BodyRotation};
+export {axisRotation, type BodyAction};
 type AlignEndpoint = Readonly<{
   body: number;
   geometry: AlignmentGeometry;
@@ -44,76 +44,70 @@ type AlignRelation = Readonly<{
   id: string;
   source: AlignEndpoint;
   target: AlignEndpoint;
-  offset: Vec3 | undefined;
-  rotations: readonly BodyRotation[];
 }>;
 type Relation = (BoundRelation & {kind: 'on'}) | AlignRelation;
-export type Body = Readonly<{name: string; relations: readonly Relation[]}>;
+export type Body = Readonly<{
+  name: string;
+  relations: readonly Relation[];
+  initial?: RigidTransform;
+  transformations?: readonly BodyAction[];
+}>;
 
-/** Authored actions apply after satisfying the relation, about self or an external axis. */
-export function beforeRelation(
+function applyExternalRotation(
   pose: RigidTransform,
-  relation: Pick<Relation, 'kind' | 'rotations' | 'offset' | 'target'>,
+  action: Exclude<BodyAction, {offset: Vec3} | {local: RigidTransform}>,
+  reference: RigidTransform,
+  inverse = false,
+): RigidTransform {
+  const {center, quaternion} = externalRotationFrame(
+    action,
+    pose.quaternion,
+    reference.quaternion,
+    inverse,
+  );
+  const point = addVectors(reference.position, center);
+  return composeTransforms(
+    {quaternion, position: subtract(point, rotateVector(point, quaternion))},
+    pose,
+  );
+}
+
+/** Undo exactly the authored prefix, in reverse call order. */
+export function beforeTransformations(
+  pose: RigidTransform,
+  relation: {
+    actions: readonly BodyAction[];
+  },
   poses: readonly RigidTransform[],
-  owner: number,
 ): RigidTransform {
-  for (const action of [...relation.rotations].reverse())
-    pose =
-      'local' in action
-        ? composeTransforms(pose, invertTransform(action.local))
-        : composeTransforms(
-            axisRotation(
-              composeTransforms(poses[action.body], action.axis),
-              -action.angle,
-            ),
-            pose,
-          );
-  if (relation.kind === 'align' && relation.offset) {
-    const target =
-      relation.target.body === owner ? pose : poses[relation.target.body];
-    const frame = composeTransforms(target, relation.target.transform);
-    pose = {
-      ...pose,
-      position: subtract(
-        pose.position,
-        rotateVector(relation.offset, frame.quaternion),
-      ),
-    };
+  for (const action of [...relation.actions].reverse()) {
+    if ('offset' in action) {
+      pose = {...pose, position: subtract(pose.position, action.offset)};
+    } else
+      pose =
+        'local' in action
+          ? composeTransforms(pose, invertTransform(action.local))
+          : applyExternalRotation(pose, action, poses[action.body], true);
   }
   return pose;
 }
 
-function afterRelation(
+export function afterTransformations(
   pose: RigidTransform,
-  relation: Relation,
+  relation: {
+    actions: readonly BodyAction[];
+  },
   poses: readonly RigidTransform[],
-  owner: number,
 ): RigidTransform {
-  if (relation.kind === 'align' && relation.offset) {
-    const target =
-      relation.target.body === owner ? pose : poses[relation.target.body];
-    pose = {
-      ...pose,
-      position: addVectors(
-        pose.position,
-        rotateVector(
-          relation.offset,
-          composeTransforms(target, relation.target.transform).quaternion,
-        ),
-      ),
-    };
+  for (const action of relation.actions) {
+    if ('offset' in action) {
+      pose = {...pose, position: addVectors(pose.position, action.offset)};
+    } else
+      pose =
+        'local' in action
+          ? composeTransforms(pose, action.local)
+          : applyExternalRotation(pose, action, poses[action.body]);
   }
-  for (const action of relation.rotations)
-    pose =
-      'local' in action
-        ? composeTransforms(pose, action.local)
-        : composeTransforms(
-            axisRotation(
-              composeTransforms(poses[action.body], action.axis),
-              action.angle,
-            ),
-            pose,
-          );
   return pose;
 }
 
@@ -271,8 +265,33 @@ export function solveBodies(
       if (relation.kind === 'align')
         validateAlignment(relation.source.geometry, relation.target.geometry);
   const active = bodies.flatMap((body, i) =>
-    body.relations.length ? [i] : [],
+    body.relations.length || body.transformations?.length ? [i] : [],
   );
+  const unplace = (
+    pose: RigidTransform,
+    owner: number,
+    poses: readonly RigidTransform[],
+  ) =>
+    beforeTransformations(
+      pose,
+      {actions: bodies[owner].transformations ?? []},
+      poses,
+    );
+  const place = (
+    pose: RigidTransform,
+    owner: number,
+    poses: readonly RigidTransform[],
+  ) =>
+    afterTransformations(
+      pose,
+      {actions: bodies[owner].transformations ?? []},
+      poses,
+    );
+  const baselinePose = (
+    pose: RigidTransform,
+    poses: readonly RigidTransform[],
+    owner: number,
+  ) => unplace(pose, owner, poses);
   const flexible = bodies.map(body =>
     body.relations.some(r => r.kind === 'align'),
   );
@@ -287,31 +306,13 @@ export function solveBodies(
   ]
     .sort(([a], [b]) => a.localeCompare(b))
     .map(([, entry]) => entry);
-  let poses = bodies.map(() => identityRigidTransform);
-  for (const owner of active) {
-    const authored =
-      unique.find(
-        entry => entry.owner === owner && entry.relation.rotations.length,
-      )?.relation ??
-      unique.find(
-        entry =>
-          entry.owner === owner &&
-          entry.relation.kind === 'align' &&
-          entry.relation.offset,
-      )?.relation;
-    if (authored)
-      poses[owner] = afterRelation(
-        identityRigidTransform,
-        authored,
-        poses,
-        owner,
-      );
-  }
+  let poses = bodies.map(body => body.initial ?? identityRigidTransform);
+  for (const owner of active) poses[owner] = place(poses[owner], owner, poses);
   for (const {owner, relation} of unique) {
     if (relation.kind !== 'align') continue;
     const sourceSelf = relation.source.body === owner;
     if (relation.target.body === relation.source.body) continue;
-    const baseline = beforeRelation(poses[owner], relation, poses, owner);
+    const baseline = baselinePose(poses[owner], poses, owner);
     const source = transformGeometry(
         relation.source.geometry,
         relation.source.body === owner ? baseline : poses[relation.source.body],
@@ -323,12 +324,7 @@ export function solveBodies(
     const seed = sourceSelf
       ? alignmentSeed(source, target)
       : alignmentSeed(target, source);
-    poses[owner] = afterRelation(
-      composeTransforms(seed, baseline),
-      relation,
-      poses,
-      owner,
-    );
+    poses[owner] = place(composeTransforms(seed, baseline), owner, poses);
   }
   const geometryScale = Math.max(
     1,
@@ -340,12 +336,25 @@ export function solveBodies(
         : [],
     ),
   );
-  const variables = active.flatMap(body =>
-    Array.from({length: 6}, (_, axis) => ({body, axis})),
+  const fullVariables = active.flatMap(body =>
+    // Bound-only bodies without authored rotations have exactly identity
+    // orientation. Do not numerically perturb those fixed axes: native bound
+    // queries can introduce noise that prevents convergence at the identity.
+    Array.from(
+      {
+        length:
+          flexible[body] ||
+          bodies[body].transformations?.some(action => !('offset' in action))
+            ? 6
+            : 3,
+      },
+      (_, axis) => ({body, axis}),
+    ),
   );
+  let variables = fullVariables.filter(variable => variable.axis < 3);
   const residual = (candidate: readonly RigidTransform[]): number[] => [
     ...unique.flatMap(({owner, relation: r}) => {
-      const baseline = beforeRelation(candidate[owner], r, candidate, owner);
+      const baseline = baselinePose(candidate[owner], candidate, owner);
       const source =
           r.source.body === owner ? baseline : candidate[r.source.body],
         target = r.target.body === owner ? baseline : candidate[r.target.body];
@@ -369,29 +378,28 @@ export function solveBodies(
         source.position,
         rotateVector(center, frame.quaternion),
       );
-      const b = composeTransforms(
-        frame,
-        translation(r.offset ?? origin),
-      ).position;
+      const b = frame.position;
       const delta = rotateVector(
         subtract(a, b),
         invertTransform(frame).quaternion,
       );
-      return r.offset ? [...delta] : [delta[1]];
+      return [delta[1]];
     }),
     ...active.flatMap(owner => {
       if (flexible[owner]) return [];
-      const rotations = bodies[owner].relations.filter(r => r.rotations.length);
-      return rotations.length
-        ? rotations.flatMap(r =>
-            beforeRelation(
-              candidate[owner],
-              r,
-              candidate,
-              owner,
-            ).quaternion.slice(0, 3),
-          )
-        : candidate[owner].quaternion.slice(0, 3);
+      const initial = bodies[owner].initial ?? identityRigidTransform;
+      const unplaced = unplace(candidate[owner], owner, candidate);
+      const orientationResidual = (pose: RigidTransform) =>
+        composeTransforms(
+          invertTransform(rotation(initial.quaternion)),
+          rotation(pose.quaternion),
+        ).quaternion.slice(0, 3);
+      return [
+        ...(bodies[owner].relations.length
+          ? []
+          : subtract(unplaced.position, initial.position)),
+        ...orientationResidual(unplaced),
+      ];
     }),
   ];
   const perturb = (
@@ -420,12 +428,80 @@ export function solveBodies(
       };
     });
   };
+  // Restore bound-only bodies' free translations without breaking alignments to
+  // dependent bodies. Every active translation participates in the nullspace;
+  // only bound-only bodies contribute preferences for their incoming pose.
+  const settleBoundTranslations = (): readonly RigidTransform[] => {
+    if (!active.some(owner => !flexible[owner])) return poses;
+    const free = active.flatMap(body => [0, 1, 2].map(axis => ({body, axis})));
+    if (!free.length) return poses;
+    const shift = (steps: readonly number[]) => {
+      const result = poses.map(pose => ({
+        ...pose,
+        position: [...pose.position] as [number, number, number],
+      }));
+      free.forEach(({body, axis}, i) => {
+        result[body].position[axis] += steps[i];
+      });
+      return result;
+    };
+    const preferences = (candidate: readonly RigidTransform[]) =>
+      unique
+        .filter(({owner}) => !flexible[owner])
+        .flatMap(({owner}) =>
+          subtract(
+            baselinePose(candidate[owner], candidate, owner).position,
+            bodies[owner].initial?.position ?? [0, 0, 0],
+          ),
+        );
+    const h = 1e-5;
+    const initial = residual(poses),
+      preference = preferences(poses);
+    const columns = free.map((_, i) => {
+      const plus = shift(free.map((_, j) => (i === j ? h : 0)));
+      const minus = shift(free.map((_, j) => (i === j ? -h : 0)));
+      const a = residual(plus),
+        b = residual(minus),
+        p = preferences(plus),
+        q = preferences(minus);
+      return {
+        constraints: a.map((v, j) => (v - b[j]) / (2 * h)),
+        preferences: p.map((v, j) => (v - q[j]) / (2 * h)),
+      };
+    });
+    const step = solveTranslations(
+      initial.map((value, i) => ({
+        id: 'bound translation',
+        value: -value,
+        coefficients: columns.map(column => column.constraints[i]),
+      })),
+      preference.map((value, i) => ({
+        id: 'pre-action position',
+        value: -value,
+        coefficients: columns.map(column => column.preferences[i]),
+      })),
+      free.length,
+    );
+    const candidate = shift(step);
+    // A nonlinear geometric locus need not admit its linearized free step.
+    // Preserve the valid geometric solution when that direction is not free.
+    return residual(candidate).every(value => Math.abs(value) < 1e-8)
+      ? candidate
+      : poses;
+  };
   let values = residual(poses),
     damping = 1e-5;
   const norm = (values: readonly number[]) =>
     values.reduce((sum, x) => sum + x * x, 0);
-  for (let iteration = 0; iteration < 160; iteration++) {
-    if (values.every(v => Math.abs(v) < 1e-8)) return poses;
+  for (let iteration = 0; iteration < 168; iteration++) {
+    // First settle translation along the seeded orientations. This prevents
+    // unconstrained rotations from tilting a contact chain merely to move it.
+    // Relations that genuinely require rotation then use the full rigid solve.
+    if (iteration === 8) {
+      variables = fullVariables;
+      damping = 1e-5;
+    }
+    if (values.every(v => Math.abs(v) < 1e-8)) return settleBoundTranslations();
     const h = 1e-6;
     const columns = variables.map((_, i) => {
       const shifted = residual(
@@ -434,7 +510,15 @@ export function solveBodies(
           variables.map((_, j) => (i === j ? h : 0)),
         ),
       );
-      return values.map((v, j) => (shifted[j] - v) / h);
+      const opposite = residual(
+        perturb(
+          poses,
+          variables.map((_, j) => (i === j ? -h : 0)),
+        ),
+      );
+      // A directional bound has a cusp at an axis-aligned pose. A forward
+      // difference invents a rotation gradient there; use both sides.
+      return values.map((_, j) => (shifted[j] - opposite[j]) / (2 * h));
     });
     const multiply = (a: readonly number[], b: readonly number[]) =>
       a.reduce((sum, x, i) => sum + x * b[i], 0);