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:
@@ -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,
|
||||
|
||||
Reference in New Issue
Block a user