brep_kernel/solvers/assembly_solver/
mates.rs

1use super::*;
2
3// ---------------------------------------------------------------------------
4// Mate expansion + validation
5// ---------------------------------------------------------------------------
6
7pub(super) fn mate_name(index: usize, mate: &AssemblyMate) -> String {
8    mate.id.clone().unwrap_or_else(|| format!("mate[{index}]"))
9}
10
11pub(super) fn vec3(v: [f64; 3]) -> Vec3 {
12    Vec3::new(v[0], v[1], v[2])
13}
14
15fn finite_point(v: [f64; 3], what: &str, mate: &str) -> Result<Vec3, String> {
16    if !(v[0].is_finite() && v[1].is_finite() && v[2].is_finite()) {
17        return Err(format!("{mate}: {what} must be finite"));
18    }
19    Ok(vec3(v))
20}
21
22fn unit_dir(v: [f64; 3], what: &str, mate: &str) -> Result<Vec3, String> {
23    let v = finite_point(v, what, mate)?;
24    let len = v.length();
25    if !(len > ASM_DIR_EPS) {
26        return Err(format!("{mate}: {what} must be a nonzero vector"));
27    }
28    Ok(v.scale(1.0 / len))
29}
30
31fn finite_scalar(x: f64, what: &str, mate: &str) -> Result<f64, String> {
32    if !x.is_finite() {
33        return Err(format!("{mate}: {what} must be finite"));
34    }
35    Ok(x)
36}
37
38/// Direction-sense atom for `align` between two unit local directions.
39fn dir_atom(a: LocalDir, b: LocalDir, align: MateAlign) -> Atom {
40    match align {
41        MateAlign::Aligned => Atom::DirMatch(a, b, 1.0),
42        MateAlign::AntiAligned => Atom::DirMatch(a, b, -1.0),
43        MateAlign::Any => Atom::DirCross(a, b),
44    }
45}
46
47/// Expand every mate into residual atoms. Returns the atoms plus a parallel
48/// map atom → mate index for residual attribution.
49pub(super) fn expand_mates(
50    bodies: &[AssemblyBody],
51    mates: &[AssemblyMate],
52) -> Result<(Vec<Atom>, Vec<usize>), String> {
53    let mut atoms: Vec<Atom> = Vec::new();
54    let mut atom_mate: Vec<usize> = Vec::new();
55    for (mi, mate) in mates.iter().enumerate() {
56        let name = mate_name(mi, mate);
57        if mate.body_a >= bodies.len() || mate.body_b >= bodies.len() {
58            return Err(format!(
59                "{name}: body index out of range (body_a={}, body_b={}, {} bodies)",
60                mate.body_a,
61                mate.body_b,
62                bodies.len()
63            ));
64        }
65        if mate.body_a == mate.body_b {
66            return Err(format!("{name}: body_a and body_b must differ"));
67        }
68        if let Some(c) = mate.body_c {
69            if c >= bodies.len() {
70                return Err(format!("{name}: body_c out of range ({c}, {} bodies)", bodies.len()));
71            }
72        }
73        let ba = mate.body_a;
74        let bb = mate.body_b;
75        let before = atoms.len();
76        match &mate.kind {
77            MateKind::CoincidentPointPoint { point_a, point_b } => {
78                let pa = finite_point(*point_a, "point_a", &name)?;
79                let pb = finite_point(*point_b, "point_b", &name)?;
80                atoms.push(Atom::PointsCoincide(
81                    LocalPoint { body: ba, p: pa },
82                    LocalPoint { body: bb, p: pb },
83                ));
84            }
85            MateKind::CoincidentPointPlane { point_a, plane_b } => {
86                let pa = finite_point(*point_a, "point_a", &name)?;
87                let po = finite_point(plane_b.origin, "plane_b.origin", &name)?;
88                let pn = unit_dir(plane_b.normal, "plane_b.normal", &name)?;
89                atoms.push(Atom::PointOnPlane(
90                    LocalPoint { body: ba, p: pa },
91                    LocalPoint { body: bb, p: po },
92                    LocalDir { body: bb, d: pn },
93                    0.0,
94                ));
95            }
96            MateKind::CoincidentPlanePlane {
97                plane_a,
98                plane_b,
99                align,
100            } => {
101                let oa = finite_point(plane_a.origin, "plane_a.origin", &name)?;
102                let na = unit_dir(plane_a.normal, "plane_a.normal", &name)?;
103                let ob = finite_point(plane_b.origin, "plane_b.origin", &name)?;
104                let nb = unit_dir(plane_b.normal, "plane_b.normal", &name)?;
105                atoms.push(dir_atom(
106                    LocalDir { body: ba, d: na },
107                    LocalDir { body: bb, d: nb },
108                    *align,
109                ));
110                // Plane B's origin lies on plane A.
111                atoms.push(Atom::PointOnPlane(
112                    LocalPoint { body: bb, p: ob },
113                    LocalPoint { body: ba, p: oa },
114                    LocalDir { body: ba, d: na },
115                    0.0,
116                ));
117            }
118            MateKind::ConcentricAxisAxis {
119                axis_a,
120                axis_b,
121                align,
122            } => {
123                let oa = finite_point(axis_a.origin, "axis_a.origin", &name)?;
124                let da = unit_dir(axis_a.direction, "axis_a.direction", &name)?;
125                let ob = finite_point(axis_b.origin, "axis_b.origin", &name)?;
126                let db = unit_dir(axis_b.direction, "axis_b.direction", &name)?;
127                atoms.push(dir_atom(
128                    LocalDir { body: ba, d: da },
129                    LocalDir { body: bb, d: db },
130                    *align,
131                ));
132                // Axis A's origin lies on axis B's line (lateral offset zero).
133                atoms.push(Atom::AxisLateral(
134                    LocalPoint { body: ba, p: oa },
135                    LocalPoint { body: bb, p: ob },
136                    LocalDir { body: bb, d: db },
137                ));
138            }
139            MateKind::DistancePointPoint {
140                point_a,
141                point_b,
142                distance,
143            } => {
144                let pa = finite_point(*point_a, "point_a", &name)?;
145                let pb = finite_point(*point_b, "point_b", &name)?;
146                let d = finite_scalar(*distance, "distance", &name)?;
147                let lp_a = LocalPoint { body: ba, p: pa };
148                let lp_b = LocalPoint { body: bb, p: pb };
149                if d.abs() <= ASM_DIR_EPS {
150                    // ‖·‖ is non-differentiable at zero: degrade to coincident.
151                    atoms.push(Atom::PointsCoincide(lp_a, lp_b));
152                } else {
153                    atoms.push(Atom::PointDistance(lp_a, lp_b, d.abs()));
154                }
155            }
156            MateKind::DistancePointPlane {
157                point_a,
158                plane_b,
159                distance,
160            } => {
161                let pa = finite_point(*point_a, "point_a", &name)?;
162                let po = finite_point(plane_b.origin, "plane_b.origin", &name)?;
163                let pn = unit_dir(plane_b.normal, "plane_b.normal", &name)?;
164                let d = finite_scalar(*distance, "distance", &name)?;
165                atoms.push(Atom::PointOnPlane(
166                    LocalPoint { body: ba, p: pa },
167                    LocalPoint { body: bb, p: po },
168                    LocalDir { body: bb, d: pn },
169                    d,
170                ));
171            }
172            MateKind::DistancePlanePlane {
173                plane_a,
174                plane_b,
175                distance,
176                align,
177            } => {
178                let oa = finite_point(plane_a.origin, "plane_a.origin", &name)?;
179                let na = unit_dir(plane_a.normal, "plane_a.normal", &name)?;
180                let ob = finite_point(plane_b.origin, "plane_b.origin", &name)?;
181                let nb = unit_dir(plane_b.normal, "plane_b.normal", &name)?;
182                let d = finite_scalar(*distance, "distance", &name)?;
183                atoms.push(dir_atom(
184                    LocalDir { body: ba, d: na },
185                    LocalDir { body: bb, d: nb },
186                    *align,
187                ));
188                // Signed offset of plane B's origin along plane A's normal.
189                atoms.push(Atom::PointOnPlane(
190                    LocalPoint { body: bb, p: ob },
191                    LocalPoint { body: ba, p: oa },
192                    LocalDir { body: ba, d: na },
193                    d,
194                ));
195            }
196            MateKind::DistancePointLine {
197                point_a,
198                axis_b,
199                distance,
200            } => {
201                let pa = finite_point(*point_a, "point_a", &name)?;
202                let ob = finite_point(axis_b.origin, "axis_b.origin", &name)?;
203                let db = unit_dir(axis_b.direction, "axis_b.direction", &name)?;
204                let d = finite_scalar(*distance, "distance", &name)?;
205                let lp = LocalPoint { body: ba, p: pa };
206                let lo = LocalPoint { body: bb, p: ob };
207                let ld = LocalDir { body: bb, d: db };
208                if d.abs() <= ASM_DIR_EPS {
209                    // ‖·‖ is non-differentiable at zero: degrade to the
210                    // point-on-line lateral atom (the same one concentric
211                    // uses) — the coincident point-line shortcut.
212                    atoms.push(Atom::AxisLateral(lp, lo, ld));
213                } else {
214                    atoms.push(Atom::PointLineDistance(lp, lo, ld, d.abs()));
215                }
216            }
217            MateKind::DistanceLineLine {
218                axis_a,
219                axis_b,
220                distance,
221            } => {
222                let oa = finite_point(axis_a.origin, "axis_a.origin", &name)?;
223                let da = unit_dir(axis_a.direction, "axis_a.direction", &name)?;
224                let ob = finite_point(axis_b.origin, "axis_b.origin", &name)?;
225                let db = unit_dir(axis_b.direction, "axis_b.direction", &name)?;
226                let d = finite_scalar(*distance, "distance", &name)?;
227                let loa = LocalPoint { body: ba, p: oa };
228                let lda = LocalDir { body: ba, d: da };
229                let lob = LocalPoint { body: bb, p: ob };
230                let ldb = LocalDir { body: bb, d: db };
231                if d.abs() <= ASM_DIR_EPS {
232                    // Distance 0 = the lines touch: intersect when skew,
233                    // collinear when parallel. The signed vector atom avoids
234                    // the ‖·‖ − 0 kink at the solution.
235                    atoms.push(Atom::LineLineTouch(loa, lda, lob, ldb));
236                } else {
237                    atoms.push(Atom::LineLineDistance(loa, lda, lob, ldb, d.abs()));
238                }
239            }
240            MateKind::Angle {
241                direction_a,
242                direction_b,
243                angle_deg,
244            } => {
245                let da = unit_dir(*direction_a, "direction_a", &name)?;
246                let db = unit_dir(*direction_b, "direction_b", &name)?;
247                let ang = finite_scalar(*angle_deg, "angle_deg", &name)?;
248                atoms.push(Atom::DirDot(
249                    LocalDir { body: ba, d: da },
250                    LocalDir { body: bb, d: db },
251                    ang.to_radians().cos(),
252                ));
253            }
254            MateKind::DirectedAngle {
255                direction_a,
256                direction_b,
257                axis_a,
258                angle_deg,
259            } => {
260                let da = unit_dir(*direction_a, "direction_a", &name)?;
261                let db = unit_dir(*direction_b, "direction_b", &name)?;
262                let axis = unit_dir(*axis_a, "axis_a", &name)?;
263                if da.dot(axis).abs() > 1e-8 {
264                    return Err(format!("{name}: axis_a must be perpendicular to direction_a"));
265                }
266                let ang = finite_scalar(*angle_deg, "angle_deg", &name)?;
267                atoms.push(Atom::DirAngle(
268                    LocalDir { body: ba, d: da },
269                    LocalDir { body: bb, d: db },
270                    LocalDir { body: ba, d: axis },
271                    ang.rem_euclid(360.0).to_radians(),
272                ));
273            }
274            MateKind::Parallel {
275                direction_a,
276                direction_b,
277                align,
278            } => {
279                let da = unit_dir(*direction_a, "direction_a", &name)?;
280                let db = unit_dir(*direction_b, "direction_b", &name)?;
281                atoms.push(dir_atom(
282                    LocalDir { body: ba, d: da },
283                    LocalDir { body: bb, d: db },
284                    *align,
285                ));
286            }
287            MateKind::Perpendicular {
288                direction_a,
289                direction_b,
290            } => {
291                let da = unit_dir(*direction_a, "direction_a", &name)?;
292                let db = unit_dir(*direction_b, "direction_b", &name)?;
293                atoms.push(Atom::DirDot(
294                    LocalDir { body: ba, d: da },
295                    LocalDir { body: bb, d: db },
296                    0.0,
297                ));
298            }
299            MateKind::TangentSpherePlane {
300                center_a,
301                radius,
302                plane_b,
303            } => {
304                let ca = finite_point(*center_a, "center_a", &name)?;
305                let po = finite_point(plane_b.origin, "plane_b.origin", &name)?;
306                let pn = unit_dir(plane_b.normal, "plane_b.normal", &name)?;
307                let r = finite_scalar(*radius, "radius", &name)?;
308                atoms.push(Atom::PointOnPlane(
309                    LocalPoint { body: ba, p: ca },
310                    LocalPoint { body: bb, p: po },
311                    LocalDir { body: bb, d: pn },
312                    r.abs(),
313                ));
314            }
315            MateKind::Symmetric { element_a, element_b, plane_c } => {
316                let bc = mate
317                    .body_c
318                    .ok_or_else(|| format!("{name}: a symmetric mate needs body_c (the mirror's body)"))?;
319                let o = LocalPoint { body: bc, p: finite_point(plane_c.origin, "plane_c.origin", &name)? };
320                let n = LocalDir { body: bc, d: unit_dir(plane_c.normal, "plane_c.normal", &name)? };
321                match (element_a, element_b) {
322                    (SymmetricElement::Point { point: pa }, SymmetricElement::Point { point: pb }) => {
323                        let pa = LocalPoint { body: ba, p: finite_point(*pa, "element_a.point", &name)? };
324                        let pb = LocalPoint { body: bb, p: finite_point(*pb, "element_b.point", &name)? };
325                        atoms.push(Atom::SymPoints(pa, pb, o, n));
326                    }
327                    (SymmetricElement::Plane { plane: pa }, SymmetricElement::Plane { plane: pb }) => {
328                        let oa = LocalPoint { body: ba, p: finite_point(pa.origin, "element_a.origin", &name)? };
329                        let na = LocalDir { body: ba, d: unit_dir(pa.normal, "element_a.normal", &name)? };
330                        let ob = LocalPoint { body: bb, p: finite_point(pb.origin, "element_b.origin", &name)? };
331                        let nb = LocalDir { body: bb, d: unit_dir(pb.normal, "element_b.normal", &name)? };
332                        atoms.push(Atom::SymDirMatch(na, nb, n));
333                        atoms.push(Atom::SymPointOnPlane(ob, oa, na, o, n));
334                    }
335                    (SymmetricElement::Axis { axis: xa }, SymmetricElement::Axis { axis: xb }) => {
336                        let oa = LocalPoint { body: ba, p: finite_point(xa.origin, "element_a.origin", &name)? };
337                        let da = LocalDir { body: ba, d: unit_dir(xa.direction, "element_a.direction", &name)? };
338                        let ob = LocalPoint { body: bb, p: finite_point(xb.origin, "element_b.origin", &name)? };
339                        let db = LocalDir { body: bb, d: unit_dir(xb.direction, "element_b.direction", &name)? };
340                        atoms.push(Atom::SymDirCross(da, db, n));
341                        atoms.push(Atom::SymAxisLateral(oa, ob, db, o, n));
342                    }
343                    _ => {
344                        return Err(format!(
345                            "{name}: a symmetric mate pairs like elements (two points, two planes or two axes)"
346                        ))
347                    }
348                }
349            }
350            MateKind::Gear { axis_a, ref_a, world_ref_a, axis_b, ref_b, world_ref_b, ratio, reverse } => {
351                let ratio = finite_scalar(*ratio, "ratio", &name)?;
352                let a = spin(ba, axis_a, *ref_a, *world_ref_a, "a", &name)?;
353                let b = spin(bb, axis_b, *ref_b, *world_ref_b, "b", &name)?;
354                atoms.push(Atom::GearPhase(a, b, ratio, if *reverse { -1.0 } else { 1.0 }));
355            }
356            MateKind::RackPinion {
357                pinion_axis,
358                pinion_ref,
359                world_pinion_ref,
360                rack_point,
361                rack_direction,
362                world_rack_point,
363                distance_per_revolution,
364                reverse,
365            } => {
366                let pinion = spin(ba, pinion_axis, *pinion_ref, *world_pinion_ref, "pinion", &name)?;
367                let p = LocalPoint { body: bb, p: finite_point(*rack_point, "rack_point", &name)? };
368                let d = LocalDir { body: bb, d: unit_dir(*rack_direction, "rack_direction", &name)? };
369                let origin = finite_point(*world_rack_point, "world_rack_point", &name)?;
370                let per_turn = finite_scalar(*distance_per_revolution, "distance_per_revolution", &name)?;
371                let k = per_turn / std::f64::consts::TAU * if *reverse { -1.0 } else { 1.0 };
372                atoms.push(Atom::RackTravel(pinion, p, d, origin, k));
373            }
374            MateKind::Screw { axis_a, ref_a, ref_b, point_b, pitch, reverse, angle0, offset0 } => {
375                let oa = LocalPoint { body: ba, p: finite_point(axis_a.origin, "axis_a.origin", &name)? };
376                let da = LocalDir { body: ba, d: unit_dir(axis_a.direction, "axis_a.direction", &name)? };
377                let ra = LocalDir { body: ba, d: unit_dir(*ref_a, "ref_a", &name)? };
378                let rb = LocalDir { body: bb, d: unit_dir(*ref_b, "ref_b", &name)? };
379                let pb = LocalPoint { body: bb, p: finite_point(*point_b, "point_b", &name)? };
380                let pitch = finite_scalar(*pitch, "pitch", &name)?;
381                let k = pitch / std::f64::consts::TAU * if *reverse { -1.0 } else { 1.0 };
382                atoms.push(Atom::ScrewAdvance(
383                    oa,
384                    da,
385                    ra,
386                    rb,
387                    pb,
388                    finite_scalar(*angle0, "angle0", &name)?,
389                    finite_scalar(*offset0, "offset0", &name)?,
390                    k,
391                ));
392            }
393            MateKind::Limit { mate: inner, min, max } => {
394                let min = finite_scalar(*min, "min", &name)?;
395                let max = finite_scalar(*max, "max", &name)?;
396                atoms.extend(limit_atoms(bodies, ba, bb, inner, min, max, &name)?);
397            }
398            MateKind::TangentCylinderPlane {
399                axis_a,
400                radius,
401                plane_b,
402            } => {
403                let oa = finite_point(axis_a.origin, "axis_a.origin", &name)?;
404                let da = unit_dir(axis_a.direction, "axis_a.direction", &name)?;
405                let po = finite_point(plane_b.origin, "plane_b.origin", &name)?;
406                let pn = unit_dir(plane_b.normal, "plane_b.normal", &name)?;
407                let r = finite_scalar(*radius, "radius", &name)?;
408                // Axis parallel to the plane…
409                atoms.push(Atom::DirDot(
410                    LocalDir { body: ba, d: da },
411                    LocalDir { body: bb, d: pn },
412                    0.0,
413                ));
414                // …at height r above it.
415                atoms.push(Atom::PointOnPlane(
416                    LocalPoint { body: ba, p: oa },
417                    LocalPoint { body: bb, p: po },
418                    LocalDir { body: bb, d: pn },
419                    r.abs(),
420                ));
421            }
422            MateKind::PointOnCurve { point_a, curve_b, along, reverse } => {
423                let pa = finite_point(*point_a, "point_a", &name)?;
424                let curve = CurveRef { body: bb, curve: std::sync::Arc::new(SampledCurve::new(curve_b, &name)?) };
425                let lp = LocalPoint { body: ba, p: pa };
426                atoms.push(Atom::PointOnCurve(lp, curve.clone()));
427                if let Some(percent) = along {
428                    let percent = finite_scalar(*percent, "along", &name)?;
429                    atoms.push(Atom::CurveAlong(lp, None, curve, percent / 100.0, *reverse));
430                }
431            }
432            MateKind::Slot { axis_a, curve_b, normal_b, along, reverse } => {
433                let oa = finite_point(axis_a.origin, "axis_a.origin", &name)?;
434                let da = unit_dir(axis_a.direction, "axis_a.direction", &name)?;
435                let nb = unit_dir(*normal_b, "normal_b", &name)?;
436                let curve = CurveRef { body: bb, curve: std::sync::Arc::new(SampledCurve::new(curve_b, &name)?) };
437                let (lp, ld) = (LocalPoint { body: ba, p: oa }, LocalDir { body: ba, d: da });
438                atoms.push(Atom::DirCross(ld, LocalDir { body: bb, d: nb }));
439                atoms.push(Atom::AxisOnCurve(lp, ld, curve.clone()));
440                if let Some(percent) = along {
441                    let percent = finite_scalar(*percent, "along", &name)?;
442                    atoms.push(Atom::CurveAlong(lp, Some(ld), curve, percent / 100.0, *reverse));
443                }
444            }
445            MateKind::Cam { center_a, axis_a, radius, profile_b, normal_b, flip } => {
446                let ca = finite_point(*center_a, "center_a", &name)?;
447                let nb = unit_dir(*normal_b, "normal_b", &name)?;
448                let r = finite_scalar(*radius, "radius", &name)?.abs();
449                let sampled = SampledCurve::new(profile_b, &name)?;
450                let start = sampled.start();
451                // Outside a closed profile is to the right of its direction
452                // when it runs counter-clockwise about the normal.
453                let side = sampled.orientation(nb) * if *flip { -1.0 } else { 1.0 };
454                let curve = CurveRef { body: bb, curve: std::sync::Arc::new(sampled) };
455                let (lp, ln) = (LocalPoint { body: ba, p: ca }, LocalDir { body: bb, d: nb });
456                let axis = match axis_a {
457                    Some(direction) => {
458                        let da = unit_dir(*direction, "axis_a", &name)?;
459                        let ld = LocalDir { body: ba, d: da };
460                        atoms.push(Atom::DirCross(ld, ln));
461                        Some(ld)
462                    }
463                    None => {
464                        atoms.push(Atom::PointOnPlane(lp, LocalPoint { body: bb, p: start }, ln, 0.0));
465                        None
466                    }
467                };
468                atoms.push(Atom::CamContact(lp, axis, curve, ln, r, side));
469            }
470        }
471        for _ in before..atoms.len() {
472            atom_mate.push(mi);
473        }
474    }
475    Ok((atoms, atom_mate))
476}
477
478/// Expand a limit mate: `inner`'s rows with its measure (always the LAST atom
479/// a measuring kind emits) replaced by the limit row. Angles convert to the
480/// measure's own units — the cosine for `Angle` (which reverses the ends),
481/// radians for `DirectedAngle`.
482fn limit_atoms(
483    bodies: &[AssemblyBody],
484    ba: usize,
485    bb: usize,
486    inner: &MateKind,
487    min: f64,
488    max: f64,
489    name: &str,
490) -> Result<Vec<Atom>, String> {
491    let (lo, hi) = match inner {
492        MateKind::Angle { .. } => {
493            if min > max {
494                return Err(format!("{name}: limit min must not exceed max"));
495            }
496            (max.to_radians().cos(), min.to_radians().cos())
497        }
498        MateKind::DirectedAngle { .. } => (min.to_radians(), max.to_radians()),
499        MateKind::DistancePointPoint { .. }
500        | MateKind::DistancePointPlane { .. }
501        | MateKind::DistancePlanePlane { .. }
502        | MateKind::DistancePointLine { .. }
503        | MateKind::DistanceLineLine { .. } => {
504            if min > max {
505                return Err(format!("{name}: limit min must not exceed max"));
506            }
507            (min, max)
508        }
509        _ => return Err(format!("{name}: only a distance or angle mate can be limited")),
510    };
511    let single = AssemblyMate { id: Some(name.to_string()), body_a: ba, body_b: bb, body_c: None, kind: inner.clone() };
512    let (mut atoms, _) = expand_mates(bodies, std::slice::from_ref(&single))?;
513    let measure = atoms
514        .pop()
515        .as_ref()
516        .and_then(Measure::of)
517        .ok_or_else(|| format!("{name}: the limited mate measures nothing"))?;
518    atoms.push(Atom::Limit(measure, lo, hi));
519    Ok(atoms)
520}
521
522/// A spin atom side: `axis` and the local reference ⟂ it on `body`, read
523/// against the world reference `world`.
524fn spin(body: usize, axis: &MateAxis, reference: [f64; 3], world: [f64; 3], side: &str, name: &str) -> Result<Spin, String> {
525    let n = unit_dir(axis.direction, &format!("{side} axis direction"), name)?;
526    let r = finite_point(reference, &format!("{side} reference"), name)?;
527    let r = r.sub(n.scale(r.dot(n)));
528    if !(r.length() > ASM_DIR_EPS) {
529        return Err(format!("{name}: the {side} reference must not lie along its axis"));
530    }
531    let world = finite_point(world, &format!("{side} world reference"), name)?;
532    Ok(Spin { reference: LocalDir { body, d: r.scale(1.0 / r.length()) }, axis: LocalDir { body, d: n }, world })
533}
534
535impl MateKind {
536    /// Re-capture a mechanical mate's references so it holds exactly the
537    /// poses given (`(rotation [w,x,y,z], translation)` of body A and body B):
538    /// the gear's and the pinion's world reference directions, the rack's
539    /// world point, the screw's angle and offset. Every other kind is left
540    /// alone. Mapping calls this at the current poses, so a new mate moves
541    /// nothing; a live drag calls it at the last good poses, so each frame
542    /// couples only the motion since then and never wraps a spin.
543    /// A mechanical mate's captured references as plain numbers (the world
544    /// reference directions / point, or the screw's angle and offset), for
545    /// persisting across runs; `None` for every other kind.
546    pub fn references(&self) -> Option<Vec<f64>> {
547        match self {
548            MateKind::Gear { world_ref_a, world_ref_b, .. } => Some([&world_ref_a[..], &world_ref_b[..]].concat()),
549            MateKind::RackPinion { world_pinion_ref, world_rack_point, .. } => {
550                Some([&world_pinion_ref[..], &world_rack_point[..]].concat())
551            }
552            // The nut's reference direction travels with the angle it was
553            // captured against: the mapping derives `ref_b` from the poses at
554            // map time, so a later run would pair a fresh `ref_b` with an old
555            // `angle0` and read the screw as turned.
556            MateKind::Screw { ref_b, angle0, offset0, .. } => Some(vec![*angle0, *offset0, ref_b[0], ref_b[1], ref_b[2]]),
557            _ => None,
558        }
559    }
560
561    /// Restore references saved by [`MateKind::references`]; `false` (and no
562    /// change) when the kind or the count does not match.
563    pub fn set_references(&mut self, values: &[f64]) -> bool {
564        let triple = |v: &[f64]| [v[0], v[1], v[2]];
565        match self {
566            MateKind::Gear { world_ref_a, world_ref_b, .. } if values.len() == 6 => {
567                *world_ref_a = triple(&values[0..3]);
568                *world_ref_b = triple(&values[3..6]);
569                true
570            }
571            MateKind::RackPinion { world_pinion_ref, world_rack_point, .. } if values.len() == 6 => {
572                *world_pinion_ref = triple(&values[0..3]);
573                *world_rack_point = triple(&values[3..6]);
574                true
575            }
576            // Two values: saved before `ref_b` was persisted; the mapped
577            // `ref_b` stands.
578            MateKind::Screw { ref_b, angle0, offset0, .. } if values.len() == 2 || values.len() == 5 => {
579                *angle0 = values[0];
580                *offset0 = values[1];
581                if values.len() == 5 {
582                    *ref_b = triple(&values[2..5]);
583                }
584                true
585            }
586            _ => false,
587        }
588    }
589
590    /// Which side of a gear or rack-and-pinion moved since its references
591    /// were captured, when exactly one did: `Some(true)` for body A, `Some(false)`
592    /// for body B. A typed edit of one gear makes that gear the driver.
593    pub fn driver(&self, pose_a: ([f64; 4], [f64; 3]), pose_b: ([f64; 4], [f64; 3])) -> Option<bool> {
594        let rotate = |pose: ([f64; 4], [f64; 3]), v: [f64; 3]| quat_rotate(pose.0, vec3(v));
595        let place = |pose: ([f64; 4], [f64; 3]), p: [f64; 3]| rotate(pose, p).add(vec3(pose.1));
596        let unit = |v: Vec3| v.scale(1.0 / v.length().max(ASM_DIR_EPS));
597        let (moved_a, moved_b) = match self {
598            MateKind::Gear { axis_a, ref_a, world_ref_a, axis_b, ref_b, world_ref_b, .. } => (
599                signed_angle(vec3(*world_ref_a), rotate(pose_a, *ref_a), unit(rotate(pose_a, axis_a.direction))).abs(),
600                signed_angle(vec3(*world_ref_b), rotate(pose_b, *ref_b), unit(rotate(pose_b, axis_b.direction))).abs(),
601            ),
602            MateKind::RackPinion { pinion_axis, pinion_ref, world_pinion_ref, rack_point, world_rack_point, .. } => (
603                signed_angle(vec3(*world_pinion_ref), rotate(pose_a, *pinion_ref), unit(rotate(pose_a, pinion_axis.direction))).abs(),
604                place(pose_b, *rack_point).sub(vec3(*world_rack_point)).length(),
605            ),
606            _ => return None,
607        };
608        const STILL: f64 = 1e-9;
609        match (moved_a > STILL, moved_b > STILL) {
610            (true, false) => Some(true),
611            (false, true) => Some(false),
612            _ => None,
613        }
614    }
615
616    pub fn rebase(&mut self, pose_a: ([f64; 4], [f64; 3]), pose_b: ([f64; 4], [f64; 3])) {
617        let rotate = |pose: ([f64; 4], [f64; 3]), v: [f64; 3]| quat_rotate(pose.0, vec3(v));
618        let place = |pose: ([f64; 4], [f64; 3]), p: [f64; 3]| rotate(pose, p).add(vec3(pose.1));
619        let triple = |v: Vec3| [v.x, v.y, v.z];
620        match self {
621            MateKind::Gear { ref_a, world_ref_a, ref_b, world_ref_b, .. } => {
622                *world_ref_a = triple(rotate(pose_a, *ref_a));
623                *world_ref_b = triple(rotate(pose_b, *ref_b));
624            }
625            MateKind::RackPinion { pinion_ref, world_pinion_ref, rack_point, world_rack_point, .. } => {
626                *world_pinion_ref = triple(rotate(pose_a, *pinion_ref));
627                *world_rack_point = triple(place(pose_b, *rack_point));
628            }
629            MateKind::Screw { axis_a, ref_a, ref_b, point_b, angle0, offset0, .. } => {
630                let n = rotate(pose_a, axis_a.direction);
631                let n = n.scale(1.0 / n.length().max(ASM_DIR_EPS));
632                *angle0 = signed_angle(rotate(pose_a, *ref_a), rotate(pose_b, *ref_b), n);
633                *offset0 = place(pose_b, *point_b).sub(place(pose_a, axis_a.origin)).dot(n);
634            }
635            _ => {}
636        }
637    }
638}
brep_kernel/solvers/assembly_solver/mates.rs

brep_kernel/solvers/assembly_solver/
mates.rs
