fix: remove planar half-space correction for on-plane distance=0 constraints
When a PlanarConstraint has distance=0 (point already on plane), the half-space tracker was using dot(z_i, z_j) as its indicator and providing a correction function that reflects the body through the plane. When combined with a Cylindrical joint, legitimate rotation about the cylinder axis causes the body's planar face normal to rotate. After ~90 degrees, the dot product crosses zero, triggering the correction which teleports the body to the other side of the plane. The C++ validator then rejects every subsequent drag step as 'flipped orientation'. Fix: return a tracking-only HalfSpace (no correction_fn) when the point is already on the plane. This matches the pattern used by Cylindrical, Revolute, and Concentric half-space trackers. The cross-product residuals in PlanarConstraint already enforce normal alignment via the solver itself, so the correction is redundant and harmful during drag.
This commit is contained in:
@@ -283,11 +283,18 @@ def _planar_half_space(
|
||||
# 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
|
||||
# Point is on the plane — track normal direction only.
|
||||
# No correction: when combined with rotational joints (Cylindrical,
|
||||
# Revolute), the body's marker normal rotates legitimately and
|
||||
# reflecting through the plane would fight the rotation.
|
||||
def indicator(e: dict[str, float]) -> float:
|
||||
return dot_expr.eval(e)
|
||||
|
||||
ref_sign = normal_ref_sign
|
||||
return HalfSpace(
|
||||
constraint_index=constraint_idx,
|
||||
reference_sign=normal_ref_sign,
|
||||
indicator_fn=indicator,
|
||||
)
|
||||
else:
|
||||
# Point is off-plane — track which side
|
||||
def indicator(e: dict[str, float]) -> float:
|
||||
|
||||
Reference in New Issue
Block a user