geop_ops_assembly/
joints.rs

1//! Joints and couplings: mates that say how one part may move against
2//! another, rather than only what touches what.
3//!
4//! A joint holds a [`Connector`] of one part on a connector of another — an
5//! origin, an axis through it and a direction square to the axis that
6//! turns are measured from — and lets the second part turn about the first
7//! one's axis, slide along it, or both, as its [`JointKind`] says. How far
8//! it is turned and slid are the joint's **coordinates**: values of the
9//! program's state, solved with the poses, so that a joint can be set to a
10//! value, held there, kept within limits, and coupled to another joint's.
11//!
12//! A coupling ([`CouplingKind`]) ties a coordinate of one joint to one of
13//! another — or of the same one — linearly: a gear pair, a rack and its
14//! pinion, a screw.
15//!
16//! Like [`crate::residuals`], every residual is computed in dual numbers.
17
18use geop_core_math::{
19    geop_error::{GeopError, GeopResult},
20    plain::{cross, dot, sub},
21    primitives::Pose,
22    scalars::{Number, Scalar, as_f64},
23    vector::Vector3,
24};
25use geop_ops_model::selection::Shape;
26use serde::{Deserialize, Serialize};
27use std::collections::BTreeMap;
28
29use crate::residuals::{D, Placing, parallel};
30
31/// A joint's end on a body, in that body's frame — or, placed, where its
32/// body puts it: where its axis passes, the axis, and the direction its
33/// turns are measured from — unit vectors, the reference square to the
34/// axis.
35#[derive(Clone, Copy, Debug, PartialEq)]
36pub struct Connector<T: Number = Scalar> {
37    pub origin: Vector3<T>,
38    pub axis: Vector3<T>,
39    pub reference: Vector3<T>,
40}
41
42impl Connector {
43    /// The connector at `origin`, along `axis` (any non-zero length), its
44    /// turns measured from `reference` made square to the axis — or, with
45    /// none given, from whichever of the body's own axes `x`, `y`, `z` runs
46    /// least along it, made square to it: a free choice, and the
47    /// well-conditioned one. So two bodies drawn the same way round, mated
48    /// on the same axis of each, are at a turn of zero.
49    pub fn new(origin: Vector3, axis: Vector3, reference: Option<Vector3>) -> GeopResult<Self> {
50        let axis = axis
51            .normalize()
52            .map_err(|e| e.with_context(format!("the axis {axis:?} of a joint")))?;
53        let reference = match reference {
54            None => axis.across()?[0],
55            Some(reference) => {
56                let square = reference.sub(&axis.prod_scalar(axis.prod_dot(&reference)));
57                square.normalize().map_err(|e| {
58                    e.with_context(format!(
59                        "the reference {reference:?} of a joint runs along its axis {axis:?}"
60                    ))
61                })?
62            }
63        };
64        Ok(Connector {
65            origin,
66            axis,
67            reference,
68        })
69    }
70
71    /// The connector `aspects` gives a joint where it is made of what the
72    /// entity is — an axis through a point it defines: a circular edge's
73    /// center and normal, a sketch circle's, or the origin and `z` axis of
74    /// a frame, turns measured from its `x` axis. Of anything else, there is
75    /// none: it is used by the frame it gives (see [`geop_ops_model::mate_entity::MateEntity::frame`]).
76    pub fn of(aspects: &Shape, face: bool) -> Option<GeopResult<Self>> {
77        if let Some(arc) = &aspects.arc {
78            return Some(Connector::new(arc.circle.center, arc.circle.normal, None));
79        }
80        if let Some(frame) = &aspects.frame {
81            return Some(Connector::new(
82                *frame.origin(),
83                *frame.w(),
84                Some(*frame.u()),
85            ));
86        }
87        if let (Some(round), false) = (&aspects.round, face) {
88            return Some(Connector::new(round.point, round.direction, None));
89        }
90        None
91    }
92
93    /// Where `pose` puts it — or, with none, where it is: of a part that
94    /// never moves.
95    pub fn placed(&self, pose: Option<&Pose<D>>) -> Connector<D> {
96        let placing = Placing::new(pose);
97        let turn = |d: &Vector3| placing.turn(&d.map(D::cst));
98        Connector {
99            origin: placing.point(&self.origin),
100            axis: turn(&self.axis),
101            reference: turn(&self.reference),
102        }
103    }
104}
105
106/// One of the ways a joint may let its second frame move on its first:
107/// turning about the first frame's `z` axis, or sliding along its `z`, `x`
108/// or `y` axis. Each it frees is a **coordinate** of the joint, called as
109/// it serializes ([`Motion::name`]).
110#[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord, Hash, Serialize, Deserialize)]
111pub enum Motion {
112    #[serde(rename = "angle")]
113    Turn,
114    #[serde(rename = "distance")]
115    Slide,
116    #[serde(rename = "x")]
117    SlideX,
118    #[serde(rename = "y")]
119    SlideY,
120}
121
122/// How many motions there are: what a joint's coordinates are indexed by.
123pub const MOTIONS: usize = 4;
124
125impl Motion {
126    pub const ALL: [Motion; MOTIONS] =
127        [Motion::Turn, Motion::Slide, Motion::SlideX, Motion::SlideY];
128
129    /// Its place in [`Motion::ALL`]: what a joint's coordinates and limits
130    /// are indexed by.
131    pub fn index(self) -> usize {
132        self as usize
133    }
134
135    /// What its coordinate is called: `angle` (in degrees), `distance` —
136    /// along the axis — `x`, `y`.
137    pub fn name(self) -> &'static str {
138        match self {
139            Motion::Turn => "angle",
140            Motion::Slide => "distance",
141            Motion::SlideX => "x",
142            Motion::SlideY => "y",
143        }
144    }
145
146    /// What its coordinate measures, as a user sets it.
147    pub fn measure(self) -> crate::solving::Measure {
148        match self {
149            Motion::Turn => crate::solving::Measure::Angle,
150            Motion::Slide | Motion::SlideX | Motion::SlideY => crate::solving::Measure::Distance,
151        }
152    }
153
154    /// What a joint that frees it does: `turn`, `slide`.
155    pub fn verb(self) -> &'static str {
156        match self {
157            Motion::Turn => "turn",
158            Motion::Slide | Motion::SlideX | Motion::SlideY => "slide",
159        }
160    }
161
162    /// A coordinate of it in its natural units — radians for an angle,
163    /// given in degrees; a length for a distance.
164    pub fn natural(self, value: D) -> GeopResult<D> {
165        match self {
166            Motion::Turn => value.degrees_to_radians(),
167            Motion::Slide | Motion::SlideX | Motion::SlideY => Ok(value),
168        }
169    }
170}
171
172/// How a joint lets its second frame turn against its first.
173#[derive(Clone, Copy, Debug, PartialEq, Eq)]
174pub enum Turning {
175    /// Not at all: the frames' axes keep where they are to each other.
176    Not,
177    /// About the first frame's `z` axis, by the joint's angle.
178    AboutZ,
179    /// About the first frame's `z` axis and about the second's, which are
180    /// kept square to each other: the cross of a universal joint. How far
181    /// it is turned is no coordinate: two angles of a cross go singular
182    /// where it folds over, as Euler angles do (see AGENTS.md, "Degenerate
183    /// parametrizations").
184    Cross,
185    /// Any way about the second frame's origin. No coordinate either, for
186    /// the same reason.
187    Free,
188}
189
190/// The joints there are, each a row of one table — how it lets the second
191/// frame turn, and which motions it frees ([`Joint::frees`]): every joint
192/// is the second frame held on the first within those, so that one
193/// formulation ([`joint_residuals`]) holds them all.
194#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash, Serialize, Deserialize)]
195#[serde(rename_all = "snake_case")]
196pub enum Joint {
197    /// Turns about the axis, nothing else: a hinge, a robot's joint.
198    Revolute,
199    /// Slides along the axis, nothing else.
200    Slider,
201    /// Turns about the axis and slides along it: a shaft in a bearing.
202    Cylindrical,
203    /// Slides in the first frame's `xy` plane and turns about its `z`: a
204    /// part sliding on a table.
205    Planar,
206    /// Slides along the first frame's `x` axis and turns about its `z`: a
207    /// pin in a straight slot.
208    PinSlot,
209    /// Turns any way about the frames' common origin: a ball in its socket.
210    Ball,
211    /// Turns about either frame's `z` axis, the two kept square and meeting
212    /// at the origins: a universal joint's cross.
213    Universal,
214    /// Neither: the frames coincide, and the two bodies move as one.
215    Fastened,
216}
217
218impl Joint {
219    /// Every joint, in the order they are offered.
220    pub const ALL: [Joint; 8] = [
221        Joint::Revolute,
222        Joint::Slider,
223        Joint::Cylindrical,
224        Joint::Planar,
225        Joint::PinSlot,
226        Joint::Ball,
227        Joint::Universal,
228        Joint::Fastened,
229    ];
230
231    /// The table: how it lets the second frame turn, and the motions it
232    /// frees as coordinates — the turn about `z` among them where it turns
233    /// about `z`.
234    pub fn frees(self) -> (Turning, &'static [Motion]) {
235        use Motion::*;
236        match self {
237            Joint::Revolute => (Turning::AboutZ, &[Turn]),
238            Joint::Slider => (Turning::Not, &[Slide]),
239            Joint::Cylindrical => (Turning::AboutZ, &[Turn, Slide]),
240            Joint::Planar => (Turning::AboutZ, &[Turn, SlideX, SlideY]),
241            Joint::PinSlot => (Turning::AboutZ, &[Turn, SlideX]),
242            Joint::Ball => (Turning::Free, &[]),
243            Joint::Universal => (Turning::Cross, &[]),
244            Joint::Fastened => (Turning::Not, &[]),
245        }
246    }
247
248    /// Its name, as it serializes: `pin_slot`.
249    pub fn name(self) -> &'static str {
250        match self {
251            Joint::Revolute => "revolute",
252            Joint::Slider => "slider",
253            Joint::Cylindrical => "cylindrical",
254            Joint::Planar => "planar",
255            Joint::PinSlot => "pin_slot",
256            Joint::Ball => "ball",
257            Joint::Universal => "universal",
258            Joint::Fastened => "fastened",
259        }
260    }
261
262    pub fn label(self) -> &'static str {
263        match self {
264            Joint::Revolute => "Revolute",
265            Joint::Slider => "Slider",
266            Joint::Cylindrical => "Cylindrical",
267            Joint::Planar => "Planar",
268            Joint::PinSlot => "Pin in slot",
269            Joint::Ball => "Ball",
270            Joint::Universal => "Universal",
271            Joint::Fastened => "Fastened",
272        }
273    }
274
275    /// What it does, in words, as the editor offers it.
276    pub fn doc(self) -> &'static str {
277        match self {
278            Joint::Revolute => {
279                "The second part turns about the first one's frame's z axis, and nothing else — a hinge: pick a frame on each part."
280            }
281            Joint::Slider => {
282                "The second part slides along the first one's frame's z axis, and nothing else: pick a frame on each part."
283            }
284            Joint::Cylindrical => {
285                "The second part turns about the first one's frame's z axis and slides along it — a shaft in a bearing."
286            }
287            Joint::Planar => {
288                "The second part slides in the plane of the first one's frame — along its x and y — and turns about its z: a part on a table."
289            }
290            Joint::PinSlot => {
291                "The second part slides along the first one's frame's x axis, the red arrow, and turns about its z: a pin in a straight slot."
292            }
293            Joint::Ball => {
294                "The second part turns any way about the first one's frame's origin, the two origins one: a ball in its socket."
295            }
296            Joint::Universal => {
297                "The second part turns about the first one's frame's z axis and about its own, the two kept square and meeting: a universal joint's cross."
298            }
299            Joint::Fastened => {
300                "The two parts are held together, their frames one: pick a frame on each part."
301            }
302        }
303    }
304}
305
306/// The limits a joint's coordinate is kept within, either missing — an
307/// angle's in degrees.
308#[derive(Clone, Copy, Debug, Default, PartialEq, Serialize, Deserialize)]
309pub struct Limits {
310    #[serde(
311        default,
312        with = "as_f64::option",
313        skip_serializing_if = "Option::is_none"
314    )]
315    pub min: Option<Scalar>,
316    #[serde(
317        default,
318        with = "as_f64::option",
319        skip_serializing_if = "Option::is_none"
320    )]
321    pub max: Option<Scalar>,
322}
323
324impl Limits {
325    /// No limit either way.
326    pub const NONE: Limits = Limits {
327        min: None,
328        max: None,
329    };
330
331    /// Within `min` and `max`.
332    pub fn between(min: Scalar, max: Scalar) -> Self {
333        Limits {
334            min: Some(min),
335            max: Some(max),
336        }
337    }
338
339    pub fn is_none(&self) -> bool {
340        self.min.is_none() && self.max.is_none()
341    }
342}
343
344/// How a joint lets its second body move against its first: which joint
345/// of the table it is ([`Joint`]), and the limits of its coordinates.
346/// Serializes flat, the limits of only those limited, by name: `{"type":
347/// "revolute", "limits": {"angle": {"min": -90.0, "max": 90.0}}}`.
348#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize)]
349#[serde(from = "JointKindFile", into = "JointKindFile")]
350pub struct JointKind {
351    pub joint: Joint,
352    /// The limits of each coordinate, by [`Motion::index`].
353    pub limits: [Limits; MOTIONS],
354}
355
356/// [`JointKind`] as a file has it.
357#[derive(Serialize, Deserialize)]
358struct JointKindFile {
359    #[serde(rename = "type")]
360    joint: Joint,
361    #[serde(default, skip_serializing_if = "BTreeMap::is_empty")]
362    limits: BTreeMap<Motion, Limits>,
363}
364
365impl From<JointKindFile> for JointKind {
366    fn from(file: JointKindFile) -> Self {
367        let mut kind = JointKind::new(file.joint);
368        for (motion, limits) in file.limits {
369            kind.limits[motion.index()] = limits;
370        }
371        kind
372    }
373}
374
375impl From<JointKind> for JointKindFile {
376    fn from(kind: JointKind) -> Self {
377        JointKindFile {
378            joint: kind.joint,
379            limits: (Motion::ALL.into_iter())
380                .map(|m| (m, kind.limits[m.index()]))
381                .filter(|(_, limits)| !limits.is_none())
382                .collect(),
383        }
384    }
385}
386
387impl JointKind {
388    /// The joint `joint`, unlimited.
389    pub const fn new(joint: Joint) -> Self {
390        JointKind {
391            joint,
392            limits: [Limits::NONE; MOTIONS],
393        }
394    }
395
396    /// It with the coordinate of `motion` kept within `limits`.
397    pub fn limited(mut self, motion: Motion, limits: Limits) -> Self {
398        self.limits[motion.index()] = limits;
399        self
400    }
401
402    /// How it lets the second frame turn (see [`Joint::frees`]).
403    pub fn turning(&self) -> Turning {
404        self.joint.frees().0
405    }
406
407    /// Whether it frees `motion`, as a coordinate.
408    pub fn moves(&self, motion: Motion) -> bool {
409        self.joint.frees().1.contains(&motion)
410    }
411
412    /// The motions it frees as coordinates.
413    pub fn motions(&self) -> Vec<Motion> {
414        self.joint.frees().1.to_vec()
415    }
416
417    /// The limits of `motion`'s coordinate, `[min, max]`, either missing.
418    pub fn limits(&self, motion: Motion) -> [Option<Scalar>; 2] {
419        let Limits { min, max } = self.limits[motion.index()];
420        [min, max]
421    }
422
423    pub fn label(&self) -> &'static str {
424        self.joint.label()
425    }
426
427    /// Checks it limits only coordinates it frees, and each the right way
428    /// round.
429    pub fn validate(&self) -> GeopResult<()> {
430        let label = self.label().to_lowercase();
431        for motion in Motion::ALL {
432            let [min, max] = self.limits(motion);
433            if !self.moves(motion) && (min.is_some() || max.is_some()) {
434                return Err(GeopError::new(format!(
435                    "a {label} joint has no {} to limit: it frees {}",
436                    motion.name(),
437                    describe_motions(&self.motions()),
438                )));
439            }
440            if let (Some(min), Some(max)) = (min, max)
441                && min.definitely_greater(max)
442            {
443                return Err(GeopError::new(format!(
444                    "the {} limits of a {label} joint are the wrong way round: {min:?} > {max:?}",
445                    motion.name(),
446                )));
447            }
448        }
449        Ok(())
450    }
451}
452
453/// `motions` in words: `angle and distance`, or `no coordinate`.
454fn describe_motions(motions: &[Motion]) -> String {
455    match motions {
456        [] => "no coordinate".to_string(),
457        [one] => one.name().to_string(),
458        [rest @ .., last] => {
459            let rest: Vec<&str> = rest.iter().map(|m| m.name()).collect();
460            format!("{} and {}", rest.join(", "), last.name())
461        }
462    }
463}
464
465/// A joint's coordinates where they are, by [`Motion::index`]: zero for a
466/// motion it does not free.
467pub type Coordinates = [D; MOTIONS];
468
469/// The residuals of a joint of `kind` between `a` — the first part's
470/// connector — and `b`, at the coordinates `at` (an angle in degrees),
471/// where they are: each a length, a direction's scaled by the part's size
472/// `scale`.
473///
474/// `b`'s origin is `a`'s moved along `a`'s axis by the distance, and along
475/// its reference and the direction square to both by `x` and `y`; and as
476/// the joint turns, `b`'s axis is parallel to `a`'s — facing either way, as
477/// a pin in a hole may — with `b`'s reference `a`'s turned about `a`'s axis
478/// by the angle (zero for a joint that does not turn), or `b`'s axis square
479/// to `a`'s for a cross, or turned any way for a ball. Fails — infeasible —
480/// where `b` is turned half a turn from what the angle says.
481pub fn joint_residuals(
482    kind: &JointKind,
483    a: &Connector<D>,
484    b: &Connector<D>,
485    at: &Coordinates,
486    scale: D,
487) -> GeopResult<Vec<D>> {
488    let (za, ra) = (&a.axis, &a.reference);
489    let l = scale;
490    let ya = za.prod_cross(ra);
491    let mut origin = a.origin.add(&za.prod_scalar(at[Motion::Slide.index()]));
492    // Only a joint that slides across its axis adds those slides: an exact
493    // zero added rounds a hair outward.
494    for (motion, along) in [(Motion::SlideX, ra), (Motion::SlideY, &ya)] {
495        if kind.moves(motion) {
496            origin = origin.add(&along.prod_scalar(at[motion.index()]));
497        }
498    }
499    let mut out: Vec<D> = b.origin.sub(&origin).to_array().to_vec();
500    match kind.turning() {
501        Turning::Not | Turning::AboutZ => {
502            // The axes parallel: `b`'s axis has no part along `a`'s
503            // reference or the direction square to both.
504            out.extend(parallel(&[*ra, ya], &b.axis, l));
505            // How far `b`'s reference is turned past the angle, `φ - θ`, as
506            // `2 tan((φ - θ) / 2)` from its sine and cosine: zero only at
507            // `φ = θ` — a sine alone is zero half a turn away too — and one
508            // row, of slope one there. Measured as `rb - u`, `b`'s reference
509            // less `a`'s turned by the angle, the part of it along `u` would
510            // vanish to second order only: a row that, at a solution the
511            // minimizer reached to its tolerance, has a slope as small as
512            // that tolerance and still counts as a constraint, pinning the
513            // joint.
514            let angle = Motion::Turn.natural(at[Motion::Turn.index()])?;
515            let (cos, sin) = (angle.cos(), angle.sin());
516            let u = ra.prod_scalar(cos).add(&ya.prod_scalar(sin));
517            let t = ya.prod_scalar(cos).sub(&ra.prod_scalar(sin));
518            let past = b
519                .reference
520                .prod_dot(&t)
521                .div(D::ONE.add(b.reference.prod_dot(&u)))?;
522            out.push(past.mul(D::TWO).mul(l));
523        }
524        // One row, of slope one where the axes are square.
525        Turning::Cross => out.push(za.prod_dot(&b.axis).mul(l)),
526        Turning::Free => {}
527    }
528    Ok(out)
529}
530
531/// Where the coordinates of a joint between `a` and `b` are, measured from
532/// where its ends are, by [`Motion::index`]: the angle `b`'s reference is
533/// turned from `a`'s about `a`'s axis, in degrees in `(-180, 180]`, and how
534/// far `b`'s origin is along `a`'s axis, its reference and the direction
535/// square to both. Only a seed for a solve, which finds the exact values:
536/// read off midpoints.
537pub fn measure(a: &Connector<D>, b: &Connector<D>) -> [f64; MOTIONS] {
538    let f = |v: &Vector3<D>| v.to_array().map(|x| x.to_f64());
539    let (oa, za, ra, ob, rb) = (
540        f(&a.origin),
541        f(&a.axis),
542        f(&a.reference),
543        f(&b.origin),
544        f(&b.reference),
545    );
546    let ya = cross(za, ra);
547    let off = sub(ob, oa);
548    let angle = dot(rb, ya).atan2(dot(rb, ra)).to_degrees();
549    [angle, dot(off, za), dot(off, ra), dot(off, ya)]
550}
551
552/// What a coupling transmits between its two joints: the motions of each
553/// it ties ([`Transmission::motions`]) and what its value is
554/// ([`CouplingKind::factor`]) — presets of one linear relation.
555#[derive(Clone, Copy, Debug, PartialEq, Eq, Serialize, Deserialize)]
556#[serde(rename_all = "snake_case")]
557pub enum Transmission {
558    /// Two turning joints: the first turns `value` times — the ratio — for
559    /// every turn of the second. A pinion of 10 teeth driving a wheel of 20
560    /// is a ratio of 2; a pair of external gears turns opposite ways.
561    Gear,
562    /// Two turning joints, as for a gear, by a belt or a chain round two
563    /// pulleys: they turn the same way.
564    Belt,
565    /// A turning joint, the pinion, and a sliding one, the rack: the rack
566    /// moves the length of arc the pinion's pitch circle, of radius
567    /// `value`, rolls off.
568    RackPinion,
569    /// A turning joint and a sliding one — the turn and the slide of one
570    /// cylindrical joint, usually: every turn moves it by `value`, the
571    /// lead.
572    Screw,
573    /// Two sliding joints: the second slides `value` times as far as the
574    /// first — a lever's two ends, a cable round a pulley.
575    Linear,
576}
577
578impl Transmission {
579    /// Every transmission, as offered.
580    pub const ALL: [Transmission; 5] = [
581        Transmission::Gear,
582        Transmission::Belt,
583        Transmission::RackPinion,
584        Transmission::Screw,
585        Transmission::Linear,
586    ];
587
588    /// The motions of the two joints it ties.
589    pub fn motions(self) -> [Motion; 2] {
590        match self {
591            Transmission::Gear | Transmission::Belt => [Motion::Turn, Motion::Turn],
592            Transmission::RackPinion | Transmission::Screw => [Motion::Turn, Motion::Slide],
593            Transmission::Linear => [Motion::Slide, Motion::Slide],
594        }
595    }
596
597    pub fn label(self) -> &'static str {
598        match self {
599            Transmission::Gear => "Gear",
600            Transmission::Belt => "Belt or chain",
601            Transmission::RackPinion => "Rack and pinion",
602            Transmission::Screw => "Screw",
603            Transmission::Linear => "Linear",
604        }
605    }
606}
607
608/// How a coupling ties its two joints' coordinates: the second's is the
609/// first's times a factor — given by `value` as the transmission says,
610/// negated if `reverse` — plus `offset`, in the second's units, degrees for
611/// an angle. Serializes flat: `{"type": "gear", "value": 2.0, "reverse":
612/// true}`.
613#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize)]
614pub struct CouplingKind {
615    #[serde(rename = "type")]
616    pub transmission: Transmission,
617    #[serde(with = "as_f64")]
618    pub value: Scalar,
619    #[serde(default)]
620    pub reverse: bool,
621    #[serde(
622        default,
623        with = "as_f64::option",
624        skip_serializing_if = "Option::is_none"
625    )]
626    pub offset: Option<Scalar>,
627}
628
629impl CouplingKind {
630    /// A coupling of `transmission` by `value`, turning the way that
631    /// transmission does unless `reverse`, at no offset.
632    pub fn new(transmission: Transmission, value: Scalar, reverse: bool) -> Self {
633        CouplingKind {
634            transmission,
635            value,
636            reverse,
637            offset: None,
638        }
639    }
640
641    /// The second coordinate per radian or unit of length of the first,
642    /// both in natural units — radians and lengths.
643    pub fn factor(&self) -> GeopResult<Scalar> {
644        let factor = match self.transmission {
645            Transmission::Gear | Transmission::Belt => Scalar::ONE.div(self.value)?,
646            Transmission::RackPinion | Transmission::Linear => self.value,
647            Transmission::Screw => self.value.div(Scalar::TWO.mul(Scalar::PI))?,
648        };
649        Ok(if self.reverse { factor.neg() } else { factor })
650    }
651
652    /// Checks it can tie the motions of one joint and another — `same`
653    /// whether they are the one joint — that the joints free: `frees`
654    /// saying whether a joint does.
655    pub fn validate(&self, same: bool, frees: [bool; 2]) -> GeopResult<()> {
656        let label = self.transmission.label().to_lowercase();
657        let [ma, mb] = self.transmission.motions();
658        if same && ma == mb {
659            return Err(GeopError::new(format!(
660                "a {label} coupling ties a joint's {} to itself",
661                ma.name()
662            )));
663        }
664        for (k, motion) in [ma, mb].into_iter().enumerate() {
665            if !frees[k] {
666                return Err(GeopError::new(format!(
667                    "a {label} coupling needs its {} joint to {}",
668                    ["first", "second"][k],
669                    motion.verb()
670                )));
671            }
672        }
673        let ratio = matches!(self.transmission, Transmission::Gear | Transmission::Belt);
674        if ratio && self.value.could_be_equal(Scalar::ZERO) {
675            return Err(GeopError::new(format!(
676                "a {label} coupling's ratio must not be zero: {:?}",
677                self.value
678            )));
679        }
680        Ok(())
681    }
682
683    /// The residual between the coordinates `a` and `b` of its two joints —
684    /// in their own units, degrees for an angle: the second less the first
685    /// times the factor and the offset, in natural units, as a length — an
686    /// angle's turned into one by the part's size `scale`.
687    pub fn residual(&self, a: D, b: D, scale: D) -> GeopResult<D> {
688        let [ma, mb] = self.transmission.motions();
689        let mut off = mb
690            .natural(b)?
691            .sub(ma.natural(a)?.mul(D::cst(self.factor()?)));
692        // Only one given: an exact zero taken away rounds a hair outward.
693        if let Some(offset) = self.offset {
694            off = off.sub(mb.natural(D::cst(offset))?);
695        }
696        Ok(match mb {
697            Motion::Turn => off.mul(scale),
698            Motion::Slide | Motion::SlideX | Motion::SlideY => off,
699        })
700    }
701}
geop_ops_assembly/joints.rs

geop_ops_assembly/
joints.rs
