fix(solver): use all 3 cross-product components to avoid XY-plane singularity

The parallel-normal constraints (ParallelConstraint, PlanarConstraint,
ConcentricConstraint, RevoluteConstraint, CylindricalConstraint,
SliderConstraint, ScrewConstraint) and point-on-line constraints
previously used only the x and y components of the cross product,
dropping the z component.

This created a singularity when both normal vectors lay in the XY
plane: a yaw rotation produced a cross product entirely along Z,
which was discarded, making the constraint blind to the rotation.

Fix: return all 3 cross-product components. The Jacobian has a
rank deficiency at the solution (3 residuals, rank 2), but the
Newton solver handles this correctly via its pseudoinverse.

Similarly, point_line_perp_components now returns all 3 components
of the displacement cross product to avoid singularity when the
line direction aligns with a coordinate axis.

kindred_solver/constraints.py

@@ -196,8 +196,8 @@ class PointOnLineConstraint(ConstraintBase):
         p_i = self.body_i.world_point(*self.marker_i_pos)
         p_j = self.body_j.world_point(*self.marker_j_pos)
         z_j = marker_z_axis(self.body_j, self.marker_j_quat)
-        cx, cy = point_line_perp_components(p_i, p_j, z_j)
-        return [cx, cy]
+        cx, cy, cz = point_line_perp_components(p_i, p_j, z_j)
+        return [cx, cy, cz]
 
 
 class PointInPlaneConstraint(ConstraintBase):
