use super::*;
fn body(id: &str, fixed: bool, rotation: [f64; 4], translation: [f64; 3]) -> AssemblyBody {
AssemblyBody {
id: id.into(),
fixed,
rotation,
translation,
}
}
fn quat_axis_angle(axis: [f64; 3], angle_deg: f64) -> [f64; 4] {
let axis = vec3(axis).normalized().unwrap();
let half = angle_deg.to_radians() * 0.5;
let s = half.sin();
[half.cos(), s * axis.x, s * axis.y, s * axis.z]
}
fn mate(id: &str, body_a: usize, body_b: usize, kind: MateKind) -> AssemblyMate {
AssemblyMate {
id: Some(id.into()),
body_a,
body_b,
kind,
}
}
fn plane(origin: [f64; 3], normal: [f64; 3]) -> MatePlane {
MatePlane { origin, normal }
}
fn axis(origin: [f64; 3], direction: [f64; 3]) -> MateAxis {
MateAxis { origin, direction }
}
fn rot(sol: &AssemblySolution, body: usize, v: [f64; 3]) -> Vec3 {
quat_rotate(sol.poses[body].rotation, vec3(v))
}
fn assert_close(actual: Vec3, expected: [f64; 3], tol: f64, what: &str) {
let d = actual.sub(vec3(expected)).length();
assert!(
d <= tol,
"{what}: expected {:?}, got ({}, {}, {}) (delta {d:.3e})",
expected,
actual.x,
actual.y,
actual.z
);
}
fn stacked_boxes_mates(a: usize, b: usize) -> Vec<AssemblyMate> {
vec![
mate(
"top-bottom",
a,
b,
MateKind::CoincidentPlanePlane {
plane_a: plane([0.0, 0.0, 1.0], [0.0, 0.0, 1.0]),
plane_b: plane([0.0, 0.0, 0.0], [0.0, 0.0, -1.0]),
align: MateAlign::AntiAligned,
},
),
mate(
"left-flush",
a,
b,
MateKind::CoincidentPlanePlane {
plane_a: plane([0.0, 0.0, 0.0], [-1.0, 0.0, 0.0]),
plane_b: plane([0.0, 0.0, 0.0], [-1.0, 0.0, 0.0]),
align: MateAlign::Aligned,
},
),
mate(
"front-flush",
a,
b,
MateKind::CoincidentPlanePlane {
plane_a: plane([0.0, 0.0, 0.0], [0.0, -1.0, 0.0]),
plane_b: plane([0.0, 0.0, 0.0], [0.0, -1.0, 0.0]),
align: MateAlign::Aligned,
},
),
]
}
#[test]
fn boxes_three_plane_coincidents_unique_pose() {
let bodies = vec![
body("base", true, identity_rotation(), [0.0, 0.0, 0.0]),
body(
"lid",
false,
quat_axis_angle([1.0, 1.0, 0.3], 20.0),
[0.3, -0.2, 0.5],
),
];
let mates = stacked_boxes_mates(0, 1);
let sol = solve_assembly(&bodies, &mates, &AssemblySolveOptions::default()).unwrap();
assert_close(
vec3(sol.poses[1].translation),
[0.0, 0.0, 1.0],
1e-9,
"lid translation",
);
assert_close(
rot(&sol, 1, [1.0, 0.0, 0.0]),
[1.0, 0.0, 0.0],
1e-9,
"lid x-axis",
);
assert_close(
rot(&sol, 1, [0.0, 0.0, 1.0]),
[0.0, 0.0, 1.0],
1e-9,
"lid z-axis",
);
assert_eq!(sol.dof, 0, "unique pose leaves no DOF");
assert_eq!(sol.rank, 6);
assert!(
sol.max_residual <= 1e-9,
"max residual {}",
sol.max_residual
);
assert_eq!(sol.mate_residuals.len(), 3);
assert_eq!(sol.redundant, 3);
assert_eq!(sol.status, "over");
}
#[test]
fn shaft_in_hole_concentric_plus_distance() {
let bodies = vec![
body("block", true, identity_rotation(), [0.0, 0.0, 0.0]),
body(
"shaft",
false,
quat_axis_angle([1.0, 0.4, 0.0], 10.0),
[0.2, -0.3, 0.7],
),
];
let mates = vec![
mate(
"bore",
0,
1,
MateKind::ConcentricAxisAxis {
axis_a: axis([0.0, 0.0, 0.0], [0.0, 0.0, 1.0]),
axis_b: axis([0.0, 0.0, 0.0], [0.0, 0.0, 1.0]),
align: MateAlign::Any,
},
),
mate(
"standoff",
0,
1,
MateKind::DistancePlanePlane {
plane_a: plane([0.0, 0.0, 0.0], [0.0, 0.0, 1.0]),
plane_b: plane([0.0, 0.0, 0.0], [0.0, 0.0, -1.0]),
distance: 0.25,
align: MateAlign::AntiAligned,
},
),
];
let sol = solve_assembly(&bodies, &mates, &AssemblySolveOptions::default()).unwrap();
assert_close(
vec3(sol.poses[1].translation),
[0.0, 0.0, 0.25],
1e-9,
"shaft translation",
);
assert_close(
rot(&sol, 1, [0.0, 0.0, 1.0]),
[0.0, 0.0, 1.0],
1e-9,
"shaft axis",
);
assert_eq!(sol.dof, 1, "spin about the bore axis must stay free");
assert_eq!(sol.rank, 5);
assert!(
sol.max_residual <= 1e-9,
"max residual {}",
sol.max_residual
);
}
#[test]
fn angle_mate_45_between_plates() {
let bodies = vec![
body("plate_a", true, identity_rotation(), [0.0, 0.0, 0.0]),
body(
"plate_b",
false,
quat_axis_angle([1.0, 0.0, 0.0], 30.0),
[0.05, -0.02, 0.04],
),
];
let mates = vec![
mate(
"hinge-pin",
0,
1,
MateKind::ConcentricAxisAxis {
axis_a: axis([0.0, 0.0, 0.0], [1.0, 0.0, 0.0]),
axis_b: axis([0.0, 0.0, 0.0], [1.0, 0.0, 0.0]),
align: MateAlign::Aligned,
},
),
mate(
"hinge-end",
0,
1,
MateKind::CoincidentPointPoint {
point_a: [0.0, 0.0, 0.0],
point_b: [0.0, 0.0, 0.0],
},
),
mate(
"open-45",
0,
1,
MateKind::Angle {
direction_a: [0.0, 0.0, 1.0],
direction_b: [0.0, 0.0, 1.0],
angle_deg: 45.0,
},
),
];
let sol = solve_assembly(&bodies, &mates, &AssemblySolveOptions::default()).unwrap();
let half = std::f64::consts::FRAC_1_SQRT_2;
assert_close(
vec3(sol.poses[1].translation),
[0.0, 0.0, 0.0],
1e-9,
"hinge origin",
);
assert_close(
rot(&sol, 1, [1.0, 0.0, 0.0]),
[1.0, 0.0, 0.0],
1e-9,
"hinge axis",
);
assert_close(
rot(&sol, 1, [0.0, 0.0, 1.0]),
[0.0, -half, half],
1e-9,
"plate normal",
);
assert_eq!(sol.dof, 0);
assert!(
sol.max_residual <= 1e-9,
"max residual {}",
sol.max_residual
);
}
#[test]
fn conflicting_distances_err() {
let bodies = vec![
body("base", true, identity_rotation(), [0.0, 0.0, 0.0]),
body("part", false, identity_rotation(), [1.4, 0.0, 0.0]),
];
let mates = vec![
mate(
"d1",
0,
1,
MateKind::DistancePointPoint {
point_a: [0.0, 0.0, 0.0],
point_b: [0.0, 0.0, 0.0],
distance: 1.0,
},
),
mate(
"d2",
0,
1,
MateKind::DistancePointPoint {
point_a: [0.0, 0.0, 0.0],
point_b: [0.0, 0.0, 0.0],
distance: 2.0,
},
),
];
let err = solve_assembly(&bodies, &mates, &AssemblySolveOptions::default()).unwrap_err();
assert!(
err.contains("conflict"),
"error must flag the conflict: {err}"
);
assert!(
err.contains("d1") && err.contains("d2"),
"error must name the conflicting mates: {err}"
);
}
#[test]
fn chain_of_three_bodies_one_shot() {
let bodies = vec![
body("a", true, identity_rotation(), [0.0, 0.0, 0.0]),
body(
"b",
false,
quat_axis_angle([0.2, 1.0, 0.1], 15.0),
[0.2, -0.1, 0.6],
),
body(
"c",
false,
quat_axis_angle([1.0, 0.0, 1.0], -10.0),
[0.1, 0.3, 1.4],
),
];
let mut mates = stacked_boxes_mates(0, 1);
mates.extend(stacked_boxes_mates(1, 2));
let sol = solve_assembly(&bodies, &mates, &AssemblySolveOptions::default()).unwrap();
assert_close(
vec3(sol.poses[1].translation),
[0.0, 0.0, 1.0],
1e-9,
"b translation",
);
assert_close(
vec3(sol.poses[2].translation),
[0.0, 0.0, 2.0],
1e-9,
"c translation",
);
for bi in [1usize, 2] {
assert_close(
rot(&sol, bi, [1.0, 0.0, 0.0]),
[1.0, 0.0, 0.0],
1e-9,
"x-axis",
);
assert_close(
rot(&sol, bi, [0.0, 0.0, 1.0]),
[0.0, 0.0, 1.0],
1e-9,
"z-axis",
);
}
assert_eq!(sol.dof, 0);
assert_eq!(sol.rank, 12);
assert!(
sol.max_residual <= 1e-9,
"max residual {}",
sol.max_residual
);
}
#[test]
fn determinism_bit_identical() {
let bodies = vec![
body("block", true, identity_rotation(), [0.0, 0.0, 0.0]),
body(
"shaft",
false,
quat_axis_angle([1.0, 0.4, 0.0], 10.0),
[0.2, -0.3, 0.7],
),
];
let mates = vec![
mate(
"bore",
0,
1,
MateKind::ConcentricAxisAxis {
axis_a: axis([0.0, 0.0, 0.0], [0.0, 0.0, 1.0]),
axis_b: axis([0.0, 0.0, 0.0], [0.0, 0.0, 1.0]),
align: MateAlign::Any,
},
),
mate(
"standoff",
0,
1,
MateKind::DistancePlanePlane {
plane_a: plane([0.0, 0.0, 0.0], [0.0, 0.0, 1.0]),
plane_b: plane([0.0, 0.0, 0.0], [0.0, 0.0, -1.0]),
distance: 0.25,
align: MateAlign::AntiAligned,
},
),
];
let opts = AssemblySolveOptions::default();
let runs: Vec<AssemblySolution> = (0..3)
.map(|_| solve_assembly(&bodies, &mates, &opts).unwrap())
.collect();
for run in &runs[1..] {
for (pa, pb) in runs[0].poses.iter().zip(&run.poses) {
for c in 0..4 {
assert_eq!(
pa.rotation[c].to_bits(),
pb.rotation[c].to_bits(),
"rotation must be bit-identical"
);
}
for c in 0..3 {
assert_eq!(
pa.translation[c].to_bits(),
pb.translation[c].to_bits(),
"translation must be bit-identical"
);
}
}
assert_eq!(runs[0].max_residual.to_bits(), run.max_residual.to_bits());
assert_eq!(runs[0].iterations, run.iterations);
}
let json0 = serde_json::to_string(&runs[0]).unwrap();
let json1 = serde_json::to_string(&runs[1]).unwrap();
assert_eq!(json0, json1);
}
#[test]
fn tangent_sphere_and_cylinder_on_plane() {
let bodies = vec![
body("table", true, identity_rotation(), [0.0, 0.0, 0.0]),
body("ball", false, identity_rotation(), [0.1, 0.2, 1.3]),
];
let mates = vec![mate(
"rest",
1,
0,
MateKind::TangentSpherePlane {
center_a: [0.0, 0.0, 0.0],
radius: 0.5,
plane_b: plane([0.0, 0.0, 0.0], [0.0, 0.0, 1.0]),
},
)];
let sol = solve_assembly(&bodies, &mates, &AssemblySolveOptions::default()).unwrap();
assert_close(
vec3(sol.poses[1].translation),
[0.1, 0.2, 0.5],
1e-9,
"ball keeps x/y, drops to r above the table",
);
assert_eq!(sol.dof, 5);
assert!(sol.max_residual <= 1e-9);
let bodies = vec![
body("table", true, identity_rotation(), [0.0, 0.0, 0.0]),
body(
"rod",
false,
quat_axis_angle([0.0, 1.0, 0.0], 5.0),
[0.0, 0.0, 1.0],
),
];
let mates = vec![mate(
"lay-flat",
1,
0,
MateKind::TangentCylinderPlane {
axis_a: axis([0.0, 0.0, 0.0], [1.0, 0.0, 0.0]),
radius: 0.3,
plane_b: plane([0.0, 0.0, 0.0], [0.0, 0.0, 1.0]),
},
)];
let sol = solve_assembly(&bodies, &mates, &AssemblySolveOptions::default()).unwrap();
assert!(
(sol.poses[1].translation[2] - 0.3).abs() <= 1e-9,
"rod axis height {}",
sol.poses[1].translation[2]
);
let axis_w = rot(&sol, 1, [1.0, 0.0, 0.0]);
assert!(
axis_w.z.abs() <= 1e-9,
"rod axis parallel to table: z = {}",
axis_w.z
);
assert_eq!(sol.dof, 4);
assert!(sol.max_residual <= 1e-9);
}
#[test]
fn parallel_any_and_perpendicular() {
let bodies = vec![
body("a", true, identity_rotation(), [0.0, 0.0, 0.0]),
body(
"b",
false,
quat_axis_angle([1.0, 0.0, 0.0], 170.0),
[0.0, 0.0, 0.0],
),
];
let mates = vec![mate(
"par",
0,
1,
MateKind::Parallel {
direction_a: [0.0, 0.0, 1.0],
direction_b: [0.0, 0.0, 1.0],
align: MateAlign::Any,
},
)];
let sol = solve_assembly(&bodies, &mates, &AssemblySolveOptions::default()).unwrap();
let d = rot(&sol, 1, [0.0, 0.0, 1.0]).dot(Vec3::new(0.0, 0.0, 1.0));
assert!((d.abs() - 1.0).abs() <= 1e-9, "parallel-any dot {d}");
assert!(d < 0.0, "nearest sense from 170 degrees is anti-parallel");
assert_eq!(sol.dof, 4);
let bodies = vec![
body("a", true, identity_rotation(), [0.0, 0.0, 0.0]),
body(
"b",
false,
quat_axis_angle([1.0, 0.0, 0.0], 80.0),
[0.0, 0.0, 0.0],
),
];
let mates = vec![mate(
"perp",
0,
1,
MateKind::Perpendicular {
direction_a: [0.0, 0.0, 1.0],
direction_b: [0.0, 0.0, 1.0],
},
)];
let sol = solve_assembly(&bodies, &mates, &AssemblySolveOptions::default()).unwrap();
let d = rot(&sol, 1, [0.0, 0.0, 1.0]).dot(Vec3::new(0.0, 0.0, 1.0));
assert!(d.abs() <= 1e-9, "perpendicular dot {d}");
assert_eq!(sol.dof, 5);
}
#[test]
fn validation_errors() {
let bodies = vec![
body("a", true, identity_rotation(), [0.0, 0.0, 0.0]),
body("b", false, identity_rotation(), [0.0, 0.0, 0.0]),
];
let opts = AssemblySolveOptions::default();
let bad_index = vec![mate(
"m",
0,
7,
MateKind::CoincidentPointPoint {
point_a: [0.0; 3],
point_b: [0.0; 3],
},
)];
assert!(solve_assembly(&bodies, &bad_index, &opts)
.unwrap_err()
.contains("out of range"));
let self_mate = vec![mate(
"m",
1,
1,
MateKind::CoincidentPointPoint {
point_a: [0.0; 3],
point_b: [0.0; 3],
},
)];
assert!(solve_assembly(&bodies, &self_mate, &opts)
.unwrap_err()
.contains("must differ"));
let zero_normal = vec![mate(
"m",
0,
1,
MateKind::CoincidentPointPlane {
point_a: [0.0; 3],
plane_b: plane([0.0; 3], [0.0; 3]),
},
)];
assert!(solve_assembly(&bodies, &zero_normal, &opts)
.unwrap_err()
.contains("nonzero"));
let bad_quat = vec![body("a", false, [0.0; 4], [0.0; 3])];
assert!(solve_assembly(&bad_quat, &[], &opts)
.unwrap_err()
.contains("quaternion"));
}
#[test]
fn no_mates_trivial() {
let bodies = vec![
body("a", true, identity_rotation(), [1.0, 2.0, 3.0]),
body(
"b",
false,
quat_axis_angle([0.0, 0.0, 1.0], 30.0),
[4.0, 5.0, 6.0],
),
];
let sol = solve_assembly(&bodies, &[], &AssemblySolveOptions::default()).unwrap();
assert_eq!(sol.poses.len(), 2);
assert_eq!(sol.dof, 6);
assert_eq!(sol.rank, 0);
assert_eq!(sol.status, "under");
assert_eq!(sol.max_residual, 0.0);
assert_close(
vec3(sol.poses[1].translation),
[4.0, 5.0, 6.0],
0.0,
"pose kept",
);
}
#[test]
fn serde_round_trip() {
let mates = vec![
mate(
"bore",
0,
1,
MateKind::ConcentricAxisAxis {
axis_a: axis([0.0, 0.0, 0.0], [0.0, 0.0, 1.0]),
axis_b: axis([0.0, 0.0, 0.0], [0.0, 0.0, 1.0]),
align: MateAlign::Any,
},
),
mate(
"open-45",
0,
1,
MateKind::Angle {
direction_a: [0.0, 0.0, 1.0],
direction_b: [0.0, 0.0, 1.0],
angle_deg: 45.0,
},
),
];
let json = serde_json::to_string(&mates).unwrap();
assert!(json.contains("\"type\":\"concentric_axis_axis\""), "{json}");
assert!(json.contains("\"type\":\"angle\""), "{json}");
let back: Vec<AssemblyMate> = serde_json::from_str(&json).unwrap();
assert_eq!(back.len(), 2);
match &back[0].kind {
MateKind::ConcentricAxisAxis { align, .. } => assert_eq!(*align, MateAlign::Any),
other => panic!("wrong kind after round-trip: {other:?}"),
}
let coincident: AssemblyMate = serde_json::from_str(
r#"{"body_a":0,"body_b":1,"type":"coincident_plane_plane",
"plane_a":{"origin":[0,0,1],"normal":[0,0,1]},
"plane_b":{"origin":[0,0,0],"normal":[0,0,-1]}}"#,
)
.unwrap();
match coincident.kind {
MateKind::CoincidentPlanePlane { align, .. } => {
assert_eq!(align, MateAlign::AntiAligned)
}
other => panic!("wrong kind: {other:?}"),
}
}
fn opts(strategy: SolveStrategy) -> AssemblySolveOptions {
AssemblySolveOptions {
strategy,
..AssemblySolveOptions::default()
}
}
fn chain_opts(strategy: SolveStrategy) -> AssemblySolveOptions {
AssemblySolveOptions {
strategy,
tolerance: 1e-11,
..AssemblySolveOptions::default()
}
}
fn chain_bodies(n: usize) -> (Vec<AssemblyBody>, Vec<AssemblyMate>) {
let mut bodies = vec![body("g", true, identity_rotation(), [0.0; 3])];
for i in 1..=n {
let fi = i as f64;
bodies.push(body(
&format!("b{i}"),
false,
quat_axis_angle([0.2 + 0.07 * fi, 1.0, 0.3 - 0.04 * fi], 10.0 + 2.5 * fi),
[0.15 - 0.02 * fi, 0.1 * ((i % 3) as f64) - 0.1, fi + 0.35],
));
}
let mut mates = Vec::new();
for i in 0..n {
mates.extend(stacked_boxes_mates(i, i + 1));
}
(bodies, mates)
}
fn assert_poses_close(a: &AssemblySolution, b: &AssemblySolution, tol: f64, what: &str) {
assert_eq!(a.poses.len(), b.poses.len());
for i in 0..a.poses.len() {
let dt = vec3(a.poses[i].translation)
.sub(vec3(b.poses[i].translation))
.length();
assert!(dt <= tol, "{what}: body {i} translation delta {dt:.3e}");
for axis in [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]] {
let da = rot(a, i, axis).sub(rot(b, i, axis)).length();
assert!(da <= tol, "{what}: body {i} axis delta {da:.3e}");
}
}
}
fn assert_poses_bit_identical(a: &AssemblySolution, b: &AssemblySolution) {
for (pa, pb) in a.poses.iter().zip(&b.poses) {
for c in 0..4 {
assert_eq!(
pa.rotation[c].to_bits(),
pb.rotation[c].to_bits(),
"rotation must be bit-identical"
);
}
for c in 0..3 {
assert_eq!(
pa.translation[c].to_bits(),
pb.translation[c].to_bits(),
"translation must be bit-identical"
);
}
}
}
#[test]
fn chain_ten_bodies_decomposed_sequential_matches_monolithic() {
let (bodies, mates) = chain_bodies(9);
let mono = solve_assembly(&bodies, &mates, &chain_opts(SolveStrategy::Monolithic)).unwrap();
let deco = solve_assembly(&bodies, &mates, &chain_opts(SolveStrategy::Decomposed)).unwrap();
assert_eq!(mono.strategy, "monolithic");
assert_eq!(deco.strategy, "decomposed");
assert_eq!(mono.steps.len(), 1);
assert_eq!(mono.steps[0].method, "monolithic");
assert_eq!(mono.steps[0].bodies.len(), 9);
assert_eq!(deco.steps.len(), 9);
for (i, step) in deco.steps.iter().enumerate() {
assert_eq!(step.method, "sequential", "step {i}");
assert_eq!(step.bodies, vec![format!("b{}", i + 1)], "step {i}");
assert!(step.iterations > 0, "step {i} did no work");
}
for sol in [&mono, &deco] {
for i in 1..bodies.len() {
assert_close(
vec3(sol.poses[i].translation),
[0.0, 0.0, i as f64],
1e-9,
"chain translation",
);
assert_close(
rot(sol, i, [1.0, 0.0, 0.0]),
[1.0, 0.0, 0.0],
1e-9,
"x-axis",
);
assert_close(
rot(sol, i, [0.0, 0.0, 1.0]),
[0.0, 0.0, 1.0],
1e-9,
"z-axis",
);
}
assert!(sol.max_residual <= 1e-9);
assert_eq!(sol.dof, 0);
}
assert_poses_close(&mono, &deco, 1e-9, "mono vs deco");
let again =
solve_assembly(&bodies, &mates, &chain_opts(SolveStrategy::Decomposed)).unwrap();
assert_poses_bit_identical(&deco, &again);
assert_eq!(deco.iterations, again.iterations);
assert_eq!(deco.residual_rows_evaluated, again.residual_rows_evaluated);
let auto = solve_assembly(&bodies, &mates, &chain_opts(SolveStrategy::Auto)).unwrap();
assert_eq!(auto.strategy, "decomposed");
assert_poses_bit_identical(&deco, &auto);
}
#[test]
fn chain_ten_decomposed_costs_less_than_monolithic() {
let (bodies, mates) = chain_bodies(9);
let mono = solve_assembly(&bodies, &mates, &chain_opts(SolveStrategy::Monolithic)).unwrap();
let deco = solve_assembly(&bodies, &mates, &chain_opts(SolveStrategy::Decomposed)).unwrap();
println!(
"PERF mono: iterations {} rows {} | deco: iterations {} rows {}",
mono.iterations,
mono.residual_rows_evaluated,
deco.iterations,
deco.residual_rows_evaluated
);
assert!(
deco.residual_rows_evaluated < mono.residual_rows_evaluated,
"decomposed work {} must be below monolithic {}",
deco.residual_rows_evaluated,
mono.residual_rows_evaluated
);
}
#[test]
fn four_body_cycle_falls_back_to_monolithic() {
let bodies = vec![
body("g", true, identity_rotation(), [0.0; 3]),
body(
"b1",
false,
quat_axis_angle([1.0, 0.2, 0.0], 8.0),
[0.1, -0.05, 1.1],
),
body(
"b2",
false,
quat_axis_angle([0.0, 1.0, 0.1], -7.0),
[-0.08, 0.12, 2.2],
),
body(
"b3",
false,
quat_axis_angle([0.3, 0.0, 1.0], 6.0),
[0.05, 0.03, 3.1],
),
];
let conc = |id: &str, a: usize, b: usize| {
mate(
id,
a,
b,
MateKind::ConcentricAxisAxis {
axis_a: axis([0.0; 3], [0.0, 0.0, 1.0]),
axis_b: axis([0.0; 3], [0.0, 0.0, 1.0]),
align: MateAlign::Any,
},
)
};
let mates = vec![
conc("g-b1", 0, 1),
conc("b1-b2", 1, 2),
conc("b2-b3", 2, 3),
conc("b3-g", 3, 0),
];
let deco = solve_assembly(&bodies, &mates, &opts(SolveStrategy::Decomposed)).unwrap();
assert_eq!(deco.strategy, "decomposed");
assert_eq!(deco.steps.len(), 1, "one fallback step for the whole cycle");
assert_eq!(deco.steps[0].method, "monolithic_fallback");
assert_eq!(deco.steps[0].bodies, vec!["b1", "b2", "b3"]);
for i in 1..4 {
let axis_w = rot(&deco, i, [0.0, 0.0, 1.0]);
let lateral = axis_w.cross(Vec3::new(0.0, 0.0, 1.0)).length();
assert!(lateral <= 1e-9, "body {i} axis off z by {lateral:.3e}");
let t = vec3(deco.poses[i].translation);
assert!(t.x.abs() <= 1e-9 && t.y.abs() <= 1e-9, "body {i} off-axis");
}
assert!(deco.max_residual <= 1e-9);
let mono = solve_assembly(&bodies, &mates, &opts(SolveStrategy::Monolithic)).unwrap();
assert_poses_bit_identical(&deco, &mono);
assert_eq!(deco.iterations, mono.iterations);
}
#[test]
fn rigid_pair_cluster_detected_and_solved_hierarchically() {
let bodies = vec![
body("g", true, identity_rotation(), [0.0; 3]),
body(
"b1",
false,
quat_axis_angle([1.0, 1.0, 0.2], 14.0),
[0.2, -0.15, 1.2],
),
body(
"b2",
false,
quat_axis_angle([0.1, 1.0, 0.6], -11.0),
[-0.1, 0.2, 2.3],
),
];
let mut mates = stacked_boxes_mates(1, 2);
mates.push(mate(
"g-top",
0,
1,
MateKind::CoincidentPlanePlane {
plane_a: plane([0.0, 0.0, 1.0], [0.0, 0.0, 1.0]),
plane_b: plane([0.0, 0.0, 0.0], [0.0, 0.0, -1.0]),
align: MateAlign::AntiAligned,
},
));
mates.push(mate(
"g-left",
0,
2,
MateKind::CoincidentPlanePlane {
plane_a: plane([0.0, 0.0, 0.0], [-1.0, 0.0, 0.0]),
plane_b: plane([0.0, 0.0, 0.0], [-1.0, 0.0, 0.0]),
align: MateAlign::Aligned,
},
));
mates.push(mate(
"g-front",
0,
2,
MateKind::CoincidentPlanePlane {
plane_a: plane([0.0, 0.0, 0.0], [0.0, -1.0, 0.0]),
plane_b: plane([0.0, 0.0, 0.0], [0.0, -1.0, 0.0]),
align: MateAlign::Aligned,
},
));
let deco = solve_assembly(&bodies, &mates, &opts(SolveStrategy::Decomposed)).unwrap();
assert_eq!(deco.strategy, "decomposed");
assert_eq!(deco.steps.len(), 2, "internal solve then rigid placement");
assert_eq!(deco.steps[0].method, "cluster_internal");
assert_eq!(deco.steps[0].bodies, vec!["b1", "b2"]);
assert_eq!(deco.steps[1].method, "cluster_sequential");
assert_eq!(deco.steps[1].bodies, vec!["b1", "b2"]);
for (i, z) in [(1usize, 1.0f64), (2, 2.0)] {
assert_close(
vec3(deco.poses[i].translation),
[0.0, 0.0, z],
1e-9,
"translation",
);
assert_close(
rot(&deco, i, [1.0, 0.0, 0.0]),
[1.0, 0.0, 0.0],
1e-9,
"x-axis",
);
assert_close(
rot(&deco, i, [0.0, 0.0, 1.0]),
[0.0, 0.0, 1.0],
1e-9,
"z-axis",
);
}
assert_eq!(deco.dof, 0);
assert!(deco.max_residual <= 1e-9);
let mono = solve_assembly(&bodies, &mates, &opts(SolveStrategy::Monolithic)).unwrap();
assert_poses_close(&mono, &deco, 1e-9, "mono vs cluster");
assert_eq!(mono.dof, deco.dof);
assert_eq!(mono.rank, deco.rank);
assert_eq!(mono.redundant, deco.redundant);
let auto = solve_assembly(&bodies, &mates, &AssemblySolveOptions::default()).unwrap();
assert_eq!(auto.strategy, "decomposed");
assert_poses_bit_identical(&deco, &auto);
}
fn analytic_line_line_distance(o1: Vec3, d1: Vec3, o2: Vec3, d2: Vec3) -> f64 {
let u = o1.sub(o2);
let c = d1.cross(d2);
let s = c.length();
if s <= 1e-9 {
u.sub(d2.scale(d2.dot(u))).length()
} else {
(u.dot(c) / s).abs()
}
}
fn world_line(
sol: &AssemblySolution,
body: usize,
origin: [f64; 3],
dir: [f64; 3],
) -> (Vec3, Vec3) {
let o = quat_rotate(sol.poses[body].rotation, vec3(origin))
.add(vec3(sol.poses[body].translation));
let d = quat_rotate(sol.poses[body].rotation, vec3(dir));
(o, d)
}
fn assert_bit_deterministic_3x(bodies: &[AssemblyBody], mates: &[AssemblyMate]) {
let opts = AssemblySolveOptions::default();
let runs: Vec<AssemblySolution> = (0..3)
.map(|_| solve_assembly(bodies, mates, &opts).unwrap())
.collect();
for run in &runs[1..] {
assert_poses_bit_identical(&runs[0], run);
assert_eq!(runs[0].max_residual.to_bits(), run.max_residual.to_bits());
assert_eq!(runs[0].iterations, run.iterations);
}
}
#[test]
fn point_line_distance_positions_body_closed_form() {
let bodies = vec![
body("frame", true, identity_rotation(), [0.0; 3]),
body("part", false, identity_rotation(), [3.0, 0.0, 5.0]),
];
let mates = vec![mate(
"standoff",
1,
0,
MateKind::DistancePointLine {
point_a: [0.0, 0.0, 0.0],
axis_b: axis([0.0, 0.0, 0.0], [0.0, 0.0, 1.0]),
distance: 2.0,
},
)];
let sol = solve_assembly(&bodies, &mates, &AssemblySolveOptions::default()).unwrap();
assert_close(
vec3(sol.poses[1].translation),
[2.0, 0.0, 5.0],
1e-9,
"part slides radially to the closed-form point",
);
assert!(
sol.max_residual <= 1e-9,
"max residual {}",
sol.max_residual
);
assert_eq!(sol.rank, 1, "one scalar distance row binds one DOF");
assert_eq!(sol.dof, 5);
assert_eq!(sol.status, "under");
assert_bit_deterministic_3x(&bodies, &mates);
}
#[test]
fn line_line_skew_distance_matches_analytic() {
let bodies = vec![
body("frame", true, identity_rotation(), [0.0; 3]),
body("rod", false, identity_rotation(), [0.5, 3.0, 0.0]),
];
let mates = vec![mate(
"clearance",
0,
1,
MateKind::DistanceLineLine {
axis_a: axis([0.0; 3], [0.0, 0.0, 1.0]),
axis_b: axis([0.0; 3], [1.0, 0.0, 0.0]),
distance: 1.0,
},
)];
let sol = solve_assembly(&bodies, &mates, &AssemblySolveOptions::default()).unwrap();
let (o2, d2) = world_line(&sol, 1, [0.0; 3], [1.0, 0.0, 0.0]);
let dist =
analytic_line_line_distance(Vec3::new(0.0, 0.0, 0.0), Vec3::new(0.0, 0.0, 1.0), o2, d2);
assert!((dist - 1.0).abs() <= 1e-9, "closest approach {dist}");
let sin = Vec3::new(0.0, 0.0, 1.0).cross(d2).length();
assert!(sin >= ASM_LL_SIN_SKEW, "solution stayed skew: sin {sin}");
assert!(
sol.max_residual <= 1e-9,
"max residual {}",
sol.max_residual
);
assert_bit_deterministic_3x(&bodies, &mates);
}
#[test]
fn line_line_parallel_degeneracy_converges() {
let bodies = vec![
body("frame", true, identity_rotation(), [0.0; 3]),
body("rod", false, identity_rotation(), [0.3, 0.4, 0.0]),
];
let mates = vec![mate(
"offset",
0,
1,
MateKind::DistanceLineLine {
axis_a: axis([0.0; 3], [0.0, 0.0, 1.0]),
axis_b: axis([0.0; 3], [0.0, 0.0, 1.0]),
distance: 2.0,
},
)];
let sol = solve_assembly(&bodies, &mates, &AssemblySolveOptions::default()).unwrap();
for pose in &sol.poses {
for c in pose.rotation.iter().chain(pose.translation.iter()) {
assert!(c.is_finite(), "non-finite pose component");
}
}
assert_close(
vec3(sol.poses[1].translation),
[1.2, 1.6, 0.0],
1e-8,
"radial slide to the closed form",
);
let (o2, d2) = world_line(&sol, 1, [0.0; 3], [0.0, 0.0, 1.0]);
let sin = Vec3::new(0.0, 0.0, 1.0).cross(d2).length();
assert!(
sin <= ASM_LL_SIN_PARALLEL,
"solution stayed in the parallel branch: sin {sin}"
);
let dist =
analytic_line_line_distance(Vec3::new(0.0, 0.0, 0.0), Vec3::new(0.0, 0.0, 1.0), o2, d2);
assert!((dist - 2.0).abs() <= 1e-9, "parallel distance {dist}");
assert!(
sol.max_residual <= 1e-9,
"max residual {}",
sol.max_residual
);
assert!(
sol.iterations <= 50,
"no oscillation: {} iterations",
sol.iterations
);
assert_eq!(sol.dof, 5);
assert_bit_deterministic_3x(&bodies, &mates);
}
#[test]
fn coincident_point_line_via_zero_distance() {
let bodies = vec![
body("frame", true, identity_rotation(), [0.0; 3]),
body("part", false, identity_rotation(), [0.3, 0.4, 0.2]),
];
let mates = vec![mate(
"on-axis",
1,
0,
MateKind::DistancePointLine {
point_a: [0.0; 3],
axis_b: axis([0.0; 3], [0.0, 0.0, 1.0]),
distance: 0.0,
},
)];
let sol = solve_assembly(&bodies, &mates, &AssemblySolveOptions::default()).unwrap();
assert_close(
vec3(sol.poses[1].translation),
[0.0, 0.0, 0.2],
1e-9,
"point dropped onto the axis",
);
assert!(
sol.max_residual <= 1e-9,
"max residual {}",
sol.max_residual
);
assert_eq!(sol.rank, 2, "a point on a line removes exactly 2 DOF");
assert_eq!(sol.dof, 4);
assert_eq!(sol.redundant, 0);
}
#[test]
fn concentric_via_parallel_plus_zero_line_line_distance() {
let bodies = vec![
body("frame", true, identity_rotation(), [0.0; 3]),
body(
"shaft",
false,
quat_axis_angle([1.0, 0.0, 0.0], 2.0),
[0.4, -0.3, 0.7],
),
];
let mates = vec![
mate(
"par",
0,
1,
MateKind::Parallel {
direction_a: [0.0, 0.0, 1.0],
direction_b: [0.0, 0.0, 1.0],
align: MateAlign::Any,
},
),
mate(
"touch",
0,
1,
MateKind::DistanceLineLine {
axis_a: axis([0.0; 3], [0.0, 0.0, 1.0]),
axis_b: axis([0.0; 3], [0.0, 0.0, 1.0]),
distance: 0.0,
},
),
];
let sol = solve_assembly(&bodies, &mates, &AssemblySolveOptions::default()).unwrap();
let (o2, d2) = world_line(&sol, 1, [0.0; 3], [0.0, 0.0, 1.0]);
let sin = Vec3::new(0.0, 0.0, 1.0).cross(d2).length();
assert!(sin <= 1e-9, "axes parallel: sin {sin}");
let lateral = (o2.x * o2.x + o2.y * o2.y).sqrt();
assert!(lateral <= 1e-9, "axes collinear: lateral {lateral}");
assert!(
sol.max_residual <= 1e-9,
"max residual {}",
sol.max_residual
);
let concentric = vec![mate(
"bore",
0,
1,
MateKind::ConcentricAxisAxis {
axis_a: axis([0.0; 3], [0.0, 0.0, 1.0]),
axis_b: axis([0.0; 3], [0.0, 0.0, 1.0]),
align: MateAlign::Any,
},
)];
let reference =
solve_assembly(&bodies, &concentric, &AssemblySolveOptions::default()).unwrap();
assert_eq!(sol.rank, reference.rank);
assert_eq!(sol.dof, reference.dof);
assert_eq!(sol.dof, 2, "spin and slide stay free");
assert_bit_deterministic_3x(&bodies, &mates);
}
#[test]
fn line_line_distance_only_reports_five_dof() {
let bodies = vec![
body("frame", true, identity_rotation(), [0.0; 3]),
body("rod", false, identity_rotation(), [0.5, 3.0, 0.0]),
];
let mates = vec![mate(
"clearance",
0,
1,
MateKind::DistanceLineLine {
axis_a: axis([0.0; 3], [0.0, 0.0, 1.0]),
axis_b: axis([0.0; 3], [1.0, 0.0, 0.0]),
distance: 3.0,
},
)];
let sol = solve_assembly(&bodies, &mates, &AssemblySolveOptions::default()).unwrap();
assert_eq!(sol.rank, 1, "one scalar distance row binds one DOF");
assert_eq!(sol.dof, 5);
assert_eq!(sol.redundant, 0);
assert_eq!(sol.status, "under");
assert_eq!(sol.iterations, 0, "already satisfied: nothing moves");
assert_close(
vec3(sol.poses[1].translation),
[0.5, 3.0, 0.0],
0.0,
"pose kept",
);
}
#[test]
fn distance_line_mates_serde_round_trip() {
let mates = vec![
mate(
"pl",
0,
1,
MateKind::DistancePointLine {
point_a: [1.0, 2.0, 3.0],
axis_b: axis([0.0; 3], [0.0, 0.0, 1.0]),
distance: 2.5,
},
),
mate(
"ll",
0,
1,
MateKind::DistanceLineLine {
axis_a: axis([0.0; 3], [0.0, 0.0, 1.0]),
axis_b: axis([1.0, 0.0, 0.0], [1.0, 0.0, 0.0]),
distance: 0.5,
},
),
];
let json = serde_json::to_string(&mates).unwrap();
assert!(json.contains("\"type\":\"distance_point_line\""), "{json}");
assert!(json.contains("\"type\":\"distance_line_line\""), "{json}");
let back: Vec<AssemblyMate> = serde_json::from_str(&json).unwrap();
match &back[0].kind {
MateKind::DistancePointLine { distance, .. } => assert_eq!(*distance, 2.5),
other => panic!("wrong kind after round-trip: {other:?}"),
}
match &back[1].kind {
MateKind::DistanceLineLine { distance, .. } => assert_eq!(*distance, 0.5),
other => panic!("wrong kind after round-trip: {other:?}"),
}
let from_app: AssemblyMate = serde_json::from_str(
r#"{"id":"edge-gap","body_a":0,"body_b":1,"type":"distance_line_line",
"axis_a":{"origin":[0,0,0],"direction":[0,0,1]},
"axis_b":{"origin":[0,1,0],"direction":[0,0,1]},
"distance":1.5}"#,
)
.unwrap();
match from_app.kind {
MateKind::DistanceLineLine { distance, .. } => assert_eq!(distance, 1.5),
other => panic!("wrong kind: {other:?}"),
}
}
#[test]
fn strategy_serde_default_and_round_trip() {
let parsed: AssemblySolveOptions = serde_json::from_str("{}").unwrap();
assert_eq!(parsed.strategy, SolveStrategy::Auto);
assert_eq!(parsed.tolerance, default_tolerance());
let parsed: AssemblySolveOptions =
serde_json::from_str(r#"{"strategy":"decomposed"}"#).unwrap();
assert_eq!(parsed.strategy, SolveStrategy::Decomposed);
let json = serde_json::to_string(&AssemblySolveOptions {
strategy: SolveStrategy::Monolithic,
..AssemblySolveOptions::default()
})
.unwrap();
assert!(json.contains("\"strategy\":\"monolithic\""), "{json}");
}