fix(solver): use world-anchored reference normal in PlanarConstraint distance residual

PlanarConstraint's point-in-plane residual used a body-attached normal
(z_j) that rotates with body_j. When combined with CylindricalConstraint,
which allows rotation about the shared axis, the plane normal tilts
during Newton iteration. This allows the body to drift along the cylinder
axis while technically satisfying the rotated distance residual.

Fix: snapshot the world-frame normal at system-build time (as Const
nodes) and use it in the distance residual. The cross-product residuals
still use body-attached normals to enforce parallelism. Only the
distance residual needs the fixed reference direction.

Works for any offset value: (p_i - p_j) · z_ref - offset = 0.

.mailmap

@@ -1,7 +0,0 @@
-forbes <contact@kindred-systems.com> forbes <joseph.forbes@kindred-systems.com>
-forbes <contact@kindred-systems.com> forbes <zoe.forbes@kindred-systems.com>
-forbes <contact@kindred-systems.com> forbes-0023 <joseph.forbes@kindred-systems.com>
-forbes <contact@kindred-systems.com> forbes-0023 <zoe.forbes@kindred-systems.com>
-forbes <contact@kindred-systems.com> josephforbes23 <joseph.forbes@kindred-systems.com>
-forbes <contact@kindred-systems.com> Zoe Forbes <forbes@copernicus-9.kindred.internal>
-forbes <contact@kindred-systems.com> admin <admin@kindred-systems.com>

kindred_solver/constraints.py

@@ -416,6 +416,7 @@ class PlanarConstraint(ConstraintBase):
         self.marker_j_pos = marker_j_pos
         self.marker_j_quat = marker_j_quat
         self.offset = offset
+        self.reference_normal = None  # Set by _build_system; world-frame Const normal
 
     def residuals(self) -> List[Expr]:
         # Parallel normals (3 cross-product residuals, rank 2 at solution)
@@ -423,10 +424,20 @@ class PlanarConstraint(ConstraintBase):
         z_j = marker_z_axis(self.body_j, self.marker_j_quat)
         cx, cy, cz = cross3(z_i, z_j)
 
-        # Point-in-plane
+        # Point-in-plane distance — use world-anchored reference normal when
+        # available so the constraining plane direction doesn't rotate with
+        # the body (prevents axial drift when combined with Cylindrical).
         p_i = self.body_i.world_point(*self.marker_i_pos)
         p_j = self.body_j.world_point(*self.marker_j_pos)
-        d = point_plane_distance(p_i, p_j, z_j)
+        if self.reference_normal is not None:
+            nz = (
+                Const(self.reference_normal[0]),
+                Const(self.reference_normal[1]),
+                Const(self.reference_normal[2]),
+            )
+        else:
+            nz = z_j
+        d = point_plane_distance(p_i, p_j, nz)
         if self.offset != 0.0:
             d = d - Const(self.offset)
 

kindred_solver/preference.py

@@ -283,11 +283,18 @@ def _planar_half_space(
     # Use the normal dot product as the primary indicator when the point
     # is already on the plane (distance ≈ 0).
     if abs(d_val) < 1e-10:
-        # Point is on the plane — track normal direction instead
+        # Point is on the plane — track normal direction only.
+        # No correction: when combined with rotational joints (Cylindrical,
+        # Revolute), the body's marker normal rotates legitimately and
+        # reflecting through the plane would fight the rotation.
         def indicator(e: dict[str, float]) -> float:
             return dot_expr.eval(e)
 
-        ref_sign = normal_ref_sign
+        return HalfSpace(
+            constraint_index=constraint_idx,
+            reference_sign=normal_ref_sign,
+            indicator_fn=indicator,
+        )
     else:
         # Point is off-plane — track which side
         def indicator(e: dict[str, float]) -> float:

kindred_solver/solver.py

@@ -39,6 +39,7 @@ from .constraints import (
     UniversalConstraint,
 )
 from .decompose import decompose, solve_decomposed
+from .geometry import marker_z_axis
 from .diagnostics import find_overconstrained
 from .dof import count_dof
 from .entities import RigidBody
@@ -202,9 +203,7 @@ class KindredSolver(kcsolve.IKCSolver):
 
         # Build result
         result = kcsolve.SolveResult()
-        result.status = (
-            kcsolve.SolveStatus.Success if converged else kcsolve.SolveStatus.Failed
-        )
+        result.status = kcsolve.SolveStatus.Success if converged else kcsolve.SolveStatus.Failed
         result.dof = dof
 
         # Diagnostics on failure
@@ -229,9 +228,7 @@ class KindredSolver(kcsolve.IKCSolver):
         )
         if not converged and result.diagnostics:
             for d in result.diagnostics:
-                log.warning(
-                    "  diagnostic: [%s] %s — %s", d.kind, d.constraint_id, d.detail
-                )
+                log.warning("  diagnostic: [%s] %s — %s", d.kind, d.constraint_id, d.detail)
 
         return result
 
@@ -331,9 +328,7 @@ class KindredSolver(kcsolve.IKCSolver):
         # Build result
         dof = count_dof(residuals, system.params, jac_exprs=jac_exprs)
         result = kcsolve.SolveResult()
-        result.status = (
-            kcsolve.SolveStatus.Success if converged else kcsolve.SolveStatus.Failed
-        )
+        result.status = kcsolve.SolveStatus.Success if converged else kcsolve.SolveStatus.Failed
         result.dof = dof
         result.placements = _extract_placements(system.params, system.bodies)
 
@@ -413,9 +408,7 @@ class KindredSolver(kcsolve.IKCSolver):
         # old and new quaternion — if we're in the -q branch, that
         # shows up as a ~340° flip and gets rejected.
         dragged_ids = self._drag_parts or set()
-        _enforce_quat_continuity(
-            params, cache.system.bodies, cache.pre_step_quats, dragged_ids
-        )
+        _enforce_quat_continuity(params, cache.system.bodies, cache.pre_step_quats, dragged_ids)
 
         # Update the stored quaternions for the next drag step
         env = params.get_env()