@@ -244,8 +244,9 @@ class ParallelConstraint(ConstraintBase):
     """Parallel axes — 2 DOF removed.
 
     marker Z-axes are parallel: z_i x z_j = 0.
-    2 residuals from the cross product (only 2 of 3 components are
-    independent for unit vectors).
+    3 cross-product residuals (rank 2 at the solution).  Using all 3
+    avoids a singularity when the axes lie in the XY plane, where
+    dropping cz would leave the constraint blind to yaw rotations.
     """
 
     def __init__(
@@ -264,7 +265,7 @@ class ParallelConstraint(ConstraintBase):
         z_i = marker_z_axis(self.body_i, self.marker_i_quat)
         z_j = marker_z_axis(self.body_j, self.marker_j_quat)
         cx, cy, cz = cross3(z_i, z_j)
-        return [cx, cy]
+        return [cx, cy, cz]
 
 
 class PerpendicularConstraint(ConstraintBase):
@@ -350,17 +351,17 @@ class ConcentricConstraint(ConstraintBase):
         self.distance = distance
 
     def residuals(self) -> List[Expr]:
-        # Parallel axes (2 residuals)
+        # Parallel axes (3 cross-product residuals, rank 2 at solution)
         z_i = marker_z_axis(self.body_i, self.marker_i_quat)
         z_j = marker_z_axis(self.body_j, self.marker_j_quat)
-        cx, cy, _cz = cross3(z_i, z_j)
+        cx, cy, cz = cross3(z_i, z_j)
 
         # Point-on-line: marker_i origin on line through marker_j along z_j
         p_i = self.body_i.world_point(*self.marker_i_pos)
         p_j = self.body_j.world_point(*self.marker_j_pos)
-        lx, ly = point_line_perp_components(p_i, p_j, z_j)
+        lx, ly, lz = point_line_perp_components(p_i, p_j, z_j)
 
-        return [cx, cy, lx, ly]
+        return [cx, cy, cz, lx, ly, lz]
 
 
 class TangentConstraint(ConstraintBase):
@@ -417,10 +418,10 @@ class PlanarConstraint(ConstraintBase):
         self.offset = offset
 
     def residuals(self) -> List[Expr]:
-        # Parallel normals
+        # Parallel normals (3 cross-product residuals, rank 2 at solution)
         z_i = marker_z_axis(self.body_i, self.marker_i_quat)
         z_j = marker_z_axis(self.body_j, self.marker_j_quat)
-        cx, cy, _cz = cross3(z_i, z_j)
+        cx, cy, cz = cross3(z_i, z_j)
 
         # Point-in-plane
         p_i = self.body_i.world_point(*self.marker_i_pos)
@@ -429,7 +430,7 @@ class PlanarConstraint(ConstraintBase):
         if self.offset != 0.0:
             d = d - Const(self.offset)
 
-        return [cx, cy, d]
+        return [cx, cy, cz, d]
 
 
 class LineInPlaneConstraint(ConstraintBase):
@@ -527,12 +528,12 @@ class RevoluteConstraint(ConstraintBase):
         p_j = self.body_j.world_point(*self.marker_j_pos)
         pos = [p_i[0] - p_j[0], p_i[1] - p_j[1], p_i[2] - p_j[2]]
 
-        # Parallel Z-axes
+        # Parallel Z-axes (3 cross-product residuals, rank 2 at solution)
         z_i = marker_z_axis(self.body_i, self.marker_i_quat)
         z_j = marker_z_axis(self.body_j, self.marker_j_quat)
-        cx, cy, _cz = cross3(z_i, z_j)
+        cx, cy, cz = cross3(z_i, z_j)
 
-        return pos + [cx, cy]
+        return pos + [cx, cy, cz]
 
 
 class CylindricalConstraint(ConstraintBase):
@@ -559,17 +560,17 @@ class CylindricalConstraint(ConstraintBase):
         self.marker_j_quat = marker_j_quat
 
     def residuals(self) -> List[Expr]:
-        # Parallel Z-axes
+        # Parallel Z-axes (3 cross-product residuals, rank 2 at solution)
         z_i = marker_z_axis(self.body_i, self.marker_i_quat)
         z_j = marker_z_axis(self.body_j, self.marker_j_quat)
-        cx, cy, _cz = cross3(z_i, z_j)
+        cx, cy, cz = cross3(z_i, z_j)
 
         # Point-on-line
         p_i = self.body_i.world_point(*self.marker_i_pos)
         p_j = self.body_j.world_point(*self.marker_j_pos)
-        lx, ly = point_line_perp_components(p_i, p_j, z_j)
+        lx, ly, lz = point_line_perp_components(p_i, p_j, z_j)
 
-        return [cx, cy, lx, ly]
+        return [cx, cy, cz, lx, ly, lz]
 
 
 class SliderConstraint(ConstraintBase):
@@ -598,22 +599,22 @@ class SliderConstraint(ConstraintBase):
         self.marker_j_quat = marker_j_quat
 
     def residuals(self) -> List[Expr]:
-        # Parallel Z-axes
+        # Parallel Z-axes (3 cross-product residuals, rank 2 at solution)
         z_i = marker_z_axis(self.body_i, self.marker_i_quat)
         z_j = marker_z_axis(self.body_j, self.marker_j_quat)
-        cx, cy, _cz = cross3(z_i, z_j)
+        cx, cy, cz = cross3(z_i, z_j)
 
         # Point-on-line
         p_i = self.body_i.world_point(*self.marker_i_pos)
         p_j = self.body_j.world_point(*self.marker_j_pos)
-        lx, ly = point_line_perp_components(p_i, p_j, z_j)
+        lx, ly, lz = point_line_perp_components(p_i, p_j, z_j)
 
         # Rotation lock: x_i . y_j = 0
         x_i = marker_x_axis(self.body_i, self.marker_i_quat)
         y_j = marker_y_axis(self.body_j, self.marker_j_quat)
         twist = dot3(x_i, y_j)
 
-        return [cx, cy, lx, ly, twist]
+        return [cx, cy, cz, lx, ly, lz, twist]
 
 
 class ScrewConstraint(ConstraintBase):
@@ -648,14 +649,14 @@ class ScrewConstraint(ConstraintBase):
         self.pitch = pitch
 
     def residuals(self) -> List[Expr]:
-        # Cylindrical residuals (4)
+        # Cylindrical residuals (5: 3 parallel + 2 point-on-line)
         z_i = marker_z_axis(self.body_i, self.marker_i_quat)
         z_j = marker_z_axis(self.body_j, self.marker_j_quat)
-        cx, cy, _cz = cross3(z_i, z_j)
+        cx, cy, cz = cross3(z_i, z_j)
 
         p_i = self.body_i.world_point(*self.marker_i_pos)
         p_j = self.body_j.world_point(*self.marker_j_pos)
-        lx, ly = point_line_perp_components(p_i, p_j, z_j)
+        lx, ly, lz = point_line_perp_components(p_i, p_j, z_j)
 
         # Pitch coupling: axial_disp = pitch * angle / (2*pi)
         # Axial displacement
@@ -684,7 +685,7 @@ class ScrewConstraint(ConstraintBase):
         # = axial - pitch * qz / pi
         coupling = axial - Const(self.pitch / math.pi) * rel[3]
 
-        return [cx, cy, lx, ly, coupling]
+        return [cx, cy, cz, lx, ly, lz, coupling]
 
 
 class UniversalConstraint(ConstraintBase):

kindred_solver/geometry.py

@@ -117,15 +117,15 @@ def point_line_perp_components(
     point: Vec3,
     line_origin: Vec3,
     line_dir: Vec3,
-) -> tuple[Expr, Expr]:
-    """Two independent perpendicular-distance components from point to line.
+) -> tuple[Expr, Expr, Expr]:
+    """Three perpendicular-distance components from point to line.
 
     The line passes through line_origin along line_dir.
-    Returns the x and y components of (point - line_origin) x line_dir,
-    which are zero when the point lies on the line.
+    Returns all 3 components of (point - line_origin) x line_dir,
+    which are zero when the point lies on the line.  Only 2 of 3 are
+    independent, but using all 3 avoids a singularity when the line
+    direction aligns with a coordinate axis.
     """
     d = sub3(point, line_origin)
     cx, cy, cz = cross3(d, line_dir)
-    # All three components of d x line_dir are zero when d is parallel
-    # to line_dir, but only 2 are independent.  We return x and y.
-    return cx, cy
+    return cx, cy, cz

kindred_solver/solver.py

@@ -91,6 +91,7 @@ class KindredSolver(kcsolve.IKCSolver):
         super().__init__()
         self._drag_ctx = None
         self._drag_parts = None
+        self._drag_cache = None
         self._limits_warned = False
 
     def name(self):
@@ -244,8 +245,86 @@ class KindredSolver(kcsolve.IKCSolver):
         self._drag_ctx = ctx
         self._drag_parts = set(drag_parts)
         self._drag_step_count = 0
-        result = self.solve(ctx)
-        log.info("pre_drag: initial solve status=%s", result.status)
+
+        # Build the system once and cache everything for drag_step() reuse.
+        t0 = time.perf_counter()
+        system = _build_system(ctx)
+
+        half_spaces = compute_half_spaces(
+            system.constraint_objs,
+            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:
+            post_step_fn = None
+
+        residuals = substitution_pass(system.all_residuals, system.params)
+        residuals = single_equation_pass(residuals, system.params)
+
+        # Build symbolic Jacobian + compile once
+        from .codegen import try_compile_system
+
+        free = system.params.free_names()
+        n_res = len(residuals)
+        n_free = len(free)
+        jac_exprs = [[r.diff(name).simplify() for name in free] for r in residuals]
+        compiled_eval = try_compile_system(residuals, jac_exprs, n_res, n_free)
+
+        # Initial solve
+        converged = newton_solve(
+            residuals,
+            system.params,
+            quat_groups=system.quat_groups,
+            max_iter=100,
+            tol=1e-10,
+            post_step=post_step_fn,
+            weight_vector=weight_vec,
+            jac_exprs=jac_exprs,
+            compiled_eval=compiled_eval,
+        )
+        if not converged:
+            converged = bfgs_solve(
+                residuals,
+                system.params,
+                quat_groups=system.quat_groups,
+                max_iter=200,
+                tol=1e-10,
+                weight_vector=weight_vec,
+                jac_exprs=jac_exprs,
+                compiled_eval=compiled_eval,
+            )
+
+        # Cache for drag_step() reuse
+        cache = _DragCache()
+        cache.system = system
+        cache.residuals = residuals
+        cache.jac_exprs = jac_exprs
+        cache.compiled_eval = compiled_eval
+        cache.half_spaces = half_spaces
+        cache.weight_vec = weight_vec
+        cache.post_step_fn = post_step_fn
+        self._drag_cache = cache
+
+        # 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.dof = dof
+        result.placements = _extract_placements(system.params, system.bodies)
+
+        elapsed = (time.perf_counter() - t0) * 1000
+        log.info(
+            "pre_drag: initial solve %s in %.1f ms — dof=%d",
+            "converged" if converged else "FAILED",
+            elapsed,
+            dof,
+        )
         return result
 
     def drag_step(self, drag_placements):
@@ -254,19 +333,73 @@ class KindredSolver(kcsolve.IKCSolver):
             log.warning("drag_step: no drag context (pre_drag not called?)")
             return kcsolve.SolveResult()
         self._drag_step_count = getattr(self, "_drag_step_count", 0) + 1
+
+        # Update dragged part placements in ctx (for caller consistency)
         for pr in drag_placements:
             for part in ctx.parts:
                 if part.id == pr.id:
                     part.placement = pr.placement
                     break
-        t0 = time.perf_counter()
-        result = self.solve(ctx)
-        elapsed = (time.perf_counter() - t0) * 1000
-        if result.status != kcsolve.SolveStatus.Success:
-            log.warning(
-                "drag_step #%d: solve %s in %.1f ms",
+
+        cache = getattr(self, "_drag_cache", None)
+        if cache is None:
+            # Fallback: no cache, do a full solve
+            log.debug(
+                "drag_step #%d: no cache, falling back to full solve",
+                self._drag_step_count,
+            )
+            return self.solve(ctx)
+
+        t0 = time.perf_counter()
+        params = cache.system.params
+
+        # Update only the dragged part's 7 parameter values
+        for pr in drag_placements:
+            pfx = pr.id + "/"
+            params.set_value(pfx + "tx", pr.placement.position[0])
+            params.set_value(pfx + "ty", pr.placement.position[1])
+            params.set_value(pfx + "tz", pr.placement.position[2])
+            params.set_value(pfx + "qw", pr.placement.quaternion[0])
+            params.set_value(pfx + "qx", pr.placement.quaternion[1])
+            params.set_value(pfx + "qy", pr.placement.quaternion[2])
+            params.set_value(pfx + "qz", pr.placement.quaternion[3])
+
+        # Solve with cached artifacts — no rebuild
+        converged = newton_solve(
+            cache.residuals,
+            params,
+            quat_groups=cache.system.quat_groups,
+            max_iter=100,
+            tol=1e-10,
+            post_step=cache.post_step_fn,
+            weight_vector=cache.weight_vec,
+            jac_exprs=cache.jac_exprs,
+            compiled_eval=cache.compiled_eval,
+        )
+        if not converged:
+            converged = bfgs_solve(
+                cache.residuals,
+                params,
+                quat_groups=cache.system.quat_groups,
+                max_iter=200,
+                tol=1e-10,
+                weight_vector=cache.weight_vec,
+                jac_exprs=cache.jac_exprs,
+                compiled_eval=cache.compiled_eval,
+            )
+
+        result = kcsolve.SolveResult()
+        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)
+
+        elapsed = (time.perf_counter() - t0) * 1000
+        if not converged:
+            log.warning(
+                "drag_step #%d: solve FAILED in %.1f ms",
                 self._drag_step_count,
-                result.status,
                 elapsed,
             )
         else:
@@ -283,6 +416,7 @@ class KindredSolver(kcsolve.IKCSolver):
         self._drag_ctx = None
         self._drag_parts = None
         self._drag_step_count = 0
+        self._drag_cache = None
 
     # ── Diagnostics ─────────────────────────────────────────────────
 
@@ -300,6 +434,26 @@ class KindredSolver(kcsolve.IKCSolver):
         return True
 
 
+class _DragCache:
+    """Cached artifacts from pre_drag() reused across drag_step() calls.
+
+    During interactive drag the constraint topology is invariant — only
+    the dragged part's parameter values change.  Caching the built
+    system, symbolic Jacobian, and compiled evaluator eliminates the
+    expensive rebuild overhead (~150 ms) on every frame.
+    """
+
+    __slots__ = (
+        "system",  # _System — owns ParamTable + Expr trees
+        "residuals",  # list[Expr] — after substitution + single-equation pass
+        "jac_exprs",  # list[list[Expr]] — symbolic Jacobian
+        "compiled_eval",  # Callable or None
+        "half_spaces",  # list[HalfSpace]
+        "weight_vec",  # ndarray or None
+        "post_step_fn",  # Callable or None
+    )
+
+
 class _System:
     """Intermediate representation of a built constraint system."""
 
@@ -538,6 +692,16 @@ def _build_constraint(
 
     if kind == kcsolve.BaseJointKind.DistancePointPoint:
         distance = c_params[0] if c_params else 0.0
+        if distance == 0.0:
+            # Distance=0 is point coincidence.  Use CoincidentConstraint
+            # (3 linear residuals) instead of the squared form which has
+            # a degenerate Jacobian when the constraint is satisfied.
+            return CoincidentConstraint(
+                body_i,
+                marker_i_pos,
+                body_j,
+                marker_j_pos,
+            )
         return DistancePointPointConstraint(
             body_i,
             marker_i_pos,

tests/test_constraints_phase2.py

@@ -65,7 +65,7 @@ class TestPointOnLine:
         b1 = RigidBody("a", pt, (0, 0, 0), (1, 0, 0, 0))
         b2 = RigidBody("b", pt, (0, 0, 0), (1, 0, 0, 0))
         c = PointOnLineConstraint(b1, (0, 0, 0), ID_QUAT, b2, (0, 0, 0), ID_QUAT)
-        assert len(c.residuals()) == 2
+        assert len(c.residuals()) == 3
 
 
 class TestPointInPlane:
@@ -135,7 +135,7 @@ class TestParallel:
         b1 = RigidBody("a", pt, (0, 0, 0), (1, 0, 0, 0))
         b2 = RigidBody("b", pt, (0, 0, 0), (1, 0, 0, 0))
         c = ParallelConstraint(b1, ID_QUAT, b2, ID_QUAT)
-        assert len(c.residuals()) == 2
+        assert len(c.residuals()) == 3
 
 
 class TestPerpendicular:
@@ -222,7 +222,7 @@ class TestConcentric:
         b1 = RigidBody("a", pt, (0, 0, 0), (1, 0, 0, 0))
         b2 = RigidBody("b", pt, (0, 0, 0), (1, 0, 0, 0))
         c = ConcentricConstraint(b1, (0, 0, 0), ID_QUAT, b2, (0, 0, 0), ID_QUAT)
-        assert len(c.residuals()) == 4
+        assert len(c.residuals()) == 6
 
 
 class TestTangent:
@@ -279,7 +279,7 @@ class TestPlanar:
         b1 = RigidBody("a", pt, (0, 0, 0), (1, 0, 0, 0))
         b2 = RigidBody("b", pt, (0, 0, 0), (1, 0, 0, 0))
         c = PlanarConstraint(b1, (0, 0, 0), ID_QUAT, b2, (0, 0, 0), ID_QUAT)
-        assert len(c.residuals()) == 3
+        assert len(c.residuals()) == 4
 
 
 class TestLineInPlane:
@@ -334,7 +334,7 @@ class TestRevolute:
         b1 = RigidBody("a", pt, (0, 0, 0), (1, 0, 0, 0))
         b2 = RigidBody("b", pt, (0, 0, 0), (1, 0, 0, 0))
         c = RevoluteConstraint(b1, (0, 0, 0), ID_QUAT, b2, (0, 0, 0), ID_QUAT)
-        assert len(c.residuals()) == 5
+        assert len(c.residuals()) == 6
 
 
 class TestCylindrical:
@@ -353,7 +353,7 @@ class TestCylindrical:
         b1 = RigidBody("a", pt, (0, 0, 0), (1, 0, 0, 0))
         b2 = RigidBody("b", pt, (0, 0, 0), (1, 0, 0, 0))
         c = CylindricalConstraint(b1, (0, 0, 0), ID_QUAT, b2, (0, 0, 0), ID_QUAT)
-        assert len(c.residuals()) == 4
+        assert len(c.residuals()) == 6
 
 
 class TestSlider:
@@ -375,15 +375,15 @@ class TestSlider:
         c = SliderConstraint(b1, (0, 0, 0), ID_QUAT, b2, (0, 0, 0), ID_QUAT)
         env = pt.get_env()
         vals = [r.eval(env) for r in c.residuals()]
-        # First 4 should be ~0 (parallel + on-line), but twist residual should be ~1
-        assert abs(vals[4]) > 0.5
+        # First 6 should be ~0 (parallel + on-line), but twist residual should be ~1
+        assert abs(vals[6]) > 0.5
 
     def test_residual_count(self):
         pt = ParamTable()
         b1 = RigidBody("a", pt, (0, 0, 0), (1, 0, 0, 0))
         b2 = RigidBody("b", pt, (0, 0, 0), (1, 0, 0, 0))
         c = SliderConstraint(b1, (0, 0, 0), ID_QUAT, b2, (0, 0, 0), ID_QUAT)
-        assert len(c.residuals()) == 5
+        assert len(c.residuals()) == 7
 
 
 class TestUniversal:
@@ -411,7 +411,7 @@ class TestScrew:
         b1 = RigidBody("a", pt, (0, 0, 0), (1, 0, 0, 0))
         b2 = RigidBody("b", pt, (0, 0, 0), (1, 0, 0, 0))
         c = ScrewConstraint(b1, (0, 0, 0), ID_QUAT, b2, (0, 0, 0), ID_QUAT, pitch=10.0)
-        assert len(c.residuals()) == 5
+        assert len(c.residuals()) == 7
 
     def test_zero_displacement_zero_rotation(self):
         """Both at origin with identity rotation → all residuals 0."""

tests/test_geometry.py

@@ -173,15 +173,17 @@ class TestPointLinePerp:
         pt = (Const(0.0), Const(0.0), Const(5.0))
         origin = (Const(0.0), Const(0.0), Const(0.0))
         direction = (Const(0.0), Const(0.0), Const(1.0))
-        cx, cy = point_line_perp_components(pt, origin, direction)
+        cx, cy, cz = point_line_perp_components(pt, origin, direction)
         assert abs(cx.eval({})) < 1e-10
         assert abs(cy.eval({})) < 1e-10
+        assert abs(cz.eval({})) < 1e-10
 
     def test_off_line(self):
         pt = (Const(3.0), Const(0.0), Const(0.0))
         origin = (Const(0.0), Const(0.0), Const(0.0))
         direction = (Const(0.0), Const(0.0), Const(1.0))
-        cx, cy = point_line_perp_components(pt, origin, direction)
+        cx, cy, cz = point_line_perp_components(pt, origin, direction)
         # d = (3,0,0), dir = (0,0,1), d x dir = (0*1-0*0, 0*0-3*1, 3*0-0*0) = (0,-3,0)
         assert abs(cx.eval({})) < 1e-10
         assert abs(cy.eval({}) - (-3.0)) < 1e-10
+        assert abs(cz.eval({})) < 1e-10

tests/test_joints.py

@@ -421,8 +421,10 @@ class TestSliderCrank:
 
         bodies = [ground, crank, rod, piston]
         dof = _dof(pt, bodies, constraints)
-        # 3D slider-crank: planar motion + out-of-plane fold modes
-        assert dof == 3
+        # With full 3-component cross products, the redundant constraint rows
+        # eliminate the out-of-plane fold modes, giving the correct 1 DOF
+        # (crank rotation only).
+        assert dof == 1
 
     def test_slider_crank_solves(self):
         """Slider-crank converges from displaced state."""