From f85dc047e8369985e2e617e78309325b4e1f36fd Mon Sep 17 00:00:00 2001 From: forbes-0023 Date: Fri, 27 Feb 2026 09:37:00 -0600 Subject: [PATCH] fix(solver): enforce quaternion continuity on dragged parts during drag (#338) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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 | 110 +++++++++++++++++++++++++++++++-------- 1 file changed, 88 insertions(+), 22 deletions(-) diff --git a/kindred_solver/solver.py b/kindred_solver/solver.py index 0a3205b..997273a 100644 --- a/kindred_solver/solver.py +++ b/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, -- 2.49.1