kindred/solver
1
0
Fork 0
Code Issues 10 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

fix(solver): prevent orientation flips during interactive drag

Browse Source 
Add half-space tracking for all compound constraints with branch
ambiguity: Planar, Revolute, Concentric, Cylindrical, Slider, Screw,
Universal, PointInPlane, and LineInPlane.  Previously only
DistancePointPoint, Parallel, Angle, and Perpendicular were tracked,
so the Newton-Raphson solver could converge to the wrong branch for
compound constraints — causing parts to drift through plane
constraints while honoring revolute joints.

Add quaternion continuity enforcement in drag_step(): after solving,
each non-dragged body's quaternion is checked against its pre-step
value and negated if in the opposite hemisphere (standard SLERP
short-arc correction).  This prevents the C++ validateNewPlacements()
from rejecting valid solutions as 'flipped orientation' due to the
quaternion double-cover ambiguity (q and -q encode the same rotation
but measure as ~340° apart).
This commit is contained in:
forbes-0023
2026-02-24 20:46:42 -06:00
parent 0825578778
commit c2ebcc3169
2 changed files with 409 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

344
kindred_solver/preference.py
View File

@@ -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
# ============================================================================

66
kindred_solver/solver.py
View File

@@ -4,6 +4,7 @@ expression-based Newton-Raphson solver."""
from __future__ import annotations
import logging
import math
import time
import kcsolve
@@ -312,6 +313,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
@@ -393,6 +402,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
@@ -456,6 +481,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
)
@@ -473,6 +499,46 @@ class _System:
)
def _enforce_quat_continuity(
params: ParamTable,
bodies: dict,
pre_step_quats: dict,
dragged_ids: set,
) -> None:
"""Ensure solved quaternions stay in the same hemisphere as pre-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".
This is the standard short-arc correction used in SLERP interpolation.
"""
for body in bodies.values():
if body.grounded or body.part_id in dragged_ids:
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")
# Quaternion dot product: positive means same hemisphere
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)
def _build_system(ctx):
"""Build the solver's internal representation from a SolveContext.