fix(solver): redirect distance=0 constraint to CoincidentConstraint

DistancePointPointConstraint uses a squared residual (||p_i-p_j||^2 - d^2)
which has a degenerate Jacobian when d=0 and the constraint is satisfied
(all partial derivatives vanish). This made the constraint invisible to
the Newton solver during drag, allowing constrained points to drift apart.

When distance=0, use CoincidentConstraint instead (3 linear residuals:
dx, dy, dz) which always has a well-conditioned Jacobian.
This commit is contained in:
forbes-0023
2026-02-21 11:46:47 -06:00
parent 64b1e24467
commit e0468cd3c1

View File

@@ -538,6 +538,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,