diff --git a/crates/engine/src/cmd/joints.rs b/crates/engine/src/cmd/joints.rs index 8d9a09fd6b..7bdd392963 100644 --- a/crates/engine/src/cmd/joints.rs +++ b/crates/engine/src/cmd/joints.rs @@ -767,7 +767,10 @@ fn add_joint(s: &mut Session, mut j: Joint, p: &Value) -> Result { // A new joint starts shown, even if a stale visibility entry came from an older design. s.visibility.hidden_joints.retain(|h| *h != id); s.doc_mut().assembly.joints.push(j); - let sol = resolve(s); + let mut sol = resolve(s); + if sol.conflicts.iter().any(|(c, ..)| *c == id) && close_by_half_turn(s, id, &sol) { + sol = resolve(s); + } let mut out = sol_json(&sol, p); out["joint"] = json!(id); out["name"] = json!(name); @@ -777,6 +780,95 @@ fn add_joint(s: &mut Session, mut j: Joint, p: &Value) -> Result { Ok(out) } +/// Sets of angle values a new loop-closing joint tries a half turn round, at most. +const HALF_TURN_TRIALS: usize = 16; + +/// A new joint that closes a loop is met from where the loop's parts sit. Where its parts only +/// meet with one of them turned a half turn about a joint on the way between them (a second +/// planar joint mating two faces that point the same way, #10331), the loop solve starts at a +/// pose turned exactly away and sees no way round, and the joint is left as a conflict. As the +/// first joint of a chain moves its part into place, turn the new joint's own angle, each angle +/// on the shortest joint path between its two parts, or one of each, a half turn, and keep the +/// first that meets it with no new conflict or error elsewhere than `before` had (stored, for +/// the caller's resolve). False, and nothing changed, when none does. Values a motion link or an +/// expression sets, and turns their limits stop short, are left alone. +fn close_by_half_turn(s: &mut Session, id: u64, before: &joints::Solution) -> bool { + let asm = &s.doc.assembly; + let Some(new) = asm.joints.iter().find(|j| j.id == id) else { return false }; + let (from, to) = (new.a.occurrence, new.b.occurrence); + // Breadth first over the other joints, from A's occurrence to B's. + let mut reached: BTreeMap> = BTreeMap::from([(from, None)]); + let mut frontier = vec![from]; + while !frontier.is_empty() && !reached.contains_key(&to) { + let mut next = Vec::new(); + for at in frontier { + for (k, j) in asm.joints.iter().enumerate().filter(|(_, j)| j.id != id && !j.suppressed) { + let other = if j.a.occurrence == at { + j.b.occurrence + } else if j.b.occurrence == at { + j.a.occurrence + } else { + continue; + }; + if let std::collections::btree_map::Entry::Vacant(e) = reached.entry(other) { + e.insert(Some(k)); + next.push(other); + } + } + } + frontier = next; + } + let mut path = Vec::new(); + let mut at = to; + while let Some(Some(k)) = reached.get(&at).copied() { + let Some(j) = asm.joints.get(k) else { break }; + path.push(k); + at = if j.a.occurrence == at { j.b.occurrence } else { j.a.occurrence }; + } + let linked: BTreeSet<(u64, usize)> = asm.links.iter().map(|l| (l.b, l.ib)).collect(); + // The angle values that may turn: on the path, and the new joint's own (turned exactly + // away about its axis, its own value sees no way round either). + let angles = |ks: &[usize]| -> Vec<(usize, usize)> { + ks.iter() + .filter_map(|k| asm.joints.get(*k).map(|j| (*k, j))) + .flat_map(|(k, j)| (0..j.kind.dofs().len()).filter(move |i| j.kind.is_angle(*i)).map(move |i| (k, j, i))) + .filter(|(_, j, i)| j.exprs.value(*i).is_none() && !linked.contains(&(j.id, *i))) + .map(|(k, _, i)| (k, i)) + .collect() + }; + let own = asm.joints.iter().position(|j| j.id == id).map(|k| angles(&[k])).unwrap_or_default(); + let mut trials: Vec> = own.iter().map(|v| vec![*v]).collect(); + for v in angles(&path) { + trials.push(vec![v]); + trials.extend(own.iter().map(|w| vec![v, *w])); + } + trials.truncate(HALF_TURN_TRIALS); + let st = s.model.state(); + 'trial: for turns in trials { + let mut doc = (*s.doc).clone(); + for (k, i) in turns { + let Some(j) = doc.assembly.joints.get_mut(k) else { continue 'trial }; + j.values.resize(j.kind.dofs().len(), 0.0); + let Some(q) = j.values.get(i).copied() else { continue 'trial }; + let mut turned = j.values.clone(); + if let Some(slot) = turned.get_mut(i) { + *slot = q + std::f64::consts::PI; + } + joints::clamp(j, &mut turned); + if turned.get(i).is_none_or(|v| (v - q - std::f64::consts::PI).abs() > 1e-9) { + continue 'trial; + } + j.values = turned; + } + let sol = joints::solve_holding_with_analysis(&doc, &st, &BTreeMap::new(), joints::Analysis::Placement); + if sol.conflicts.len() < before.conflicts.len() && !sol.conflicts.iter().any(|(c, ..)| *c == id) && sol.errors.len() <= before.errors.len() { + s.doc_mut().assembly.joints = doc.assembly.joints; + return true; + } + } + false +} + fn joint_origin(s: &mut Session, p: &Value) -> Result { let cmd = "joint.origin"; let origin = origin_param(s, Some(p), cmd)?; diff --git a/crates/engine/src/cmd/joints_tests.rs b/crates/engine/src/cmd/joints_tests.rs index 64abf18e20..e4614b1c99 100644 --- a/crates/engine/src/cmd/joints_tests.rs +++ b/crates/engine/src/cmd/joints_tests.rs @@ -680,6 +680,104 @@ fn values_left_out_of_a_drive_follow_the_loop() { assert!(near(place(&s, "Plate", Vec3::ZERO), Vec3::new(10.0, 10.0, 0.0)), "{:?}", place(&s, "Plate", Vec3::ZERO)); } +/// Two boards 400 x 100 x 20 for #10331: yellow at the origin, grounded, and red 300 mm along Y. +fn boards() -> Session { + let mut s = Session::default(); + for (name, corner) in [("Yellow", [0.0, 0.0, 0.0]), ("Red", [0.0, 300.0, 0.0])] { + run(&mut s, "component.activate", json!({"component": "root"})); + run(&mut s, "component.create", json!({"name": name, "grounded": false})); + run(&mut s, "solid.box", json!({"length": 400, "width": 100, "height": 20, "corner": corner})); + } + let y = occ(&s, "Yellow"); + run(&mut s, "occurrence.ground", json!({"occurrence": y, "grounded": true})); + s +} + +/// #10331: a board on a planar joint, mated by a second planar joint at a face that points the +/// same way as the face it meets. It meets it turned a half turn on the first joint, the way the +/// first joint moves its part into place, in either pick order, and still slides along the +/// line the two planes share. Faces that can never meet stay a conflict, and nothing moves. +#[test] +fn a_second_planar_joint_turns_its_part_a_half_turn_to_meet() { + // Each case: the yellow face, the red face (world points once red lies on yellow, at + // x 0..400, y 0..100, z 20..40), and where red's origin ends up. + let cases = [ + ("fronts", [200.0, 0.0, 10.0], [200.0, 0.0, 30.0], Some(Vec3::new(400.0, 300.0, 20.0))), + ("ends", [0.0, 50.0, 10.0], [0.0, 50.0, 30.0], Some(Vec3::new(0.0, 400.0, 20.0))), + ("top against front", [200.0, 0.0, 10.0], [200.0, 50.0, 40.0], None), + ]; + for red_first in [false, true] { + for (case, yellow_face, red_face, expected) in cases { + let mut s = boards(); + let planar = |s: &mut Session, name: &str, yellow: [f64; 3], red: [f64; 3]| { + let (yellow, red) = (json!({"occurrence": "Yellow", "face": yellow}), json!({"occurrence": "Red", "face": red})); + let (a, b) = if red_first { (red, yellow) } else { (yellow, red) }; + run(s, "joint.create", json!({"type": "planar", "a": a, "b": b, "name": name})) + }; + let r = planar(&mut s, "Top", [200.0, 50.0, 20.0], [200.0, 350.0, 0.0]); + assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{case}: {r}"); + let on_top = place(&s, "Red", Vec3::ZERO); + let r = planar(&mut s, "Side", yellow_face, red_face); + let label = format!("{case}, red picked first: {red_first}"); + let Some(expected) = expected else { + assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(1), "{label}: {r}"); + assert!(near(place(&s, "Red", Vec3::ZERO), on_top), "{label}: {:?}", place(&s, "Red", Vec3::ZERO)); + continue; + }; + assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "{label}: {r}"); + assert!(place(&s, "Red", Vec3::ZERO).dist(expected) < 1e-6, "{label}: {:?}", place(&s, "Red", Vec3::ZERO)); + assert_eq!(dof_of(&mut s, "Red"), 1, "{label}"); + let axes = solidcraft_doc::joints::motion_axes(&s.doc, &s.model.state(), occ(&s, "Red")); + assert!(axes.len() == 1 && !axes.iter().any(|a| a.angular), "{label}: {axes:?}"); + } + } +} + +/// #10331, with the board turned on the table first (the report's board was turned 40 degrees): +/// a second planar joint between any side of the yellow board and any side of the red one meets, +/// whichever turn the red board needs on the first joint to get there. +#[test] +fn any_two_sides_of_turned_boards_meet_on_a_second_planar_joint() { + // The sides' centres, the red ones where the red board was made. + let reds = [ + ("red front", Vec3::new(200.0, 300.0, 10.0)), + ("red back", Vec3::new(200.0, 400.0, 10.0)), + ("red left end", Vec3::new(0.0, 350.0, 10.0)), + ("red right end", Vec3::new(400.0, 350.0, 10.0)), + ]; + let yellows = [ + ("yellow front", [200.0, 0.0, 10.0]), + ("yellow back", [200.0, 100.0, 10.0]), + ("yellow left end", [0.0, 50.0, 10.0]), + ("yellow right end", [400.0, 50.0, 10.0]), + ]; + for turn in [40.0f64, 0.0, 137.0] { + for (yn, yp) in yellows { + for (rn, rl) in reds { + let mut s = boards(); + run( + &mut s, + "occurrence.move", + json!({"occurrence": "Red:1", "space": "world", "axis": [0, 0, 1], "angle": format!("{turn} deg"), "origin": [200, 350, 0]}), + ); + let top = place(&s, "Red", Vec3::new(200.0, 350.0, 0.0)); + run( + &mut s, + "joint.create", + json!({"type": "planar", "a": {"occurrence": "Yellow", "face": [200, 50, 20]}, "b": {"occurrence": "Red", "face": [top.x, top.y, top.z]}, "name": "Top"}), + ); + let rp = place(&s, "Red", rl); + let r = run( + &mut s, + "joint.create", + json!({"type": "planar", "a": {"occurrence": "Yellow", "face": yp}, "b": {"occurrence": "Red", "face": [rp.x, rp.y, rp.z]}, "name": "Side"}), + ); + assert_eq!(r["conflicts"].as_array().map(Vec::len), Some(0), "turned {turn} degrees, {yn} against {rn}: {r}"); + } + } + } +} + /// A slider-crank: crank 15 about the origin, rod 40, a piston sliding along X. Returns the crank /// pin and the wrist pin. fn slider_crank() -> (Session, Vec3, Vec3) { diff --git a/crates/ui-egui/src/joint_handles.rs b/crates/ui-egui/src/joint_handles.rs index 8954a0b913..1672810897 100644 --- a/crates/ui-egui/src/joint_handles.rs +++ b/crates/ui-egui/src/joint_handles.rs @@ -1023,8 +1023,9 @@ mod tests { let invalid = crate::prefs::lenient(&json!({"joint_rotation_coarse":0,"joint_rotation_fine":-3})); assert_eq!((invalid.joint_rotation_coarse, invalid.joint_rotation_fine), (10., 0.1)); } - /// Two boards held by two planar joints whose planes cross (#10331): the second joint can't - /// close its loop, so Move names it instead of saying the part has no movable axis. + /// Two boards held by two planar joints whose faces can never meet (the yellow front against + /// the red top, #10331): the second joint can't close its loop, so Move names it instead of + /// saying the part has no movable axis. #[test] fn blocked_move_names_a_conflicting_joint_of_the_part() { let mut h = Harness::new(); @@ -1038,7 +1039,7 @@ mod tests { h.app.run("occurrence.ground", json!({"occurrence":"Yellow:1","grounded":true})).unwrap(); let planar = |name: &str, a: [i32; 3], b: [i32; 3]| json!({"type":"planar","a":{"occurrence":"Yellow","face":a},"b":{"occurrence":"Red","face":b},"name":name}); h.app.run("joint.create", planar("Top", [200, 50, 20], [200, 350, 0])).unwrap(); - h.app.run("joint.create", planar("Front", [200, 0, 10], [200, 0, 30])).unwrap(); + h.app.run("joint.create", planar("Front", [200, 0, 10], [200, 50, 40])).unwrap(); let solved = joints::solve(&h.app.session.doc, &h.app.session.model.state()); let conflicting: Vec<_> = solved.conflicts.iter().map(|(_, name, _)| name.as_str()).collect(); assert!(!conflicting.is_empty(), "the scenario must conflict: {:?}", solved.conflicts);