fix(solver): enforce quaternion continuity on dragged parts during drag (#338)

The _enforce_quat_continuity function previously skipped dragged parts,
assuming the GUI directly controls their placement.  However, Newton
re-solves all free params (including the dragged part's) to satisfy
constraints, and can converge to an equivalent but distinct quaternion
branch.  The C++ validateNewPlacements() then sees a >91 degree rotation
and rejects the step.

Two-level fix:

1. Remove the dragged_ids skip — apply continuity to ALL non-grounded
   bodies, including the dragged part.

2. Add rotation angle check beyond simple hemisphere negation: compute
   the relative quaternion angle using the same formula as the C++
   validator (2*acos(w)).  If it exceeds 91 degrees, reset to the
   previous step's quaternion.  This catches branch jumps where the
   solver finds a geometrically different but constraint-satisfying
   orientation (e.g. Cylindrical + Planar with 180-degree ambiguity).

Verified: all 291 solver tests pass.

kindred_solver/preference.py

@@ -21,12 +21,21 @@ import numpy as np
 
 from .constraints import (
     AngleConstraint,
+    ConcentricConstraint,
     ConstraintBase,
+    CylindricalConstraint,
     DistancePointPointConstraint,
+    LineInPlaneConstraint,
     ParallelConstraint,
     PerpendicularConstraint,
+    PlanarConstraint,
+    PointInPlaneConstraint,
+    RevoluteConstraint,
+    ScrewConstraint,
+    SliderConstraint,
+    UniversalConstraint,
 )
-from .geometry import cross3, dot3, marker_z_axis
+from .geometry import cross3, dot3, marker_z_axis, point_plane_distance, sub3
 from .params import ParamTable
 
 
@@ -107,6 +116,33 @@ def _build_half_space(
     if isinstance(obj, PerpendicularConstraint):
         return _perpendicular_half_space(obj, constraint_idx, env, params)
 
+    if isinstance(obj, PlanarConstraint):
+        return _planar_half_space(obj, constraint_idx, env, params)
+
+    if isinstance(obj, RevoluteConstraint):
+        return _revolute_half_space(obj, constraint_idx, env, params)
+
+    if isinstance(obj, ConcentricConstraint):
+        return _concentric_half_space(obj, constraint_idx, env, params)
+
+    if isinstance(obj, PointInPlaneConstraint):
+        return _point_in_plane_half_space(obj, constraint_idx, env, params)
+
+    if isinstance(obj, LineInPlaneConstraint):
+        return _line_in_plane_half_space(obj, constraint_idx, env, params)
+
+    if isinstance(obj, CylindricalConstraint):
+        return _axis_direction_half_space(obj, constraint_idx, env)
+
+    if isinstance(obj, SliderConstraint):
+        return _axis_direction_half_space(obj, constraint_idx, env)
+
+    if isinstance(obj, ScrewConstraint):
+        return _axis_direction_half_space(obj, constraint_idx, env)
+
+    if isinstance(obj, UniversalConstraint):
+        return _universal_half_space(obj, constraint_idx, env)
+
     return None
 
 
@@ -212,6 +248,312 @@ def _parallel_half_space(
     )
 
 
+def _planar_half_space(
+    obj: PlanarConstraint,
+    constraint_idx: int,
+    env: dict[str, float],
+    params: ParamTable,
+) -> HalfSpace | None:
+    """Half-space for Planar: track which side of the plane the point is on
+    AND which direction the normals face.
+
+    A Planar constraint has parallel normals (cross product = 0) plus
+    point-in-plane (signed distance = 0).  Both have branch ambiguity:
+    the normals can be same-direction or opposite, and the point can
+    approach the plane from either side.  We track the signed distance
+    from marker_i to the plane defined by marker_j — this captures
+    the plane-side and is the primary drift mode during drag.
+    """
+    # Point-in-plane signed distance as indicator
+    p_i = obj.body_i.world_point(*obj.marker_i_pos)
+    p_j = obj.body_j.world_point(*obj.marker_j_pos)
+    z_j = marker_z_axis(obj.body_j, obj.marker_j_quat)
+    d_expr = point_plane_distance(p_i, p_j, z_j)
+
+    d_val = d_expr.eval(env)
+
+    # Also track normal alignment (same as Parallel half-space)
+    z_i = marker_z_axis(obj.body_i, obj.marker_i_quat)
+    dot_expr = dot3(z_i, z_j)
+    dot_val = dot_expr.eval(env)
+    normal_ref_sign = math.copysign(1.0, dot_val) if abs(dot_val) > 1e-14 else 1.0
+
+    # If offset is zero and distance is near-zero, we still need the normal
+    # direction indicator to prevent flipping through the plane.
+    # 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
+        def indicator(e: dict[str, float]) -> float:
+            return dot_expr.eval(e)
+
+        ref_sign = normal_ref_sign
+    else:
+        # Point is off-plane — track which side
+        def indicator(e: dict[str, float]) -> float:
+            return d_expr.eval(e)
+
+        ref_sign = math.copysign(1.0, d_val)
+
+    # Correction: reflect the moving body's position through the plane
+    moving_body = obj.body_j if not obj.body_j.grounded else obj.body_i
+    if moving_body.grounded:
+        return None
+
+    px_name = f"{moving_body.part_id}/tx"
+    py_name = f"{moving_body.part_id}/ty"
+    pz_name = f"{moving_body.part_id}/tz"
+
+    def correction(p: ParamTable, _val: float) -> None:
+        e = p.get_env()
+        # Recompute signed distance and normal direction
+        d_cur = d_expr.eval(e)
+        nx = z_j[0].eval(e)
+        ny = z_j[1].eval(e)
+        nz = z_j[2].eval(e)
+        n_len = math.sqrt(nx * nx + ny * ny + nz * nz)
+        if n_len < 1e-15:
+            return
+        nx, ny, nz = nx / n_len, ny / n_len, nz / n_len
+        # Reflect through plane: move body by -2*d along normal
+        sign = -1.0 if moving_body is obj.body_j else 1.0
+        if not p.is_fixed(px_name):
+            p.set_value(px_name, p.get_value(px_name) + sign * 2.0 * d_cur * nx)
+        if not p.is_fixed(py_name):
+            p.set_value(py_name, p.get_value(py_name) + sign * 2.0 * d_cur * ny)
+        if not p.is_fixed(pz_name):
+            p.set_value(pz_name, p.get_value(pz_name) + sign * 2.0 * d_cur * nz)
+
+    return HalfSpace(
+        constraint_index=constraint_idx,
+        reference_sign=ref_sign,
+        indicator_fn=indicator,
+        correction_fn=correction,
+    )
+
+
+def _revolute_half_space(
+    obj: RevoluteConstraint,
+    constraint_idx: int,
+    env: dict[str, float],
+    params: ParamTable,
+) -> HalfSpace | None:
+    """Half-space for Revolute: track hinge axis direction.
+
+    A revolute has coincident origins + parallel Z-axes.  The parallel
+    axes can flip direction (same ambiguity as Parallel).  Track
+    dot(z_i, z_j) to prevent the axis from inverting.
+    """
+    z_i = marker_z_axis(obj.body_i, obj.marker_i_quat)
+    z_j = marker_z_axis(obj.body_j, obj.marker_j_quat)
+    dot_expr = dot3(z_i, z_j)
+
+    ref_val = dot_expr.eval(env)
+    ref_sign = math.copysign(1.0, ref_val) if abs(ref_val) > 1e-14 else 1.0
+
+    return HalfSpace(
+        constraint_index=constraint_idx,
+        reference_sign=ref_sign,
+        indicator_fn=lambda e: dot_expr.eval(e),
+    )
+
+
+def _concentric_half_space(
+    obj: ConcentricConstraint,
+    constraint_idx: int,
+    env: dict[str, float],
+    params: ParamTable,
+) -> HalfSpace | None:
+    """Half-space for Concentric: track axis direction.
+
+    Concentric has parallel axes + point-on-line.  The parallel axes
+    can flip direction.  Track dot(z_i, z_j).
+    """
+    z_i = marker_z_axis(obj.body_i, obj.marker_i_quat)
+    z_j = marker_z_axis(obj.body_j, obj.marker_j_quat)
+    dot_expr = dot3(z_i, z_j)
+
+    ref_val = dot_expr.eval(env)
+    ref_sign = math.copysign(1.0, ref_val) if abs(ref_val) > 1e-14 else 1.0
+
+    return HalfSpace(
+        constraint_index=constraint_idx,
+        reference_sign=ref_sign,
+        indicator_fn=lambda e: dot_expr.eval(e),
+    )
+
+
+def _point_in_plane_half_space(
+    obj: PointInPlaneConstraint,
+    constraint_idx: int,
+    env: dict[str, float],
+    params: ParamTable,
+) -> HalfSpace | None:
+    """Half-space for PointInPlane: track which side of the plane.
+
+    The signed distance to the plane can be satisfied from either side.
+    Track which side the initial configuration is on.
+    """
+    p_i = obj.body_i.world_point(*obj.marker_i_pos)
+    p_j = obj.body_j.world_point(*obj.marker_j_pos)
+    n_j = marker_z_axis(obj.body_j, obj.marker_j_quat)
+    d_expr = point_plane_distance(p_i, p_j, n_j)
+
+    d_val = d_expr.eval(env)
+    if abs(d_val) < 1e-10:
+        return None  # already on the plane, no branch to track
+
+    ref_sign = math.copysign(1.0, d_val)
+
+    moving_body = obj.body_j if not obj.body_j.grounded else obj.body_i
+    if moving_body.grounded:
+        return None
+
+    px_name = f"{moving_body.part_id}/tx"
+    py_name = f"{moving_body.part_id}/ty"
+    pz_name = f"{moving_body.part_id}/tz"
+
+    def correction(p: ParamTable, _val: float) -> None:
+        e = p.get_env()
+        d_cur = d_expr.eval(e)
+        nx = n_j[0].eval(e)
+        ny = n_j[1].eval(e)
+        nz = n_j[2].eval(e)
+        n_len = math.sqrt(nx * nx + ny * ny + nz * nz)
+        if n_len < 1e-15:
+            return
+        nx, ny, nz = nx / n_len, ny / n_len, nz / n_len
+        sign = -1.0 if moving_body is obj.body_j else 1.0
+        if not p.is_fixed(px_name):
+            p.set_value(px_name, p.get_value(px_name) + sign * 2.0 * d_cur * nx)
+        if not p.is_fixed(py_name):
+            p.set_value(py_name, p.get_value(py_name) + sign * 2.0 * d_cur * ny)
+        if not p.is_fixed(pz_name):
+            p.set_value(pz_name, p.get_value(pz_name) + sign * 2.0 * d_cur * nz)
+
+    return HalfSpace(
+        constraint_index=constraint_idx,
+        reference_sign=ref_sign,
+        indicator_fn=lambda e: d_expr.eval(e),
+        correction_fn=correction,
+    )
+
+
+def _line_in_plane_half_space(
+    obj: LineInPlaneConstraint,
+    constraint_idx: int,
+    env: dict[str, float],
+    params: ParamTable,
+) -> HalfSpace | None:
+    """Half-space for LineInPlane: track which side of the plane.
+
+    Same plane-side ambiguity as PointInPlane.
+    """
+    p_i = obj.body_i.world_point(*obj.marker_i_pos)
+    p_j = obj.body_j.world_point(*obj.marker_j_pos)
+    n_j = marker_z_axis(obj.body_j, obj.marker_j_quat)
+    d_expr = point_plane_distance(p_i, p_j, n_j)
+
+    d_val = d_expr.eval(env)
+    if abs(d_val) < 1e-10:
+        return None
+
+    ref_sign = math.copysign(1.0, d_val)
+
+    moving_body = obj.body_j if not obj.body_j.grounded else obj.body_i
+    if moving_body.grounded:
+        return None
+
+    px_name = f"{moving_body.part_id}/tx"
+    py_name = f"{moving_body.part_id}/ty"
+    pz_name = f"{moving_body.part_id}/tz"
+
+    def correction(p: ParamTable, _val: float) -> None:
+        e = p.get_env()
+        d_cur = d_expr.eval(e)
+        nx = n_j[0].eval(e)
+        ny = n_j[1].eval(e)
+        nz = n_j[2].eval(e)
+        n_len = math.sqrt(nx * nx + ny * ny + nz * nz)
+        if n_len < 1e-15:
+            return
+        nx, ny, nz = nx / n_len, ny / n_len, nz / n_len
+        sign = -1.0 if moving_body is obj.body_j else 1.0
+        if not p.is_fixed(px_name):
+            p.set_value(px_name, p.get_value(px_name) + sign * 2.0 * d_cur * nx)
+        if not p.is_fixed(py_name):
+            p.set_value(py_name, p.get_value(py_name) + sign * 2.0 * d_cur * ny)
+        if not p.is_fixed(pz_name):
+            p.set_value(pz_name, p.get_value(pz_name) + sign * 2.0 * d_cur * nz)
+
+    return HalfSpace(
+        constraint_index=constraint_idx,
+        reference_sign=ref_sign,
+        indicator_fn=lambda e: d_expr.eval(e),
+        correction_fn=correction,
+    )
+
+
+def _axis_direction_half_space(
+    obj,
+    constraint_idx: int,
+    env: dict[str, float],
+) -> HalfSpace | None:
+    """Half-space for any constraint with parallel Z-axes (Cylindrical, Slider, Screw).
+
+    Tracks dot(z_i, z_j) to prevent axis inversion.
+    """
+    z_i = marker_z_axis(obj.body_i, obj.marker_i_quat)
+    z_j = marker_z_axis(obj.body_j, obj.marker_j_quat)
+    dot_expr = dot3(z_i, z_j)
+
+    ref_val = dot_expr.eval(env)
+    ref_sign = math.copysign(1.0, ref_val) if abs(ref_val) > 1e-14 else 1.0
+
+    return HalfSpace(
+        constraint_index=constraint_idx,
+        reference_sign=ref_sign,
+        indicator_fn=lambda e: dot_expr.eval(e),
+    )
+
+
+def _universal_half_space(
+    obj: UniversalConstraint,
+    constraint_idx: int,
+    env: dict[str, float],
+) -> HalfSpace | None:
+    """Half-space for Universal: track which quadrant of perpendicularity.
+
+    Universal has dot(z_i, z_j) = 0 (perpendicular).  The cross product
+    sign distinguishes which "side" of perpendicular.
+    """
+    z_i = marker_z_axis(obj.body_i, obj.marker_i_quat)
+    z_j = marker_z_axis(obj.body_j, obj.marker_j_quat)
+    cx, cy, cz = cross3(z_i, z_j)
+
+    cx_val = cx.eval(env)
+    cy_val = cy.eval(env)
+    cz_val = cz.eval(env)
+
+    components = [
+        (abs(cx_val), cx, cx_val),
+        (abs(cy_val), cy, cy_val),
+        (abs(cz_val), cz, cz_val),
+    ]
+    _, best_expr, best_val = max(components, key=lambda t: t[0])
+
+    if abs(best_val) < 1e-14:
+        return None
+
+    ref_sign = math.copysign(1.0, best_val)
+
+    return HalfSpace(
+        constraint_index=constraint_idx,
+        reference_sign=ref_sign,
+        indicator_fn=lambda e: best_expr.eval(e),
+    )
+
+
 # ============================================================================
 # Minimum-movement weighting
 # ============================================================================

kindred_solver/solver.py

@@ -4,6 +4,7 @@ expression-based Newton-Raphson solver."""
 from __future__ import annotations
 
 import logging
+import math
 import time
 
 import kcsolve
@@ -130,8 +131,7 @@ class KindredSolver(kcsolve.IKCSolver):
             for c in ctx.constraints:
                 if c.limits:
                     log.warning(
-                        "Joint limits on '%s' ignored "
-                        "(not yet supported by Kindred solver)",
+                        "Joint limits on '%s' ignored (not yet supported by Kindred solver)",
                         c.id,
                     )
                     self._limits_warned = True
@@ -143,8 +143,6 @@ class KindredSolver(kcsolve.IKCSolver):
             system.constraint_indices,
             system.params,
         )
-        weight_vec = build_weight_vector(system.params)
-
         if half_spaces:
             post_step_fn = lambda p: apply_half_space_correction(p, half_spaces)
         else:
@@ -154,6 +152,10 @@ class KindredSolver(kcsolve.IKCSolver):
         residuals = substitution_pass(system.all_residuals, system.params)
         residuals = single_equation_pass(residuals, system.params)
 
+        # Build weight vector *after* pre-passes so its length matches the
+        # remaining free parameters (single_equation_pass may fix some).
+        weight_vec = build_weight_vector(system.params)
+
         # Solve (decomposed for large assemblies, monolithic for small)
         jac_exprs = None  # may be populated by _monolithic_solve
         if n_free_bodies >= _DECOMPOSE_THRESHOLD:
@@ -255,7 +257,6 @@ class KindredSolver(kcsolve.IKCSolver):
             system.constraint_indices,
             system.params,
         )
-        weight_vec = build_weight_vector(system.params)
 
         if half_spaces:
             post_step_fn = lambda p: apply_half_space_correction(p, half_spaces)
@@ -263,7 +264,17 @@ class KindredSolver(kcsolve.IKCSolver):
             post_step_fn = None
 
         residuals = substitution_pass(system.all_residuals, system.params)
-        residuals = single_equation_pass(residuals, system.params)
+        # NOTE: single_equation_pass is intentionally skipped for drag.
+        # It permanently fixes variables and removes residuals from the
+        # list.  During drag the dragged part's parameters change each
+        # frame, which can invalidate those analytic solutions and cause
+        # constraints (e.g. Planar distance=0) to stop being enforced.
+        # The substitution pass alone is safe because it only replaces
+        # genuinely grounded parameters with constants.
+
+        # Build weight vector *after* pre-passes so its length matches the
+        # remaining free parameters (single_equation_pass may fix some).
+        weight_vec = build_weight_vector(system.params)
 
         # Build symbolic Jacobian + compile once
         from .codegen import try_compile_system
@@ -307,6 +318,14 @@ class KindredSolver(kcsolve.IKCSolver):
         cache.half_spaces = half_spaces
         cache.weight_vec = weight_vec
         cache.post_step_fn = post_step_fn
+
+        # Snapshot solved quaternions for continuity enforcement in drag_step()
+        env = system.params.get_env()
+        cache.pre_step_quats = {}
+        for body in system.bodies.values():
+            if not body.grounded:
+                cache.pre_step_quats[body.part_id] = body.extract_quaternion(env)
+
         self._drag_cache = cache
 
         # Build result
@@ -388,6 +407,22 @@ class KindredSolver(kcsolve.IKCSolver):
                 compiled_eval=cache.compiled_eval,
             )
 
+        # Quaternion continuity: ensure solved quaternions stay in the
+        # same hemisphere as the previous step.  q and -q encode the
+        # same rotation, but the C++ side measures angle between the
+        # 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
+        )
+
+        # Update the stored quaternions for the next drag step
+        env = params.get_env()
+        for body in cache.system.bodies.values():
+            if not body.grounded:
+                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
@@ -451,6 +486,7 @@ class _DragCache:
         "half_spaces",  # list[HalfSpace]
         "weight_vec",  # ndarray or None
         "post_step_fn",  # Callable or None
+        "pre_step_quats",  # dict[str, tuple] — last-accepted quaternions per body
     )
 
 
@@ -468,6 +504,98 @@ class _System:
     )
 
 
+def _enforce_quat_continuity(
+    params: ParamTable,
+    bodies: dict,
+    pre_step_quats: dict,
+    dragged_ids: set,
+) -> None:
+    """Ensure solved quaternions stay close to the previous step.
+
+    Two levels of correction, applied to ALL non-grounded bodies
+    (including dragged parts, whose params Newton re-solves):
+
+    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.
+    """
+    _MAX_ANGLE = 91.0 * math.pi / 180.0  # match C++ threshold
+
+    for body in bodies.values():
+        if body.grounded:
+            continue
+        prev = pre_step_quats.get(body.part_id)
+        if prev is None:
+            continue
+
+        pfx = body.part_id + "/"
+        qw = params.get_value(pfx + "qw")
+        qx = params.get_value(pfx + "qx")
+        qy = params.get_value(pfx + "qy")
+        qz = params.get_value(pfx + "qz")
+
+        # Level 1: hemisphere check (standard SLERP short-arc correction)
+        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)
+
+
 def _build_system(ctx):
     """Build the solver's internal representation from a SolveContext.
 

tests/test_drag.py

@@ -0,0 +1,253 @@
+"""Regression tests for interactive drag.
+
+These tests exercise the drag protocol at the solver-internals level,
+verifying that constraints remain enforced across drag steps when the
+pre-pass has been applied to cached residuals.
+
+Bug scenario: single_equation_pass runs during pre_drag, analytically
+solving variables from upstream constraints and baking their values as
+constants into downstream residual expressions.  When a drag step
+changes those variables, the cached residuals use stale constants and
+downstream constraints (e.g. Planar distance=0) stop being enforced.
+
+Fix: skip single_equation_pass in the drag path.  Only substitution_pass
+(which replaces genuinely grounded parameters) is safe to cache.
+"""
+
+import math
+
+import pytest
+from kindred_solver.constraints import (
+    CoincidentConstraint,
+    PlanarConstraint,
+    RevoluteConstraint,
+)
+from kindred_solver.entities import RigidBody
+from kindred_solver.newton import newton_solve
+from kindred_solver.params import ParamTable
+from kindred_solver.prepass import single_equation_pass, substitution_pass
+
+ID_QUAT = (1, 0, 0, 0)
+
+
+def _build_residuals(bodies, constraint_objs):
+    """Build raw residual list + quat groups (no prepass)."""
+    all_residuals = []
+    for c in constraint_objs:
+        all_residuals.extend(c.residuals())
+
+    quat_groups = []
+    for body in bodies:
+        if not body.grounded:
+            all_residuals.append(body.quat_norm_residual())
+            quat_groups.append(body.quat_param_names())
+
+    return all_residuals, quat_groups
+
+
+def _eval_raw_residuals(bodies, constraint_objs, params):
+    """Evaluate original constraint residuals at current param values.
+
+    Returns the max absolute residual value — the ground truth for
+    whether constraints are satisfied regardless of prepass state.
+    """
+    raw, _ = _build_residuals(bodies, constraint_objs)
+    env = params.get_env()
+    return max(abs(r.eval(env)) for r in raw)
+
+
+class TestPrepassDragRegression:
+    """single_equation_pass bakes stale values that break drag.
+
+    Setup: ground --Revolute--> arm --Planar(d=0)--> plate
+
+    The Revolute pins arm's origin to ground (fixes arm/tx, arm/ty,
+    arm/tz via single_equation_pass).  The Planar keeps plate coplanar
+    with arm.  After prepass, the Planar residual has arm's position
+    baked as Const(0.0).
+
+    During drag: arm/tz is set to 5.0.  Because arm/tz is marked fixed
+    by prepass, Newton can't correct it, AND the Planar residual still
+    uses Const(0.0) instead of the live value 5.0.  The Revolute
+    constraint (arm at origin) is silently violated.
+    """
+
+    def _setup(self):
+        pt = ParamTable()
+        ground = RigidBody("g", pt, (0, 0, 0), ID_QUAT, grounded=True)
+        arm = RigidBody("arm", pt, (10, 0, 0), ID_QUAT)
+        plate = RigidBody("plate", pt, (10, 5, 0), ID_QUAT)
+
+        constraints = [
+            RevoluteConstraint(ground, (0, 0, 0), ID_QUAT, arm, (0, 0, 0), ID_QUAT),
+            PlanarConstraint(arm, (0, 0, 0), ID_QUAT, plate, (0, 0, 0), ID_QUAT, offset=0.0),
+        ]
+        bodies = [ground, arm, plate]
+        return pt, bodies, constraints
+
+    def test_bug_stale_constants_after_single_equation_pass(self):
+        """Document the bug: prepass bakes arm/tz=0, drag breaks constraints."""
+        pt, bodies, constraints = self._setup()
+        raw_residuals, quat_groups = _build_residuals(bodies, constraints)
+
+        # Simulate OLD pre_drag: substitution + single_equation_pass
+        residuals = substitution_pass(raw_residuals, pt)
+        residuals = single_equation_pass(residuals, pt)
+
+        ok = newton_solve(residuals, pt, quat_groups=quat_groups, max_iter=100, tol=1e-10)
+        assert ok
+
+        # Verify prepass fixed arm's position params
+        assert pt.is_fixed("arm/tx")
+        assert pt.is_fixed("arm/ty")
+        assert pt.is_fixed("arm/tz")
+
+        # Simulate drag: move arm up (set_value, as drag_step does)
+        pt.set_value("arm/tz", 5.0)
+        pt.set_value("plate/tz", 5.0)  # initial guess near drag
+
+        ok = newton_solve(residuals, pt, quat_groups=quat_groups, max_iter=100, tol=1e-10)
+        # Solver "converges" on the stale cached residuals
+        assert ok
+
+        # But the TRUE constraints are violated: arm should be at z=0
+        # (Revolute pins it to ground) yet it's at z=5
+        max_err = _eval_raw_residuals(bodies, constraints, pt)
+        assert max_err > 1.0, (
+            f"Expected large raw residual violation, got {max_err:.6e}. "
+            "The bug should cause the Revolute z-residual to be ~5.0"
+        )
+
+    def test_fix_no_single_equation_pass_for_drag(self):
+        """With the fix: skip single_equation_pass, constraints hold."""
+        pt, bodies, constraints = self._setup()
+        raw_residuals, quat_groups = _build_residuals(bodies, constraints)
+
+        # Simulate FIXED pre_drag: substitution only
+        residuals = substitution_pass(raw_residuals, pt)
+
+        ok = newton_solve(residuals, pt, quat_groups=quat_groups, max_iter=100, tol=1e-10)
+        assert ok
+
+        # arm/tz should NOT be fixed
+        assert not pt.is_fixed("arm/tz")
+
+        # Simulate drag: move arm up
+        pt.set_value("arm/tz", 5.0)
+        pt.set_value("plate/tz", 5.0)
+
+        ok = newton_solve(residuals, pt, quat_groups=quat_groups, max_iter=100, tol=1e-10)
+        assert ok
+
+        # Newton pulls arm back to z=0 (Revolute enforced) and plate follows
+        max_err = _eval_raw_residuals(bodies, constraints, pt)
+        assert max_err < 1e-8, f"Raw residual violation {max_err:.6e} — constraints not satisfied"
+
+
+class TestCoincidentPlanarDragRegression:
+    """Coincident upstream + Planar downstream — same bug class.
+
+    ground --Coincident--> bracket --Planar(d=0)--> plate
+
+    Coincident fixes bracket/tx,ty,tz.  After prepass, the Planar
+    residual has bracket's position baked.  Drag moves bracket;
+    the Planar uses stale constants.
+    """
+
+    def _setup(self):
+        pt = ParamTable()
+        ground = RigidBody("g", pt, (0, 0, 0), ID_QUAT, grounded=True)
+        bracket = RigidBody("bracket", pt, (0, 0, 0), ID_QUAT)
+        plate = RigidBody("plate", pt, (10, 5, 0), ID_QUAT)
+
+        constraints = [
+            CoincidentConstraint(ground, (0, 0, 0), bracket, (0, 0, 0)),
+            PlanarConstraint(bracket, (0, 0, 0), ID_QUAT, plate, (0, 0, 0), ID_QUAT, offset=0.0),
+        ]
+        bodies = [ground, bracket, plate]
+        return pt, bodies, constraints
+
+    def test_bug_coincident_planar(self):
+        """Prepass fixes bracket/tz, Planar uses stale constant during drag."""
+        pt, bodies, constraints = self._setup()
+        raw, qg = _build_residuals(bodies, constraints)
+
+        residuals = substitution_pass(raw, pt)
+        residuals = single_equation_pass(residuals, pt)
+
+        ok = newton_solve(residuals, pt, quat_groups=qg, max_iter=100, tol=1e-10)
+        assert ok
+        assert pt.is_fixed("bracket/tz")
+
+        # Drag bracket up
+        pt.set_value("bracket/tz", 5.0)
+        pt.set_value("plate/tz", 5.0)
+
+        ok = newton_solve(residuals, pt, quat_groups=qg, max_iter=100, tol=1e-10)
+        assert ok
+
+        # True constraints violated
+        max_err = _eval_raw_residuals(bodies, constraints, pt)
+        assert max_err > 1.0, f"Expected raw violation from stale prepass, got {max_err:.6e}"
+
+    def test_fix_coincident_planar(self):
+        """With the fix: constraints satisfied after drag."""
+        pt, bodies, constraints = self._setup()
+        raw, qg = _build_residuals(bodies, constraints)
+
+        residuals = substitution_pass(raw, pt)
+        # No single_equation_pass
+
+        ok = newton_solve(residuals, pt, quat_groups=qg, max_iter=100, tol=1e-10)
+        assert ok
+        assert not pt.is_fixed("bracket/tz")
+
+        # Drag bracket up
+        pt.set_value("bracket/tz", 5.0)
+        pt.set_value("plate/tz", 5.0)
+
+        ok = newton_solve(residuals, pt, quat_groups=qg, max_iter=100, tol=1e-10)
+        assert ok
+
+        max_err = _eval_raw_residuals(bodies, constraints, pt)
+        assert max_err < 1e-8, f"Raw residual violation {max_err:.6e} — constraints not satisfied"
+
+
+class TestDragDoesNotBreakStaticSolve:
+    """Verify that the static solve path (with single_equation_pass) still works.
+
+    The fix only affects pre_drag — the static solve() path continues to
+    use single_equation_pass for faster convergence.
+    """
+
+    def test_static_solve_still_uses_prepass(self):
+        """Static solve with single_equation_pass converges correctly."""
+        pt = ParamTable()
+        ground = RigidBody("g", pt, (0, 0, 0), ID_QUAT, grounded=True)
+        arm = RigidBody("arm", pt, (10, 0, 0), ID_QUAT)
+        plate = RigidBody("plate", pt, (10, 5, 8), ID_QUAT)
+
+        constraints = [
+            RevoluteConstraint(ground, (0, 0, 0), ID_QUAT, arm, (0, 0, 0), ID_QUAT),
+            PlanarConstraint(arm, (0, 0, 0), ID_QUAT, plate, (0, 0, 0), ID_QUAT, offset=0.0),
+        ]
+        bodies = [ground, arm, plate]
+        raw, qg = _build_residuals(bodies, constraints)
+
+        # Full prepass (static solve path)
+        residuals = substitution_pass(raw, pt)
+        residuals = single_equation_pass(residuals, pt)
+
+        ok = newton_solve(residuals, pt, quat_groups=qg, max_iter=100, tol=1e-10)
+        assert ok
+
+        # All raw constraints satisfied
+        max_err = _eval_raw_residuals(bodies, constraints, pt)
+        assert max_err < 1e-8
+
+        # arm at origin (Revolute), plate coplanar (z=0)
+        env = pt.get_env()
+        assert abs(env["arm/tx"]) < 1e-8
+        assert abs(env["arm/ty"]) < 1e-8
+        assert abs(env["arm/tz"]) < 1e-8
+        assert abs(env["plate/tz"]) < 1e-8