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/solver.py

@@ -202,7 +202,9 @@ 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
@@ -227,7 +229,9 @@ 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
 
@@ -327,7 +331,9 @@ 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)
 
@@ -418,7 +424,9 @@ 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)
 
@@ -502,18 +510,34 @@ def _enforce_quat_continuity(
     pre_step_quats: dict,
     dragged_ids: set,
 ) -> None:
-    """Ensure solved quaternions stay in the same hemisphere as pre-step.
+    """Ensure solved quaternions stay close to the previous step.
 
-    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".
+    Two levels of correction, applied to ALL non-grounded bodies
+    (including dragged parts, whose params Newton re-solves):
 
-    This is the standard short-arc correction used in SLERP interpolation.
+    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 or body.part_id in dragged_ids:
+        if body.grounded:
             continue
         prev = pre_step_quats.get(body.part_id)
         if prev is None:
@@ -525,15 +549,51 @@ def _enforce_quat_continuity(
         qy = params.get_value(pfx + "qy")
         qz = params.get_value(pfx + "qz")
 
-        # Quaternion dot product: positive means same hemisphere
+        # 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:
-            # 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)
+            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):
@@ -649,7 +709,9 @@ 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
@@ -679,7 +741,9 @@ 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
@@ -711,7 +775,9 @@ def _monolithic_solve(all_residuals, params, quat_groups, post_step=None, weight
     )
     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,