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.

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/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
@@ -407,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()
@@ -606,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())

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"
+        )