test: add console test reproducing planar drag quaternion flip (#338)
Adds console_test_planar_drag.py — a live FreeCAD console test that reproduces the quaternion branch-jump failure from #338. Test 2 (realistic geometry) reliably triggers the bug: 10/40 drag steps rejected by the C++ validateNewPlacements() simulator when the solver converges to an equivalent but distinct quaternion branch around 240-330 deg axial rotation. Key findings from the test: - The failure is NOT simple hemisphere negation (q vs -q) - The solver finds geometrically valid but quaternion-distinct solutions when Cylindrical + Planar constraints have multiple satisfying orientations - _enforce_quat_continuity only catches sign flips, not these deeper branch jumps - The C++ validator uses acos(w) not acos(|w|), so opposite- hemisphere quaternions show as ~360 deg rotation
This commit is contained in:
forbes-0023
720
tests/console_test_planar_drag.py
Normal file
720
tests/console_test_planar_drag.py
Normal file
@@ -0,0 +1,720 @@
|
||||
"""
|
||||
Console test: Cylindrical + Planar drag — reproduces #338.
|
||||
|
||||
Paste into the FreeCAD Python console (or run via exec(open(...).read())).
|
||||
|
||||
This test builds scenarios that trigger the quaternion hemisphere flip
|
||||
during drag that causes the C++ validateNewPlacements() to reject every
|
||||
step with "flipped orientation (360.0 degrees)".
|
||||
|
||||
Key insight: the C++ Rotation::evaluateVector() computes
|
||||
_angle = 2 * acos(w)
|
||||
using the RAW w component (not |w|). When the solver returns a
|
||||
quaternion in the opposite hemisphere (w < 0), the relative rotation
|
||||
relativeRot = newRot * oldRot.inverse()
|
||||
has w ≈ -1, giving angle ≈ 2*acos(-1) = 2*pi = 360 degrees.
|
||||
The 91-degree threshold then rejects it.
|
||||
|
||||
The solver's _enforce_quat_continuity SHOULD fix this, but it skips
|
||||
dragged parts. For the non-dragged bar, the fix only works if the
|
||||
pre_step_quats baseline is correct. This test reproduces the failure
|
||||
by using realistic non-identity geometry.
|
||||
"""
|
||||
|
||||
import math
|
||||
|
||||
import kcsolve
|
||||
|
||||
_results = []
|
||||
|
||||
|
||||
def _report(name, passed, detail=""):
|
||||
status = "PASS" if passed else "FAIL"
|
||||
msg = f" [{status}] {name}"
|
||||
if detail:
|
||||
msg += f" -- {detail}"
|
||||
print(msg)
|
||||
_results.append((name, passed))
|
||||
|
||||
|
||||
# ── Quaternion math ──────────────────────────────────────────────────
|
||||
|
||||
|
||||
def _qmul(a, b):
|
||||
"""Hamilton product (w, x, y, z)."""
|
||||
aw, ax, ay, az = a
|
||||
bw, bx, by, bz = b
|
||||
return (
|
||||
aw * bw - ax * bx - ay * by - az * bz,
|
||||
aw * bx + ax * bw + ay * bz - az * by,
|
||||
aw * by - ax * bz + ay * bw + az * bx,
|
||||
aw * bz + ax * by - ay * bx + az * bw,
|
||||
)
|
||||
|
||||
|
||||
def _qconj(q):
|
||||
"""Conjugate (= inverse for unit quaternion)."""
|
||||
return (q[0], -q[1], -q[2], -q[3])
|
||||
|
||||
|
||||
def _qnorm(q):
|
||||
"""Normalize quaternion."""
|
||||
n = math.sqrt(sum(c * c for c in q))
|
||||
return tuple(c / n for c in q) if n > 1e-15 else q
|
||||
|
||||
|
||||
def _axis_angle_quat(axis, angle_rad):
|
||||
"""Quaternion (w, x, y, z) for rotation about a normalized axis."""
|
||||
ax, ay, az = axis
|
||||
n = math.sqrt(ax * ax + ay * ay + az * az)
|
||||
if n < 1e-15:
|
||||
return (1.0, 0.0, 0.0, 0.0)
|
||||
ax, ay, az = ax / n, ay / n, az / n
|
||||
s = math.sin(angle_rad / 2.0)
|
||||
return (math.cos(angle_rad / 2.0), ax * s, ay * s, az * s)
|
||||
|
||||
|
||||
def _rotation_angle_cpp(q_old, q_new):
|
||||
"""Rotation angle (degrees) matching C++ validateNewPlacements().
|
||||
|
||||
C++ pipeline:
|
||||
Rotation(x, y, z, w) — stores quat as (x, y, z, w)
|
||||
evaluateVector(): _angle = 2 * acos(quat[3]) // quat[3] = w
|
||||
getRawValue(axis, angle) returns _angle
|
||||
|
||||
CRITICAL: C++ uses acos(w), NOT acos(|w|).
|
||||
When w < 0 (opposite hemisphere), acos(w) > pi/2, so angle > pi.
|
||||
For w ≈ -1 (identity rotation in wrong hemisphere): angle ≈ 2*pi = 360 deg.
|
||||
|
||||
Note: the relative rotation quaternion is constructed via
|
||||
relativeRot = newRot * oldRot.inverse()
|
||||
which goes through Rotation::operator*() and normalize()+evaluateVector().
|
||||
"""
|
||||
q_rel = _qmul(q_new, _qconj(q_old))
|
||||
q_rel = _qnorm(q_rel)
|
||||
|
||||
# q_rel is in (w, x, y, z) order. FreeCAD stores (x, y, z, w), so
|
||||
# when it constructs a Rotation from (q0=x, q1=y, q2=z, q3=w),
|
||||
# evaluateVector() reads quat[3] = w.
|
||||
w = q_rel[0]
|
||||
w = max(-1.0, min(1.0, w))
|
||||
|
||||
# C++ evaluateVector: checks (quat[3] > -1.0) && (quat[3] < 1.0)
|
||||
# Exact ±1 hits else-branch → angle = 0. In practice the multiply
|
||||
# + normalize produces values like -0.99999... which still enters
|
||||
# the acos branch. We replicate C++ exactly here.
|
||||
if w > -1.0 and w < 1.0:
|
||||
angle_rad = math.acos(w) * 2.0
|
||||
else:
|
||||
angle_rad = 0.0
|
||||
return math.degrees(angle_rad)
|
||||
|
||||
|
||||
def _rotation_angle_abs(q_old, q_new):
|
||||
"""Angle using |w| — what a CORRECT validator would use."""
|
||||
q_rel = _qmul(q_new, _qconj(q_old))
|
||||
q_rel = _qnorm(q_rel)
|
||||
w = abs(q_rel[0])
|
||||
w = min(1.0, w)
|
||||
return math.degrees(2.0 * math.acos(w))
|
||||
|
||||
|
||||
# ── Context builders ─────────────────────────────────────────────────
|
||||
|
||||
|
||||
def _build_ctx(
|
||||
ground_pos,
|
||||
ground_quat,
|
||||
bar_pos,
|
||||
bar_quat,
|
||||
cyl_marker_i_quat,
|
||||
cyl_marker_j_quat,
|
||||
cyl_marker_i_pos=(0, 0, 0),
|
||||
cyl_marker_j_pos=(0, 0, 0),
|
||||
planar_marker_i_quat=None,
|
||||
planar_marker_j_quat=None,
|
||||
planar_marker_i_pos=(0, 0, 0),
|
||||
planar_marker_j_pos=(0, 0, 0),
|
||||
planar_offset=0.0,
|
||||
):
|
||||
"""Build SolveContext with ground + bar, Cylindrical + Planar joints.
|
||||
|
||||
Uses explicit marker quaternions instead of identity, so the
|
||||
constraint geometry matches realistic assemblies.
|
||||
"""
|
||||
if planar_marker_i_quat is None:
|
||||
planar_marker_i_quat = cyl_marker_i_quat
|
||||
if planar_marker_j_quat is None:
|
||||
planar_marker_j_quat = cyl_marker_j_quat
|
||||
|
||||
ground = kcsolve.Part()
|
||||
ground.id = "ground"
|
||||
ground.placement = kcsolve.Transform()
|
||||
ground.placement.position = list(ground_pos)
|
||||
ground.placement.quaternion = list(ground_quat)
|
||||
ground.grounded = True
|
||||
|
||||
bar = kcsolve.Part()
|
||||
bar.id = "bar"
|
||||
bar.placement = kcsolve.Transform()
|
||||
bar.placement.position = list(bar_pos)
|
||||
bar.placement.quaternion = list(bar_quat)
|
||||
bar.grounded = False
|
||||
|
||||
cyl = kcsolve.Constraint()
|
||||
cyl.id = "cylindrical"
|
||||
cyl.part_i = "ground"
|
||||
cyl.marker_i = kcsolve.Transform()
|
||||
cyl.marker_i.position = list(cyl_marker_i_pos)
|
||||
cyl.marker_i.quaternion = list(cyl_marker_i_quat)
|
||||
cyl.part_j = "bar"
|
||||
cyl.marker_j = kcsolve.Transform()
|
||||
cyl.marker_j.position = list(cyl_marker_j_pos)
|
||||
cyl.marker_j.quaternion = list(cyl_marker_j_quat)
|
||||
cyl.type = kcsolve.BaseJointKind.Cylindrical
|
||||
|
||||
planar = kcsolve.Constraint()
|
||||
planar.id = "planar"
|
||||
planar.part_i = "ground"
|
||||
planar.marker_i = kcsolve.Transform()
|
||||
planar.marker_i.position = list(planar_marker_i_pos)
|
||||
planar.marker_i.quaternion = list(planar_marker_i_quat)
|
||||
planar.part_j = "bar"
|
||||
planar.marker_j = kcsolve.Transform()
|
||||
planar.marker_j.position = list(planar_marker_j_pos)
|
||||
planar.marker_j.quaternion = list(planar_marker_j_quat)
|
||||
planar.type = kcsolve.BaseJointKind.Planar
|
||||
planar.params = [planar_offset]
|
||||
|
||||
ctx = kcsolve.SolveContext()
|
||||
ctx.parts = [ground, bar]
|
||||
ctx.constraints = [cyl, planar]
|
||||
return ctx
|
||||
|
||||
|
||||
# ── Test 1: Validator function correctness ───────────────────────────
|
||||
|
||||
|
||||
def test_validator_function():
|
||||
"""Verify our _rotation_angle_cpp matches C++ behavior for hemisphere flips."""
|
||||
print("\n--- Test 1: Validator function correctness ---")
|
||||
|
||||
# Same quaternion → 0 degrees
|
||||
q = (1.0, 0.0, 0.0, 0.0)
|
||||
angle = _rotation_angle_cpp(q, q)
|
||||
_report("same quat → 0 deg", abs(angle) < 0.1, f"{angle:.1f}")
|
||||
|
||||
# Near-negated quaternion (tiny perturbation from exact -1 to avoid
|
||||
# the C++ boundary condition where |w| == 1 → angle = 0).
|
||||
# In practice the solver never returns EXACTLY -1; it returns
|
||||
# -0.999999... which enters the acos() branch and gives ~360 deg.
|
||||
q_near_neg = _qnorm((-0.99999, 0.00001, 0.0, 0.0))
|
||||
angle = _rotation_angle_cpp(q, q_near_neg)
|
||||
_report("near-negated quat → ~360 deg", angle > 350.0, f"{angle:.1f}")
|
||||
|
||||
# Same test with |w| correction → should be ~0
|
||||
angle_abs = _rotation_angle_abs(q, q_near_neg)
|
||||
_report("|w|-corrected → ~0 deg", angle_abs < 1.0, f"{angle_abs:.1f}")
|
||||
|
||||
# Non-trivial quaternion vs near-negated version (realistic float noise)
|
||||
q2 = _qnorm((-0.5181, -0.5181, 0.4812, -0.4812))
|
||||
# Simulate what happens: solver returns same rotation in opposite hemisphere
|
||||
q2_neg = _qnorm(tuple(-c + 1e-8 for c in q2))
|
||||
angle = _rotation_angle_cpp(q2, q2_neg)
|
||||
_report("real quat near-negated → >180 deg", angle > 180.0, f"{angle:.1f}")
|
||||
|
||||
# 10-degree rotation — should be fine
|
||||
q_small = _axis_angle_quat((0, 0, 1), math.radians(10))
|
||||
angle = _rotation_angle_cpp(q, q_small)
|
||||
_report("10 deg rotation → ~10 deg", abs(angle - 10.0) < 1.0, f"{angle:.1f}")
|
||||
|
||||
|
||||
# ── Test 2: Synthetic drag with realistic geometry ───────────────────
|
||||
|
||||
|
||||
def test_drag_realistic():
|
||||
"""Drag with non-identity markers and non-trivial bar orientation.
|
||||
|
||||
This reproduces the real assembly geometry:
|
||||
- Cylindrical axis is along a diagonal (not global Z)
|
||||
- Bar starts at a complex orientation far from identity
|
||||
- Drag includes axial perturbation (rotation about constraint axis)
|
||||
|
||||
The solver must re-converge the bar's orientation on each step.
|
||||
If it lands on the -q hemisphere, the C++ validator rejects.
|
||||
"""
|
||||
print("\n--- Test 2: Realistic drag with non-identity geometry ---")
|
||||
solver = kcsolve.load("kindred")
|
||||
|
||||
# Marker quaternion: rotates local Z to point along (1,1,0)/sqrt(2)
|
||||
# This means the cylindrical axis is diagonal in the XY plane
|
||||
marker_q = _qnorm(_axis_angle_quat((0, 1, 0), math.radians(45)))
|
||||
|
||||
# Bar starts at a complex orientation (from real assembly data)
|
||||
# This is close to the actual q=(-0.5181, -0.5181, 0.4812, -0.4812)
|
||||
bar_quat_init = _qnorm((-0.5181, -0.5181, 0.4812, -0.4812))
|
||||
|
||||
# Ground at a non-trivial orientation too (real assembly had q=(0.707,0,0,0.707))
|
||||
ground_quat = _qnorm((0.7071, 0.0, 0.0, 0.7071))
|
||||
|
||||
# Positions far from origin (like real assembly)
|
||||
ground_pos = (100.0, 0.0, 0.0)
|
||||
bar_pos = (500.0, -500.0, 0.0)
|
||||
|
||||
ctx = _build_ctx(
|
||||
ground_pos=ground_pos,
|
||||
ground_quat=ground_quat,
|
||||
bar_pos=bar_pos,
|
||||
bar_quat=bar_quat_init,
|
||||
cyl_marker_i_quat=marker_q,
|
||||
cyl_marker_j_quat=marker_q,
|
||||
# Planar uses identity markers (XY plane constraint)
|
||||
planar_marker_i_quat=(1, 0, 0, 0),
|
||||
planar_marker_j_quat=(1, 0, 0, 0),
|
||||
)
|
||||
|
||||
# ── Save baseline (simulates savePlacementsForUndo) ──
|
||||
baseline_quat = bar_quat_init
|
||||
|
||||
# ── pre_drag ──
|
||||
drag_result = solver.pre_drag(ctx, ["bar"])
|
||||
_report(
|
||||
"drag: pre_drag converged",
|
||||
drag_result.status == kcsolve.SolveStatus.Success,
|
||||
f"status={drag_result.status}",
|
||||
)
|
||||
|
||||
# Check pre_drag result against baseline
|
||||
for pr in drag_result.placements:
|
||||
if pr.id == "bar":
|
||||
solved_quat = tuple(pr.placement.quaternion)
|
||||
angle_cpp = _rotation_angle_cpp(baseline_quat, solved_quat)
|
||||
angle_abs = _rotation_angle_abs(baseline_quat, solved_quat)
|
||||
ok = angle_cpp <= 91.0
|
||||
_report(
|
||||
"drag: pre_drag passes validator",
|
||||
ok,
|
||||
f"C++ angle={angle_cpp:.1f}, |w| angle={angle_abs:.1f}, "
|
||||
f"q=({solved_quat[0]:+.4f},{solved_quat[1]:+.4f},"
|
||||
f"{solved_quat[2]:+.4f},{solved_quat[3]:+.4f})",
|
||||
)
|
||||
if ok:
|
||||
baseline_quat = solved_quat
|
||||
|
||||
# ── drag steps with axial perturbation ──
|
||||
n_steps = 40
|
||||
accepted = 0
|
||||
rejected = 0
|
||||
first_reject_step = None
|
||||
|
||||
for step in range(1, n_steps + 1):
|
||||
# Drag the bar along the cylindrical axis with ROTATION perturbation
|
||||
# Each step: translate along the axis + rotate about it
|
||||
t = step / n_steps
|
||||
angle_about_axis = math.radians(step * 15.0) # 15 deg/step, goes past 360
|
||||
|
||||
# The cylindrical axis direction (marker Z in ground frame)
|
||||
# For our 45-deg-rotated marker: axis ≈ (sin45, 0, cos45) in ground-local
|
||||
# But ground is also rotated. Let's just move along a diagonal.
|
||||
slide = step * 5.0
|
||||
drag_pos = [
|
||||
bar_pos[0] + slide * 0.707,
|
||||
bar_pos[1] + slide * 0.707,
|
||||
bar_pos[2],
|
||||
]
|
||||
|
||||
# Build the dragged orientation: start from bar_quat_init,
|
||||
# apply rotation about the constraint axis
|
||||
axis_rot = _axis_angle_quat((0.707, 0.707, 0), angle_about_axis)
|
||||
drag_quat = list(_qnorm(_qmul(axis_rot, bar_quat_init)))
|
||||
|
||||
pr = kcsolve.SolveResult.PartResult()
|
||||
pr.id = "bar"
|
||||
pr.placement = kcsolve.Transform()
|
||||
pr.placement.position = drag_pos
|
||||
pr.placement.quaternion = drag_quat
|
||||
|
||||
result = solver.drag_step([pr])
|
||||
converged = result.status == kcsolve.SolveStatus.Success
|
||||
|
||||
bar_quat_out = None
|
||||
for rpr in result.placements:
|
||||
if rpr.id == "bar":
|
||||
bar_quat_out = tuple(rpr.placement.quaternion)
|
||||
break
|
||||
|
||||
if bar_quat_out is None:
|
||||
_report(f"step {step:2d}", False, "bar not in result")
|
||||
rejected += 1
|
||||
continue
|
||||
|
||||
# ── Simulate validateNewPlacements() ──
|
||||
angle_cpp = _rotation_angle_cpp(baseline_quat, bar_quat_out)
|
||||
angle_abs = _rotation_angle_abs(baseline_quat, bar_quat_out)
|
||||
validator_ok = angle_cpp <= 91.0
|
||||
|
||||
if validator_ok:
|
||||
baseline_quat = bar_quat_out
|
||||
accepted += 1
|
||||
else:
|
||||
rejected += 1
|
||||
if first_reject_step is None:
|
||||
first_reject_step = step
|
||||
|
||||
_report(
|
||||
f"step {step:2d} ({step * 15:3d} deg)",
|
||||
validator_ok and converged,
|
||||
f"C++={angle_cpp:.1f} |w|={angle_abs:.1f} "
|
||||
f"{'ACCEPT' if validator_ok else 'REJECT'} "
|
||||
f"q=({bar_quat_out[0]:+.4f},{bar_quat_out[1]:+.4f},"
|
||||
f"{bar_quat_out[2]:+.4f},{bar_quat_out[3]:+.4f})",
|
||||
)
|
||||
|
||||
solver.post_drag()
|
||||
|
||||
print(f"\n Summary: accepted={accepted}/{n_steps}, rejected={rejected}/{n_steps}")
|
||||
_report(
|
||||
"drag: all steps accepted by C++ validator",
|
||||
rejected == 0,
|
||||
f"{rejected} rejected"
|
||||
+ (f", first at step {first_reject_step}" if first_reject_step else ""),
|
||||
)
|
||||
|
||||
|
||||
# ── Test 3: Drag with NEGATED initial bar quaternion ─────────────────
|
||||
|
||||
|
||||
def test_drag_negated_init():
|
||||
"""Start the bar at -q (same rotation, opposite hemisphere from solver
|
||||
convention) to maximize the chance of hemisphere mismatch.
|
||||
|
||||
The C++ side saves the FreeCAD object's current Placement.Rotation
|
||||
as the baseline. If FreeCAD stores q but the solver internally
|
||||
prefers -q, the very first solve output can differ in hemisphere.
|
||||
"""
|
||||
print("\n--- Test 3: Drag with negated initial quaternion ---")
|
||||
solver = kcsolve.load("kindred")
|
||||
|
||||
# A non-trivial orientation with w < 0
|
||||
# This is a valid unit quaternion representing a real rotation
|
||||
bar_quat_neg = _qnorm((-0.5, -0.5, 0.5, -0.5)) # w < 0
|
||||
|
||||
# The same rotation in the positive hemisphere
|
||||
bar_quat_pos = tuple(-c for c in bar_quat_neg) # w > 0
|
||||
|
||||
# Identity markers (simplify to isolate the hemisphere issue)
|
||||
ident = (1.0, 0.0, 0.0, 0.0)
|
||||
|
||||
ctx = _build_ctx(
|
||||
ground_pos=(0, 0, 0),
|
||||
ground_quat=ident,
|
||||
bar_pos=(10, 0, 0),
|
||||
bar_quat=bar_quat_neg, # Start in NEGATIVE hemisphere
|
||||
cyl_marker_i_quat=ident,
|
||||
cyl_marker_j_quat=ident,
|
||||
)
|
||||
|
||||
# C++ baseline is saved BEFORE pre_drag — so it uses the w<0 form
|
||||
baseline_quat = bar_quat_neg
|
||||
|
||||
# pre_drag: solver may normalize to positive hemisphere internally
|
||||
drag_result = solver.pre_drag(ctx, ["bar"])
|
||||
_report(
|
||||
"negated: pre_drag converged",
|
||||
drag_result.status == kcsolve.SolveStatus.Success,
|
||||
)
|
||||
|
||||
for pr in drag_result.placements:
|
||||
if pr.id == "bar":
|
||||
solved = tuple(pr.placement.quaternion)
|
||||
# Did the solver flip to positive hemisphere?
|
||||
dot = sum(a * b for a, b in zip(baseline_quat, solved))
|
||||
|
||||
angle_cpp = _rotation_angle_cpp(baseline_quat, solved)
|
||||
hemisphere_match = dot >= 0
|
||||
|
||||
_report(
|
||||
"negated: pre_drag hemisphere match",
|
||||
hemisphere_match,
|
||||
f"dot={dot:+.4f}, C++ angle={angle_cpp:.1f} deg, "
|
||||
f"baseline w={baseline_quat[0]:+.4f}, "
|
||||
f"solved w={solved[0]:+.4f}",
|
||||
)
|
||||
|
||||
validator_ok = angle_cpp <= 91.0
|
||||
_report(
|
||||
"negated: pre_drag passes C++ validator",
|
||||
validator_ok,
|
||||
f"angle={angle_cpp:.1f} deg (threshold=91)",
|
||||
)
|
||||
|
||||
if validator_ok:
|
||||
baseline_quat = solved
|
||||
|
||||
# Drag steps with small perturbation
|
||||
n_steps = 20
|
||||
accepted = 0
|
||||
rejected = 0
|
||||
first_reject = None
|
||||
|
||||
for step in range(1, n_steps + 1):
|
||||
angle_rad = math.radians(step * 18.0)
|
||||
R = 10.0
|
||||
drag_pos = [R * math.cos(angle_rad), R * math.sin(angle_rad), 0.0]
|
||||
|
||||
# Apply the drag rotation in the NEGATIVE hemisphere to match
|
||||
# how FreeCAD would track the mouse-projected placement
|
||||
z_rot = _axis_angle_quat((0, 0, 1), angle_rad)
|
||||
drag_quat = list(_qnorm(_qmul(z_rot, bar_quat_neg)))
|
||||
|
||||
pr = kcsolve.SolveResult.PartResult()
|
||||
pr.id = "bar"
|
||||
pr.placement = kcsolve.Transform()
|
||||
pr.placement.position = drag_pos
|
||||
pr.placement.quaternion = drag_quat
|
||||
|
||||
result = solver.drag_step([pr])
|
||||
|
||||
for rpr in result.placements:
|
||||
if rpr.id == "bar":
|
||||
out_q = tuple(rpr.placement.quaternion)
|
||||
angle_cpp = _rotation_angle_cpp(baseline_quat, out_q)
|
||||
ok = angle_cpp <= 91.0
|
||||
if ok:
|
||||
baseline_quat = out_q
|
||||
accepted += 1
|
||||
else:
|
||||
rejected += 1
|
||||
if first_reject is None:
|
||||
first_reject = step
|
||||
_report(
|
||||
f"neg step {step:2d} ({step * 18:3d} deg)",
|
||||
ok,
|
||||
f"C++={angle_cpp:.1f} "
|
||||
f"q=({out_q[0]:+.4f},{out_q[1]:+.4f},"
|
||||
f"{out_q[2]:+.4f},{out_q[3]:+.4f})",
|
||||
)
|
||||
break
|
||||
|
||||
solver.post_drag()
|
||||
print(f"\n Summary: accepted={accepted}/{n_steps}, rejected={rejected}/{n_steps}")
|
||||
_report(
|
||||
"negated: all steps accepted",
|
||||
rejected == 0,
|
||||
f"{rejected} rejected"
|
||||
+ (f", first at step {first_reject}" if first_reject else ""),
|
||||
)
|
||||
|
||||
|
||||
# ── Test 4: Live assembly if available ───────────────────────────────
|
||||
|
||||
|
||||
def test_live_assembly():
|
||||
"""If a FreeCAD assembly is open, extract its actual geometry and run
|
||||
the drag simulation with real markers and placements."""
|
||||
print("\n--- Test 4: Live assembly introspection ---")
|
||||
try:
|
||||
import FreeCAD as App
|
||||
except ImportError:
|
||||
_report("live: FreeCAD available", False, "not running inside FreeCAD")
|
||||
return
|
||||
|
||||
doc = App.ActiveDocument
|
||||
if doc is None:
|
||||
_report("live: document open", False, "no active document")
|
||||
return
|
||||
|
||||
asm = None
|
||||
for obj in doc.Objects:
|
||||
if obj.TypeId == "Assembly::AssemblyObject":
|
||||
asm = obj
|
||||
break
|
||||
|
||||
if asm is None:
|
||||
_report("live: assembly found", False, "no Assembly object in document")
|
||||
return
|
||||
|
||||
_report("live: assembly found", True, f"'{asm.Name}'")
|
||||
|
||||
# Introspect parts
|
||||
parts = []
|
||||
joints = []
|
||||
grounded = []
|
||||
for obj in asm.Group:
|
||||
if hasattr(obj, "TypeId"):
|
||||
if obj.TypeId == "Assembly::JointGroup":
|
||||
for jobj in obj.Group:
|
||||
if hasattr(jobj, "Proxy"):
|
||||
joints.append(jobj)
|
||||
elif hasattr(obj, "Placement"):
|
||||
parts.append(obj)
|
||||
|
||||
for jobj in joints:
|
||||
proxy = getattr(jobj, "Proxy", None)
|
||||
if proxy and type(proxy).__name__ == "GroundedJoint":
|
||||
ref = getattr(jobj, "ObjectToGround", None)
|
||||
if ref:
|
||||
grounded.append(ref.Name)
|
||||
|
||||
print(f" Parts: {len(parts)}, Joints: {len(joints)}, Grounded: {grounded}")
|
||||
|
||||
# Print each part's placement
|
||||
for p in parts:
|
||||
plc = p.Placement
|
||||
rot = plc.Rotation
|
||||
q = rot.Q # FreeCAD (x, y, z, w)
|
||||
q_wxyz = (q[3], q[0], q[1], q[2])
|
||||
pos = plc.Base
|
||||
is_gnd = p.Name in grounded
|
||||
print(
|
||||
f" {p.Label:40s} pos=({pos.x:.1f}, {pos.y:.1f}, {pos.z:.1f}) "
|
||||
f"q(wxyz)=({q_wxyz[0]:.4f}, {q_wxyz[1]:.4f}, "
|
||||
f"{q_wxyz[2]:.4f}, {q_wxyz[3]:.4f}) "
|
||||
f"{'[GROUNDED]' if is_gnd else ''}"
|
||||
)
|
||||
|
||||
# Print joint details
|
||||
for jobj in joints:
|
||||
proxy = getattr(jobj, "Proxy", None)
|
||||
ptype = type(proxy).__name__ if proxy else "unknown"
|
||||
kind = getattr(jobj, "JointType", "?")
|
||||
print(f" Joint: {jobj.Label} type={ptype} kind={kind}")
|
||||
|
||||
# Check: does any non-grounded part have w < 0 in its current quaternion?
|
||||
# That alone would cause the validator to reject on the first solve.
|
||||
for p in parts:
|
||||
if p.Name in grounded:
|
||||
continue
|
||||
q = p.Placement.Rotation.Q # (x, y, z, w)
|
||||
w = q[3]
|
||||
if w < 0:
|
||||
print(
|
||||
f"\n ** {p.Label} has w={w:.4f} < 0 in current placement! **"
|
||||
f"\n If the solver returns w>0, the C++ validator sees ~360 deg flip."
|
||||
)
|
||||
|
||||
_report("live: assembly introspected", True)
|
||||
|
||||
|
||||
# ── Test 5: Direct hemisphere flip reproduction ──────────────────────
|
||||
|
||||
|
||||
def test_hemisphere_flip_direct():
|
||||
"""Directly reproduce the hemisphere flip by feeding the solver
|
||||
a dragged placement where the quaternion is in the opposite
|
||||
hemisphere from what pre_drag returned.
|
||||
|
||||
This simulates what happens when:
|
||||
1. FreeCAD stores Placement with q = (w<0, x, y, z) form
|
||||
2. Solver normalizes to w>0 during pre_drag
|
||||
3. Next drag_step gets mouse placement in the w<0 form
|
||||
4. Solver output may flip back to w<0
|
||||
"""
|
||||
print("\n--- Test 5: Direct hemisphere flip ---")
|
||||
solver = kcsolve.load("kindred")
|
||||
|
||||
# Use a quaternion representing 90-deg rotation about Z
|
||||
# In positive hemisphere: (cos45, 0, 0, sin45) = (0.707, 0, 0, 0.707)
|
||||
# In negative hemisphere: (-0.707, 0, 0, -0.707)
|
||||
q_pos = _axis_angle_quat((0, 0, 1), math.radians(90))
|
||||
q_neg = tuple(-c for c in q_pos)
|
||||
|
||||
ident = (1.0, 0.0, 0.0, 0.0)
|
||||
|
||||
# Build context with positive-hemisphere quaternion
|
||||
ctx = _build_ctx(
|
||||
ground_pos=(0, 0, 0),
|
||||
ground_quat=ident,
|
||||
bar_pos=(10, 0, 0),
|
||||
bar_quat=q_pos,
|
||||
cyl_marker_i_quat=ident,
|
||||
cyl_marker_j_quat=ident,
|
||||
)
|
||||
|
||||
# C++ baseline saves q_pos
|
||||
baseline_quat = q_pos
|
||||
|
||||
result = solver.pre_drag(ctx, ["bar"])
|
||||
_report("flip: pre_drag converged", result.status == kcsolve.SolveStatus.Success)
|
||||
|
||||
for pr in result.placements:
|
||||
if pr.id == "bar":
|
||||
baseline_quat = tuple(pr.placement.quaternion)
|
||||
print(
|
||||
f" pre_drag baseline: ({baseline_quat[0]:+.4f},"
|
||||
f"{baseline_quat[1]:+.4f},{baseline_quat[2]:+.4f},"
|
||||
f"{baseline_quat[3]:+.4f})"
|
||||
)
|
||||
|
||||
# Now feed drag steps where we alternate hemispheres in the dragged
|
||||
# placement to see if the solver output flips
|
||||
test_drags = [
|
||||
("same hemisphere", q_pos),
|
||||
("opposite hemisphere", q_neg),
|
||||
("back to same", q_pos),
|
||||
("opposite again", q_neg),
|
||||
("large rotation pos", _axis_angle_quat((0, 0, 1), math.radians(170))),
|
||||
(
|
||||
"large rotation neg",
|
||||
tuple(-c for c in _axis_angle_quat((0, 0, 1), math.radians(170))),
|
||||
),
|
||||
]
|
||||
|
||||
for name, drag_q in test_drags:
|
||||
pr = kcsolve.SolveResult.PartResult()
|
||||
pr.id = "bar"
|
||||
pr.placement = kcsolve.Transform()
|
||||
pr.placement.position = [10.0, 0.0, 0.0]
|
||||
pr.placement.quaternion = list(drag_q)
|
||||
|
||||
result = solver.drag_step([pr])
|
||||
|
||||
for rpr in result.placements:
|
||||
if rpr.id == "bar":
|
||||
out_q = tuple(rpr.placement.quaternion)
|
||||
angle_cpp = _rotation_angle_cpp(baseline_quat, out_q)
|
||||
angle_abs = _rotation_angle_abs(baseline_quat, out_q)
|
||||
ok = angle_cpp <= 91.0
|
||||
|
||||
_report(
|
||||
f"flip: {name}",
|
||||
ok,
|
||||
f"C++={angle_cpp:.1f} |w|={angle_abs:.1f} "
|
||||
f"in_w={drag_q[0]:+.4f} out_w={out_q[0]:+.4f}",
|
||||
)
|
||||
if ok:
|
||||
baseline_quat = out_q
|
||||
break
|
||||
|
||||
solver.post_drag()
|
||||
|
||||
|
||||
# ── Run all ──────────────────────────────────────────────────────────
|
||||
|
||||
|
||||
def run_all():
|
||||
print("\n" + "=" * 70)
|
||||
print(" Console Test: Planar + Cylindrical Drag (#338 / #339)")
|
||||
print(" Realistic geometry + C++ validator simulation")
|
||||
print("=" * 70)
|
||||
|
||||
test_validator_function()
|
||||
test_drag_realistic()
|
||||
test_drag_negated_init()
|
||||
test_live_assembly()
|
||||
test_hemisphere_flip_direct()
|
||||
|
||||
# Summary
|
||||
passed = sum(1 for _, p in _results if p)
|
||||
total = len(_results)
|
||||
print(f"\n{'=' * 70}")
|
||||
print(f" {passed}/{total} passed")
|
||||
if passed < total:
|
||||
failed = [n for n, p in _results if not p]
|
||||
print(f" FAILED ({len(failed)}):")
|
||||
for f in failed:
|
||||
print(f" - {f}")
|
||||
print("=" * 70 + "\n")
|
||||
|
||||
|
||||
run_all()
|
||||
Reference in New Issue
Block a user