kindred/solver
1
0
Fork 0
Code Issues 10 Pull Requests 1 Actions Packages Projects Releases Wiki Activity
Files
9e07ef86790a7361582f38528c4337def7907d78
solver/tests/console_test_planar_drag.py
forbes-0023 9e07ef8679 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
2026-02-27 09:30:27 -06:00

721 lines
25 KiB
Python
Raw Blame History

"""
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 View Git Blame Copy Permalink