@@ -424,9 +417,7 @@ class KindredSolver(kcsolve.IKCSolver):
                 cache.pre_step_quats[body.part_id] = body.extract_quaternion(env)
 
         result = kcsolve.SolveResult()
-        result.status = (
-            kcsolve.SolveStatus.Success if converged else kcsolve.SolveStatus.Failed
-        )
+        result.status = kcsolve.SolveStatus.Success if converged else kcsolve.SolveStatus.Failed
         result.dof = -1  # skip DOF counting during drag for speed
         result.placements = _extract_placements(params, cache.system.bodies)
 
@@ -510,34 +501,18 @@ def _enforce_quat_continuity(
     pre_step_quats: dict,
     dragged_ids: set,
 ) -> None:
-    """Ensure solved quaternions stay close to the previous step.
+    """Ensure solved quaternions stay in the same hemisphere as pre-step.
 
-    Two levels of correction, applied to ALL non-grounded bodies
-    (including dragged parts, whose params Newton re-solves):
+    For each non-grounded, non-dragged body, check whether the solved
+    quaternion is in the opposite hemisphere from the pre-step quaternion
+    (dot product < 0).  If so, negate it — q and -q represent the same
+    rotation, but staying in the same hemisphere prevents the C++ side
+    from seeing a large-angle "flip".
 
-    1. **Hemisphere check** (cheap): if dot(q_prev, q_solved) < 0, negate
-       q_solved.  This catches the common q-vs-(-q) sign flip.
-
-    2. **Rotation angle check**: compute the rotation angle from q_prev
-       to q_solved using the same formula as the C++ validator
-       (2*acos(w) of the relative quaternion).  If the angle exceeds
-       the C++ threshold (91°), reset the body's quaternion to q_prev.
-       This catches deeper branch jumps where the solver converged to a
-       geometrically different but constraint-satisfying orientation.
-       The next Newton iteration from the caller will re-converge from
-       the safer starting point.
-
-    This applies to dragged parts too: the GUI sets the dragged part's
-    params to the mouse-projected placement, then Newton re-solves all
-    free params (including the dragged part's) to satisfy constraints.
-    The solver can converge to an equivalent quaternion on the opposite
-    branch, which the C++ validateNewPlacements() rejects as a >91°
-    flip.
+    This is the standard short-arc correction used in SLERP interpolation.
     """
-    _MAX_ANGLE = 91.0 * math.pi / 180.0  # match C++ threshold
-
     for body in bodies.values():
-        if body.grounded:
+        if body.grounded or body.part_id in dragged_ids:
             continue
         prev = pre_step_quats.get(body.part_id)
         if prev is None:
@@ -549,51 +524,15 @@ def _enforce_quat_continuity(
         qy = params.get_value(pfx + "qy")
         qz = params.get_value(pfx + "qz")
 
-        # Level 1: hemisphere check (standard SLERP short-arc correction)
+        # Quaternion dot product: positive means same hemisphere
         dot = prev[0] * qw + prev[1] * qx + prev[2] * qy + prev[3] * qz
+
         if dot < 0.0:
-            qw, qx, qy, qz = -qw, -qx, -qy, -qz
-            params.set_value(pfx + "qw", qw)
-            params.set_value(pfx + "qx", qx)
-            params.set_value(pfx + "qy", qy)
-            params.set_value(pfx + "qz", qz)
-
-        # Level 2: rotation angle check (catches branch jumps beyond sign flip)
-        # Compute relative quaternion: q_rel = q_new * conj(q_prev)
-        pw, px, py, pz = prev
-        rel_w = qw * pw + qx * px + qy * py + qz * pz
-        rel_x = qx * pw - qw * px - qy * pz + qz * py
-        rel_y = qy * pw - qw * py - qz * px + qx * pz
-        rel_z = qz * pw - qw * pz - qx * py + qy * px
-        # Normalize
-        rel_norm = math.sqrt(
-            rel_w * rel_w + rel_x * rel_x + rel_y * rel_y + rel_z * rel_z
-        )
-        if rel_norm > 1e-15:
-            rel_w /= rel_norm
-        rel_w = max(-1.0, min(1.0, rel_w))
-
-        # C++ evaluateVector: angle = 2 * acos(w)
-        if -1.0 < rel_w < 1.0:
-            angle = 2.0 * math.acos(rel_w)
-        else:
-            angle = 0.0
-
-        if abs(angle) > _MAX_ANGLE:
-            # The solver jumped to a different constraint branch.
-            # Reset to the previous step's quaternion — the caller's
-            # Newton solve was already complete, so this just ensures
-            # the output stays near the previous configuration.
-            log.debug(
-                "_enforce_quat_continuity: %s jumped %.1f deg, "
-                "resetting to previous quaternion",
-                body.part_id,
-                math.degrees(angle),
-            )
-            params.set_value(pfx + "qw", pw)
-            params.set_value(pfx + "qx", px)
-            params.set_value(pfx + "qy", py)
-            params.set_value(pfx + "qz", pz)
+            # Negate to stay in the same hemisphere (identical rotation)
+            params.set_value(pfx + "qw", -qw)
+            params.set_value(pfx + "qx", -qx)
+            params.set_value(pfx + "qy", -qy)
+            params.set_value(pfx + "qz", -qz)
 
 
 def _build_system(ctx):
@@ -666,6 +605,19 @@ def _build_system(ctx):
                 c.type,
             )
             continue
+        # For PlanarConstraint, snapshot the world-frame normal at the
+        # initial configuration so the distance residual uses a constant
+        # reference direction.  This prevents axial drift when a Planar
+        # is combined with a Cylindrical on the same body pair.
+        if isinstance(obj, PlanarConstraint):
+            env = params.get_env()
+            z_j = marker_z_axis(obj.body_j, obj.marker_j_quat)
+            obj.reference_normal = (
+                z_j[0].eval(env),
+                z_j[1].eval(env),
+                z_j[2].eval(env),
+            )
+
         constraint_objs.append(obj)
         constraint_indices.append(idx)
         all_residuals.extend(obj.residuals())
@@ -709,9 +661,7 @@ def _run_diagnostics(residuals, params, residual_ranges, ctx, jac_exprs=None):
     if not hasattr(kcsolve, "ConstraintDiagnostic"):
         return diagnostics
 
-    diags = find_overconstrained(
-        residuals, params, residual_ranges, jac_exprs=jac_exprs
-    )
+    diags = find_overconstrained(residuals, params, residual_ranges, jac_exprs=jac_exprs)
     for d in diags:
         cd = kcsolve.ConstraintDiagnostic()
         cd.constraint_id = ctx.constraints[d.constraint_index].id
@@ -741,9 +691,7 @@ def _extract_placements(params, bodies):
     return placements
 
 
-def _monolithic_solve(
-    all_residuals, params, quat_groups, post_step=None, weight_vector=None
-):
+def _monolithic_solve(all_residuals, params, quat_groups, post_step=None, weight_vector=None):
     """Newton-Raphson solve with BFGS fallback on the full system.
 
     Returns ``(converged, jac_exprs)`` so the caller can reuse the
@@ -775,9 +723,7 @@ def _monolithic_solve(
     )
     nr_ms = (time.perf_counter() - t0) * 1000
     if not converged:
-        log.info(
-            "_monolithic_solve: Newton-Raphson failed (%.1f ms), trying BFGS", nr_ms
-        )
+        log.info("_monolithic_solve: Newton-Raphson failed (%.1f ms), trying BFGS", nr_ms)
         t1 = time.perf_counter()
         converged = bfgs_solve(
             all_residuals,

tests/console_test_planar_drag.py

@@ -1,720 +0,0 @@
-"""
-Console test: Cylindrical + Planar drag — reproduces #338.
-
-Paste into the FreeCAD Python console (or run via exec(open(...).read())).
-
-This test builds scenarios that trigger the quaternion hemisphere flip
-during drag that causes the C++ validateNewPlacements() to reject every
-step with "flipped orientation (360.0 degrees)".
-
-Key insight: the C++ Rotation::evaluateVector() computes
-    _angle = 2 * acos(w)
-using the RAW w component (not |w|).  When the solver returns a
-quaternion in the opposite hemisphere (w < 0), the relative rotation
-  relativeRot = newRot * oldRot.inverse()
-has w ≈ -1, giving angle ≈ 2*acos(-1) = 2*pi = 360 degrees.
-The 91-degree threshold then rejects it.
-
-The solver's _enforce_quat_continuity SHOULD fix this, but it skips
-dragged parts.  For the non-dragged bar, the fix only works if the
-pre_step_quats baseline is correct.  This test reproduces the failure
-by using realistic non-identity geometry.
-"""
-
-import math
-
-import kcsolve
-
-_results = []
-
-
-def _report(name, passed, detail=""):
-    status = "PASS" if passed else "FAIL"
-    msg = f"  [{status}] {name}"
-    if detail:
-        msg += f" -- {detail}"
-    print(msg)
-    _results.append((name, passed))
-
-
-# ── Quaternion math ──────────────────────────────────────────────────
-
-
-def _qmul(a, b):
-    """Hamilton product (w, x, y, z)."""
-    aw, ax, ay, az = a
-    bw, bx, by, bz = b
-    return (
-        aw * bw - ax * bx - ay * by - az * bz,
-        aw * bx + ax * bw + ay * bz - az * by,
-        aw * by - ax * bz + ay * bw + az * bx,
-        aw * bz + ax * by - ay * bx + az * bw,
-    )
-
-
-def _qconj(q):
-    """Conjugate (= inverse for unit quaternion)."""
-    return (q[0], -q[1], -q[2], -q[3])
-
-
-def _qnorm(q):
-    """Normalize quaternion."""
-    n = math.sqrt(sum(c * c for c in q))
-    return tuple(c / n for c in q) if n > 1e-15 else q
-
-
-def _axis_angle_quat(axis, angle_rad):
-    """Quaternion (w, x, y, z) for rotation about a normalized axis."""
-    ax, ay, az = axis
-    n = math.sqrt(ax * ax + ay * ay + az * az)
-    if n < 1e-15:
-        return (1.0, 0.0, 0.0, 0.0)
-    ax, ay, az = ax / n, ay / n, az / n
-    s = math.sin(angle_rad / 2.0)
-    return (math.cos(angle_rad / 2.0), ax * s, ay * s, az * s)
-
-
-def _rotation_angle_cpp(q_old, q_new):
-    """Rotation angle (degrees) matching C++ validateNewPlacements().
-
-    C++ pipeline:
-        Rotation(x, y, z, w) — stores quat as (x, y, z, w)
-        evaluateVector():  _angle = 2 * acos(quat[3])   // quat[3] = w
-        getRawValue(axis, angle) returns _angle
-
-    CRITICAL: C++ uses acos(w), NOT acos(|w|).
-    When w < 0 (opposite hemisphere), acos(w) > pi/2, so angle > pi.
-    For w ≈ -1 (identity rotation in wrong hemisphere): angle ≈ 2*pi = 360 deg.
-
-    Note: the relative rotation quaternion is constructed via
-        relativeRot = newRot * oldRot.inverse()
-    which goes through Rotation::operator*() and normalize()+evaluateVector().
-    """
-    q_rel = _qmul(q_new, _qconj(q_old))
-    q_rel = _qnorm(q_rel)
-
-    # q_rel is in (w, x, y, z) order. FreeCAD stores (x, y, z, w), so
-    # when it constructs a Rotation from (q0=x, q1=y, q2=z, q3=w),
-    # evaluateVector() reads quat[3] = w.
-    w = q_rel[0]
-    w = max(-1.0, min(1.0, w))
-
-    # C++ evaluateVector: checks (quat[3] > -1.0) && (quat[3] < 1.0)
-    # Exact ±1 hits else-branch → angle = 0.  In practice the multiply
-    # + normalize produces values like -0.99999... which still enters
-    # the acos branch.  We replicate C++ exactly here.
-    if w > -1.0 and w < 1.0:
-        angle_rad = math.acos(w) * 2.0
-    else:
-        angle_rad = 0.0
-    return math.degrees(angle_rad)
-
-
-def _rotation_angle_abs(q_old, q_new):
-    """Angle using |w| — what a CORRECT validator would use."""
-    q_rel = _qmul(q_new, _qconj(q_old))
-    q_rel = _qnorm(q_rel)
-    w = abs(q_rel[0])
-    w = min(1.0, w)
-    return math.degrees(2.0 * math.acos(w))
-
-
-# ── Context builders ─────────────────────────────────────────────────
-
-
-def _build_ctx(
-    ground_pos,
-    ground_quat,
-    bar_pos,
-    bar_quat,
-    cyl_marker_i_quat,
-    cyl_marker_j_quat,
-    cyl_marker_i_pos=(0, 0, 0),
-    cyl_marker_j_pos=(0, 0, 0),
-    planar_marker_i_quat=None,
-    planar_marker_j_quat=None,
-    planar_marker_i_pos=(0, 0, 0),
-    planar_marker_j_pos=(0, 0, 0),
-    planar_offset=0.0,
-):
-    """Build SolveContext with ground + bar, Cylindrical + Planar joints.
-
-    Uses explicit marker quaternions instead of identity, so the
-    constraint geometry matches realistic assemblies.
-    """
-    if planar_marker_i_quat is None:
-        planar_marker_i_quat = cyl_marker_i_quat
-    if planar_marker_j_quat is None:
-        planar_marker_j_quat = cyl_marker_j_quat
-
-    ground = kcsolve.Part()
-    ground.id = "ground"
-    ground.placement = kcsolve.Transform()
-    ground.placement.position = list(ground_pos)
-    ground.placement.quaternion = list(ground_quat)
-    ground.grounded = True
-
-    bar = kcsolve.Part()
-    bar.id = "bar"
-    bar.placement = kcsolve.Transform()
-    bar.placement.position = list(bar_pos)
-    bar.placement.quaternion = list(bar_quat)
-    bar.grounded = False
-
-    cyl = kcsolve.Constraint()
-    cyl.id = "cylindrical"
-    cyl.part_i = "ground"
-    cyl.marker_i = kcsolve.Transform()
-    cyl.marker_i.position = list(cyl_marker_i_pos)
-    cyl.marker_i.quaternion = list(cyl_marker_i_quat)
-    cyl.part_j = "bar"
-    cyl.marker_j = kcsolve.Transform()
-    cyl.marker_j.position = list(cyl_marker_j_pos)
-    cyl.marker_j.quaternion = list(cyl_marker_j_quat)
-    cyl.type = kcsolve.BaseJointKind.Cylindrical
-
-    planar = kcsolve.Constraint()
-    planar.id = "planar"
-    planar.part_i = "ground"
-    planar.marker_i = kcsolve.Transform()
-    planar.marker_i.position = list(planar_marker_i_pos)
-    planar.marker_i.quaternion = list(planar_marker_i_quat)
-    planar.part_j = "bar"
-    planar.marker_j = kcsolve.Transform()
-    planar.marker_j.position = list(planar_marker_j_pos)
-    planar.marker_j.quaternion = list(planar_marker_j_quat)
-    planar.type = kcsolve.BaseJointKind.Planar
-    planar.params = [planar_offset]
-
-    ctx = kcsolve.SolveContext()
-    ctx.parts = [ground, bar]
-    ctx.constraints = [cyl, planar]
-    return ctx
-
-
-# ── Test 1: Validator function correctness ───────────────────────────
-
-
-def test_validator_function():
-    """Verify our _rotation_angle_cpp matches C++ behavior for hemisphere flips."""
-    print("\n--- Test 1: Validator function correctness ---")
-
-    # Same quaternion → 0 degrees
-    q = (1.0, 0.0, 0.0, 0.0)
-    angle = _rotation_angle_cpp(q, q)
-    _report("same quat → 0 deg", abs(angle) < 0.1, f"{angle:.1f}")
-
-    # Near-negated quaternion (tiny perturbation from exact -1 to avoid
-    # the C++ boundary condition where |w| == 1 → angle = 0).
-    # In practice the solver never returns EXACTLY -1; it returns
-    # -0.999999... which enters the acos() branch and gives ~360 deg.
-    q_near_neg = _qnorm((-0.99999, 0.00001, 0.0, 0.0))
-    angle = _rotation_angle_cpp(q, q_near_neg)
-    _report("near-negated quat → ~360 deg", angle > 350.0, f"{angle:.1f}")
-
-    # Same test with |w| correction → should be ~0
-    angle_abs = _rotation_angle_abs(q, q_near_neg)
-    _report("|w|-corrected → ~0 deg", angle_abs < 1.0, f"{angle_abs:.1f}")
-
-    # Non-trivial quaternion vs near-negated version (realistic float noise)
-    q2 = _qnorm((-0.5181, -0.5181, 0.4812, -0.4812))
-    # Simulate what happens: solver returns same rotation in opposite hemisphere
-    q2_neg = _qnorm(tuple(-c + 1e-8 for c in q2))
-    angle = _rotation_angle_cpp(q2, q2_neg)
-    _report("real quat near-negated → >180 deg", angle > 180.0, f"{angle:.1f}")
-
-    # 10-degree rotation — should be fine
-    q_small = _axis_angle_quat((0, 0, 1), math.radians(10))
-    angle = _rotation_angle_cpp(q, q_small)
-    _report("10 deg rotation → ~10 deg", abs(angle - 10.0) < 1.0, f"{angle:.1f}")
-
-
-# ── Test 2: Synthetic drag with realistic geometry ───────────────────
-
-
-def test_drag_realistic():
-    """Drag with non-identity markers and non-trivial bar orientation.
-
-    This reproduces the real assembly geometry:
-    - Cylindrical axis is along a diagonal (not global Z)
-    - Bar starts at a complex orientation far from identity
-    - Drag includes axial perturbation (rotation about constraint axis)
-
-    The solver must re-converge the bar's orientation on each step.
-    If it lands on the -q hemisphere, the C++ validator rejects.
-    """
-    print("\n--- Test 2: Realistic drag with non-identity geometry ---")
-    solver = kcsolve.load("kindred")
-
-    # Marker quaternion: rotates local Z to point along (1,1,0)/sqrt(2)
-    # This means the cylindrical axis is diagonal in the XY plane
-    marker_q = _qnorm(_axis_angle_quat((0, 1, 0), math.radians(45)))
-
-    # Bar starts at a complex orientation (from real assembly data)
-    # This is close to the actual q=(-0.5181, -0.5181, 0.4812, -0.4812)
-    bar_quat_init = _qnorm((-0.5181, -0.5181, 0.4812, -0.4812))
-
-    # Ground at a non-trivial orientation too (real assembly had q=(0.707,0,0,0.707))
-    ground_quat = _qnorm((0.7071, 0.0, 0.0, 0.7071))
-
-    # Positions far from origin (like real assembly)
-    ground_pos = (100.0, 0.0, 0.0)
-    bar_pos = (500.0, -500.0, 0.0)
-
-    ctx = _build_ctx(
-        ground_pos=ground_pos,
-        ground_quat=ground_quat,
-        bar_pos=bar_pos,
-        bar_quat=bar_quat_init,
-        cyl_marker_i_quat=marker_q,
-        cyl_marker_j_quat=marker_q,
-        # Planar uses identity markers (XY plane constraint)
-        planar_marker_i_quat=(1, 0, 0, 0),
-        planar_marker_j_quat=(1, 0, 0, 0),
-    )
-
-    # ── Save baseline (simulates savePlacementsForUndo) ──
-    baseline_quat = bar_quat_init
-
-    # ── pre_drag ──
-    drag_result = solver.pre_drag(ctx, ["bar"])
-    _report(
-        "drag: pre_drag converged",
-        drag_result.status == kcsolve.SolveStatus.Success,
-        f"status={drag_result.status}",
-    )
-
-    # Check pre_drag result against baseline
-    for pr in drag_result.placements:
-        if pr.id == "bar":
-            solved_quat = tuple(pr.placement.quaternion)
-            angle_cpp = _rotation_angle_cpp(baseline_quat, solved_quat)
-            angle_abs = _rotation_angle_abs(baseline_quat, solved_quat)
-            ok = angle_cpp <= 91.0
-            _report(
-                "drag: pre_drag passes validator",
-                ok,
-                f"C++ angle={angle_cpp:.1f}, |w| angle={angle_abs:.1f}, "
-                f"q=({solved_quat[0]:+.4f},{solved_quat[1]:+.4f},"
-                f"{solved_quat[2]:+.4f},{solved_quat[3]:+.4f})",
-            )
-            if ok:
-                baseline_quat = solved_quat
-
-    # ── drag steps with axial perturbation ──
-    n_steps = 40
-    accepted = 0
-    rejected = 0
-    first_reject_step = None
-
-    for step in range(1, n_steps + 1):
-        # Drag the bar along the cylindrical axis with ROTATION perturbation
-        # Each step: translate along the axis + rotate about it
-        t = step / n_steps
-        angle_about_axis = math.radians(step * 15.0)  # 15 deg/step, goes past 360
-
-        # The cylindrical axis direction (marker Z in ground frame)
-        # For our 45-deg-rotated marker: axis ≈ (sin45, 0, cos45) in ground-local
-        # But ground is also rotated. Let's just move along a diagonal.
-        slide = step * 5.0
-        drag_pos = [
-            bar_pos[0] + slide * 0.707,
-            bar_pos[1] + slide * 0.707,
-            bar_pos[2],
-        ]
-
-        # Build the dragged orientation: start from bar_quat_init,
-        # apply rotation about the constraint axis
-        axis_rot = _axis_angle_quat((0.707, 0.707, 0), angle_about_axis)
-        drag_quat = list(_qnorm(_qmul(axis_rot, bar_quat_init)))
-
-        pr = kcsolve.SolveResult.PartResult()
-        pr.id = "bar"
-        pr.placement = kcsolve.Transform()
-        pr.placement.position = drag_pos
-        pr.placement.quaternion = drag_quat
-
-        result = solver.drag_step([pr])
-        converged = result.status == kcsolve.SolveStatus.Success
-
-        bar_quat_out = None
-        for rpr in result.placements:
-            if rpr.id == "bar":
-                bar_quat_out = tuple(rpr.placement.quaternion)
-                break
-
-        if bar_quat_out is None:
-            _report(f"step {step:2d}", False, "bar not in result")
-            rejected += 1
-            continue
-
-        # ── Simulate validateNewPlacements() ──
-        angle_cpp = _rotation_angle_cpp(baseline_quat, bar_quat_out)
-        angle_abs = _rotation_angle_abs(baseline_quat, bar_quat_out)
-        validator_ok = angle_cpp <= 91.0
-
-        if validator_ok:
-            baseline_quat = bar_quat_out
-            accepted += 1
-        else:
-            rejected += 1
-            if first_reject_step is None:
-                first_reject_step = step
-
-        _report(
-            f"step {step:2d} ({step * 15:3d} deg)",
-            validator_ok and converged,
-            f"C++={angle_cpp:.1f} |w|={angle_abs:.1f} "
-            f"{'ACCEPT' if validator_ok else 'REJECT'} "
-            f"q=({bar_quat_out[0]:+.4f},{bar_quat_out[1]:+.4f},"
-            f"{bar_quat_out[2]:+.4f},{bar_quat_out[3]:+.4f})",
-        )
-
-    solver.post_drag()
-
-    print(f"\n  Summary: accepted={accepted}/{n_steps}, rejected={rejected}/{n_steps}")
-    _report(
-        "drag: all steps accepted by C++ validator",
-        rejected == 0,
-        f"{rejected} rejected"
-        + (f", first at step {first_reject_step}" if first_reject_step else ""),
-    )
-
-
-# ── Test 3: Drag with NEGATED initial bar quaternion ─────────────────
-
-
-def test_drag_negated_init():
-    """Start the bar at -q (same rotation, opposite hemisphere from solver
-    convention) to maximize the chance of hemisphere mismatch.
-
-    The C++ side saves the FreeCAD object's current Placement.Rotation
-    as the baseline.  If FreeCAD stores q but the solver internally
-    prefers -q, the very first solve output can differ in hemisphere.
-    """
-    print("\n--- Test 3: Drag with negated initial quaternion ---")
-    solver = kcsolve.load("kindred")
-
-    # A non-trivial orientation with w < 0
-    # This is a valid unit quaternion representing a real rotation
-    bar_quat_neg = _qnorm((-0.5, -0.5, 0.5, -0.5))  # w < 0
-
-    # The same rotation in the positive hemisphere
-    bar_quat_pos = tuple(-c for c in bar_quat_neg)  # w > 0
-
-    # Identity markers (simplify to isolate the hemisphere issue)
-    ident = (1.0, 0.0, 0.0, 0.0)
-
-    ctx = _build_ctx(
-        ground_pos=(0, 0, 0),
-        ground_quat=ident,
-        bar_pos=(10, 0, 0),
-        bar_quat=bar_quat_neg,  # Start in NEGATIVE hemisphere
-        cyl_marker_i_quat=ident,
-        cyl_marker_j_quat=ident,
-    )
-
-    # C++ baseline is saved BEFORE pre_drag — so it uses the w<0 form
-    baseline_quat = bar_quat_neg
-
-    # pre_drag: solver may normalize to positive hemisphere internally
-    drag_result = solver.pre_drag(ctx, ["bar"])
-    _report(
-        "negated: pre_drag converged",
-        drag_result.status == kcsolve.SolveStatus.Success,
-    )
-
-    for pr in drag_result.placements:
-        if pr.id == "bar":
-            solved = tuple(pr.placement.quaternion)
-            # Did the solver flip to positive hemisphere?
-            dot = sum(a * b for a, b in zip(baseline_quat, solved))
-
-            angle_cpp = _rotation_angle_cpp(baseline_quat, solved)
-            hemisphere_match = dot >= 0
-
-            _report(
-                "negated: pre_drag hemisphere match",
-                hemisphere_match,
-                f"dot={dot:+.4f}, C++ angle={angle_cpp:.1f} deg, "
-                f"baseline w={baseline_quat[0]:+.4f}, "
-                f"solved w={solved[0]:+.4f}",
-            )
-
-            validator_ok = angle_cpp <= 91.0
-            _report(
-                "negated: pre_drag passes C++ validator",
-                validator_ok,
-                f"angle={angle_cpp:.1f} deg (threshold=91)",
-            )
-
-            if validator_ok:
-                baseline_quat = solved
-
-    # Drag steps with small perturbation
-    n_steps = 20
-    accepted = 0
-    rejected = 0
-    first_reject = None
-
-    for step in range(1, n_steps + 1):
-        angle_rad = math.radians(step * 18.0)
-        R = 10.0
-        drag_pos = [R * math.cos(angle_rad), R * math.sin(angle_rad), 0.0]
-
-        # Apply the drag rotation in the NEGATIVE hemisphere to match
-        # how FreeCAD would track the mouse-projected placement
-        z_rot = _axis_angle_quat((0, 0, 1), angle_rad)
-        drag_quat = list(_qnorm(_qmul(z_rot, bar_quat_neg)))
-
-        pr = kcsolve.SolveResult.PartResult()
-        pr.id = "bar"
-        pr.placement = kcsolve.Transform()
-        pr.placement.position = drag_pos
-        pr.placement.quaternion = drag_quat
-
-        result = solver.drag_step([pr])
-
-        for rpr in result.placements:
-            if rpr.id == "bar":
-                out_q = tuple(rpr.placement.quaternion)
-                angle_cpp = _rotation_angle_cpp(baseline_quat, out_q)
-                ok = angle_cpp <= 91.0
-                if ok:
-                    baseline_quat = out_q
-                    accepted += 1
-                else:
-                    rejected += 1
-                    if first_reject is None:
-                        first_reject = step
-                _report(
-                    f"neg step {step:2d} ({step * 18:3d} deg)",
-                    ok,
-                    f"C++={angle_cpp:.1f} "
-                    f"q=({out_q[0]:+.4f},{out_q[1]:+.4f},"
-                    f"{out_q[2]:+.4f},{out_q[3]:+.4f})",
-                )
-                break
-
-    solver.post_drag()
-    print(f"\n  Summary: accepted={accepted}/{n_steps}, rejected={rejected}/{n_steps}")
-    _report(
-        "negated: all steps accepted",
-        rejected == 0,
-        f"{rejected} rejected"
-        + (f", first at step {first_reject}" if first_reject else ""),
-    )
-
-
-# ── Test 4: Live assembly if available ───────────────────────────────
-
-
-def test_live_assembly():
-    """If a FreeCAD assembly is open, extract its actual geometry and run
-    the drag simulation with real markers and placements."""
-    print("\n--- Test 4: Live assembly introspection ---")
-    try:
-        import FreeCAD as App
-    except ImportError:
-        _report("live: FreeCAD available", False, "not running inside FreeCAD")
-        return
-
-    doc = App.ActiveDocument
-    if doc is None:
-        _report("live: document open", False, "no active document")
-        return
-
-    asm = None
-    for obj in doc.Objects:
-        if obj.TypeId == "Assembly::AssemblyObject":
-            asm = obj
-            break
-
-    if asm is None:
-        _report("live: assembly found", False, "no Assembly object in document")
-        return
-
-    _report("live: assembly found", True, f"'{asm.Name}'")
-
-    # Introspect parts
-    parts = []
-    joints = []
-    grounded = []
-    for obj in asm.Group:
-        if hasattr(obj, "TypeId"):
-            if obj.TypeId == "Assembly::JointGroup":
-                for jobj in obj.Group:
-                    if hasattr(jobj, "Proxy"):
-                        joints.append(jobj)
-            elif hasattr(obj, "Placement"):
-                parts.append(obj)
-
-    for jobj in joints:
-        proxy = getattr(jobj, "Proxy", None)
-        if proxy and type(proxy).__name__ == "GroundedJoint":
-            ref = getattr(jobj, "ObjectToGround", None)
-            if ref:
-                grounded.append(ref.Name)
-
-    print(f"  Parts: {len(parts)}, Joints: {len(joints)}, Grounded: {grounded}")
-
-    # Print each part's placement
-    for p in parts:
-        plc = p.Placement
-        rot = plc.Rotation
-        q = rot.Q  # FreeCAD (x, y, z, w)
-        q_wxyz = (q[3], q[0], q[1], q[2])
-        pos = plc.Base
-        is_gnd = p.Name in grounded
-        print(
-            f"  {p.Label:40s} pos=({pos.x:.1f}, {pos.y:.1f}, {pos.z:.1f}) "
-            f"q(wxyz)=({q_wxyz[0]:.4f}, {q_wxyz[1]:.4f}, "
-            f"{q_wxyz[2]:.4f}, {q_wxyz[3]:.4f}) "
-            f"{'[GROUNDED]' if is_gnd else ''}"
-        )
-
-    # Print joint details
-    for jobj in joints:
-        proxy = getattr(jobj, "Proxy", None)
-        ptype = type(proxy).__name__ if proxy else "unknown"
-        kind = getattr(jobj, "JointType", "?")
-        print(f"  Joint: {jobj.Label} type={ptype} kind={kind}")
-
-    # Check: does any non-grounded part have w < 0 in its current quaternion?
-    # That alone would cause the validator to reject on the first solve.
-    for p in parts:
-        if p.Name in grounded:
-            continue
-        q = p.Placement.Rotation.Q  # (x, y, z, w)
-        w = q[3]
-        if w < 0:
-            print(
-                f"\n  ** {p.Label} has w={w:.4f} < 0 in current placement! **"
-                f"\n     If the solver returns w>0, the C++ validator sees ~360 deg flip."
-            )
-
-    _report("live: assembly introspected", True)
-
-
-# ── Test 5: Direct hemisphere flip reproduction ──────────────────────
-
-
-def test_hemisphere_flip_direct():
-    """Directly reproduce the hemisphere flip by feeding the solver
-    a dragged placement where the quaternion is in the opposite
-    hemisphere from what pre_drag returned.
-
-    This simulates what happens when:
-    1. FreeCAD stores Placement with q = (w<0, x, y, z) form
-    2. Solver normalizes to w>0 during pre_drag
-    3. Next drag_step gets mouse placement in the w<0 form
-    4. Solver output may flip back to w<0
-    """
-    print("\n--- Test 5: Direct hemisphere flip ---")
-    solver = kcsolve.load("kindred")
-
-    # Use a quaternion representing 90-deg rotation about Z
-    # In positive hemisphere: (cos45, 0, 0, sin45) = (0.707, 0, 0, 0.707)
-    # In negative hemisphere: (-0.707, 0, 0, -0.707)
-    q_pos = _axis_angle_quat((0, 0, 1), math.radians(90))
-    q_neg = tuple(-c for c in q_pos)
-
-    ident = (1.0, 0.0, 0.0, 0.0)
-
-    # Build context with positive-hemisphere quaternion
-    ctx = _build_ctx(
-        ground_pos=(0, 0, 0),
-        ground_quat=ident,
-        bar_pos=(10, 0, 0),
-        bar_quat=q_pos,
-        cyl_marker_i_quat=ident,
-        cyl_marker_j_quat=ident,
-    )
-
-    # C++ baseline saves q_pos
-    baseline_quat = q_pos
-
-    result = solver.pre_drag(ctx, ["bar"])
-    _report("flip: pre_drag converged", result.status == kcsolve.SolveStatus.Success)
-
-    for pr in result.placements:
-        if pr.id == "bar":
-            baseline_quat = tuple(pr.placement.quaternion)
-            print(
-                f"  pre_drag baseline: ({baseline_quat[0]:+.4f},"
-                f"{baseline_quat[1]:+.4f},{baseline_quat[2]:+.4f},"
-                f"{baseline_quat[3]:+.4f})"
-            )
-
-    # Now feed drag steps where we alternate hemispheres in the dragged
-    # placement to see if the solver output flips
-    test_drags = [
-        ("same hemisphere", q_pos),
-        ("opposite hemisphere", q_neg),
-        ("back to same", q_pos),
-        ("opposite again", q_neg),
-        ("large rotation pos", _axis_angle_quat((0, 0, 1), math.radians(170))),
-        (
-            "large rotation neg",
-            tuple(-c for c in _axis_angle_quat((0, 0, 1), math.radians(170))),
-        ),
-    ]
-
-    for name, drag_q in test_drags:
-        pr = kcsolve.SolveResult.PartResult()
-        pr.id = "bar"
-        pr.placement = kcsolve.Transform()
-        pr.placement.position = [10.0, 0.0, 0.0]
-        pr.placement.quaternion = list(drag_q)
-
-        result = solver.drag_step([pr])
-
-        for rpr in result.placements:
-            if rpr.id == "bar":
-                out_q = tuple(rpr.placement.quaternion)
-                angle_cpp = _rotation_angle_cpp(baseline_quat, out_q)
-                angle_abs = _rotation_angle_abs(baseline_quat, out_q)
-                ok = angle_cpp <= 91.0
-
-                _report(
-                    f"flip: {name}",
-                    ok,
-                    f"C++={angle_cpp:.1f} |w|={angle_abs:.1f} "
-                    f"in_w={drag_q[0]:+.4f} out_w={out_q[0]:+.4f}",
-                )
-                if ok:
-                    baseline_quat = out_q
-                break
-
-    solver.post_drag()
-
-
-# ── Run all ──────────────────────────────────────────────────────────
-
-
-def run_all():
-    print("\n" + "=" * 70)
-    print("  Console Test: Planar + Cylindrical Drag (#338 / #339)")
-    print("  Realistic geometry + C++ validator simulation")
-    print("=" * 70)
-
-    test_validator_function()
-    test_drag_realistic()
-    test_drag_negated_init()
-    test_live_assembly()
-    test_hemisphere_flip_direct()
-
-    # Summary
-    passed = sum(1 for _, p in _results if p)
-    total = len(_results)
-    print(f"\n{'=' * 70}")
-    print(f"  {passed}/{total} passed")
-    if passed < total:
-        failed = [n for n, p in _results if not p]
-        print(f"  FAILED ({len(failed)}):")
-        for f in failed:
-            print(f"    - {f}")
-    print("=" * 70 + "\n")
-
-
-run_all()

tests/test_drag.py

@@ -19,10 +19,12 @@ import math
 import pytest
 from kindred_solver.constraints import (
     CoincidentConstraint,
+    CylindricalConstraint,
     PlanarConstraint,
     RevoluteConstraint,
 )
 from kindred_solver.entities import RigidBody
+from kindred_solver.geometry import marker_z_axis
 from kindred_solver.newton import newton_solve
 from kindred_solver.params import ParamTable
 from kindred_solver.prepass import single_equation_pass, substitution_pass
@@ -251,3 +253,118 @@ class TestDragDoesNotBreakStaticSolve:
         assert abs(env["arm/ty"]) < 1e-8
         assert abs(env["arm/tz"]) < 1e-8
         assert abs(env["plate/tz"]) < 1e-8
+
+
+def _set_reference_normal(planar_obj, params):
+    """Snapshot world-frame normal as reference (mirrors _build_system logic)."""
+    env = params.get_env()
+    z_j = marker_z_axis(planar_obj.body_j, planar_obj.marker_j_quat)
+    planar_obj.reference_normal = (
+        z_j[0].eval(env),
+        z_j[1].eval(env),
+        z_j[2].eval(env),
+    )
+
+
+class TestPlanarCylindricalAxialDrift:
+    """Planar + Cylindrical on the same body pair: axial drift regression.
+
+    Bug: PlanarConstraint's distance residual uses a body-attached normal
+    (z_j) that rotates with the body.  When combined with Cylindrical
+    (which allows rotation about the axis), the normal can tilt during
+    Newton iteration, allowing the body to drift along the cylinder axis
+    while technically satisfying the rotated distance residual.
+
+    Fix: snapshot the world-frame normal at system-build time and use
+    Const nodes in the distance residual (reference_normal).
+    """
+
+    def _setup(self, offset=0.0):
+        """Cylindrical + Planar along Z between ground and a free body.
+
+        Free body starts displaced along Z so the solver must move it.
+        """
+        pt = ParamTable()
+        ground = RigidBody("g", pt, (0, 0, 0), ID_QUAT, grounded=True)
+        free = RigidBody("free", pt, (0, 0, 5), ID_QUAT)
+
+        cyl = CylindricalConstraint(
+            ground,
+            (0, 0, 0),
+            ID_QUAT,
+            free,
+            (0, 0, 0),
+            ID_QUAT,
+        )
+        planar = PlanarConstraint(
+            ground,
+            (0, 0, 0),
+            ID_QUAT,
+            free,
+            (0, 0, 0),
+            ID_QUAT,
+            offset=offset,
+        )
+        bodies = [ground, free]
+        constraints = [cyl, planar]
+        return pt, bodies, constraints, planar
+
+    def test_reference_normal_prevents_axial_drift(self):
+        """With reference_normal, free body converges to z=0 (distance=0)."""
+        pt, bodies, constraints, planar = self._setup(offset=0.0)
+        _set_reference_normal(planar, pt)
+        raw, qg = _build_residuals(bodies, constraints)
+
+        residuals = substitution_pass(raw, pt)
+        ok = newton_solve(residuals, pt, quat_groups=qg, max_iter=100, tol=1e-10)
+        assert ok
+
+        env = pt.get_env()
+        assert abs(env["free/tz"]) < 1e-8, (
+            f"free/tz = {env['free/tz']:.6e}, expected ~0.0 (axial drift)"
+        )
+
+    def test_reference_normal_with_offset(self):
+        """With reference_normal and offset=3.0, free body converges to z=-3.
+
+        Sign convention: point_plane_distance = (p_ground - p_free) · n,
+        so offset=3 means -z_free - 3 = 0, i.e. z_free = -3.
+        """
+        pt, bodies, constraints, planar = self._setup(offset=3.0)
+        _set_reference_normal(planar, pt)
+        raw, qg = _build_residuals(bodies, constraints)
+
+        residuals = substitution_pass(raw, pt)
+        ok = newton_solve(residuals, pt, quat_groups=qg, max_iter=100, tol=1e-10)
+        assert ok
+
+        env = pt.get_env()
+        assert abs(env["free/tz"] + 3.0) < 1e-8, f"free/tz = {env['free/tz']:.6e}, expected ~-3.0"
+
+    def test_drag_step_no_drift(self):
+        """After drag perturbation, re-solve keeps axial position locked."""
+        pt, bodies, constraints, planar = self._setup(offset=0.0)
+        _set_reference_normal(planar, pt)
+        raw, qg = _build_residuals(bodies, constraints)
+
+        residuals = substitution_pass(raw, pt)
+        ok = newton_solve(residuals, pt, quat_groups=qg, max_iter=100, tol=1e-10)
+        assert ok
+
+        env = pt.get_env()
+        assert abs(env["free/tz"]) < 1e-8
+
+        # Simulate drag: perturb the free body's position
+        pt.set_value("free/tx", 3.0)
+        pt.set_value("free/ty", 2.0)
+        pt.set_value("free/tz", 1.0)  # axial perturbation
+
+        ok = newton_solve(residuals, pt, quat_groups=qg, max_iter=100, tol=1e-10)
+        assert ok
+
+        env = pt.get_env()
+        # Cylindrical allows x/y freedom only on the axis line,
+        # but Planar distance=0 must hold: z stays at 0
+        assert abs(env["free/tz"]) < 1e-8, (
+            f"free/tz = {env['free/tz']:.6e}, expected ~0.0 after drag re-solve"
+        )