geop_ops_model/placed/
motion.rs

1//! Motion studies: joint coordinates driven by a time table
2//! ([`MotionStudy`]), each a step's ([`motion_study`]), read in the order
3//! the steps made them ([`motion_studies`]) — by the editor, which plays
4//! one by setting the coordinates to their values at a time and solving,
5//! and by whatever plays, checks or exports a motion.
6
7use geop_core_math::geop_error::{GeopError, GeopResult};
8use geop_core_program::{
9    element::{ElementReference, HasElements, Kind, whole},
10    reader::Reader,
11    target::{Target, TargetReference},
12};
13use serde::{Deserialize, Serialize};
14
15/// Joint coordinates driven by a time table: their values at keyframes,
16/// and between two keyframes the straight line between them. A motion runs
17/// from its first keyframe's time to its last's, and is played, checked
18/// and exported at `frames` times evenly apart over that.
19#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
20pub struct MotionStudy {
21    /// The coordinates driven, by the names of their states:
22    /// `add_part(fore,m1).angle`.
23    pub drivers: Vec<String>,
24    /// The keyframes, each its time — in seconds — and then a value per
25    /// driver, in its own units: degrees for an angle.
26    pub keys: Vec<Vec<f64>>,
27    /// How many frames it is played at, checked and exported at: at least
28    /// two, its first keyframe and its last.
29    pub frames: usize,
30}
31
32impl MotionStudy {
33    /// Checks it is one: keyframes a time and a value for every driver
34    /// each, later each than the one before, and frames enough to start
35    /// and end.
36    pub fn validate(&self) -> GeopResult<()> {
37        if self.keys.is_empty() {
38            return Err(GeopError::new("a motion needs a keyframe: add a row"));
39        }
40        for (k, key) in self.keys.iter().enumerate() {
41            if key.len() != self.drivers.len() + 1 {
42                return Err(GeopError::new(format!(
43                    "keyframe {} has {} values for {} drivers and its time",
44                    k + 1,
45                    key.len(),
46                    self.drivers.len()
47                )));
48            }
49            if key.iter().any(|v| !v.is_finite()) {
50                return Err(GeopError::new(format!(
51                    "keyframe {} holds a value that is no number: {key:?}",
52                    k + 1
53                )));
54            }
55        }
56        for (k, pair) in self.keys.windows(2).enumerate() {
57            if pair[1][0] <= pair[0][0] {
58                return Err(GeopError::new(format!(
59                    "keyframe {} is at {} s, no later than the one before it at {} s",
60                    k + 2,
61                    pair[1][0],
62                    pair[0][0]
63                )));
64            }
65        }
66        if self.frames < 2 {
67            return Err(GeopError::new(format!(
68                "a motion is played at its start and its end at least: 2 frames, not {}",
69                self.frames
70            )));
71        }
72        Ok(())
73    }
74
75    /// When it starts and ends, in seconds.
76    pub fn span(&self) -> (f64, f64) {
77        let time = |key: Option<&Vec<f64>>| key.and_then(|k| k.first().copied());
78        let start = time(self.keys.first()).unwrap_or(0.0);
79        (start, time(self.keys.last()).unwrap_or(start))
80    }
81
82    /// The value of each driver at the time `t`, by driver: between two
83    /// keyframes on the line between them, before the first its first, and
84    /// after the last its last.
85    pub fn at(&self, t: f64) -> Vec<(String, f64)> {
86        let after = self.keys.iter().position(|key| key[0] > t);
87        let value = |d: usize| match after {
88            Some(0) => self.keys[0][d + 1],
89            None => self.keys.last().map_or(0.0, |key| key[d + 1]),
90            Some(k) => {
91                let (a, b) = (&self.keys[k - 1], &self.keys[k]);
92                let s = (t - a[0]) / (b[0] - a[0]);
93                a[d + 1] + s * (b[d + 1] - a[d + 1])
94            }
95        };
96        (self.drivers.iter().enumerate())
97            .map(|(d, driver)| (driver.clone(), value(d)))
98            .collect()
99    }
100
101    /// The times of its frames: evenly apart from its start to its end.
102    pub fn times(&self) -> Vec<f64> {
103        let (start, end) = self.span();
104        let frames = self.frames.max(2);
105        (0..frames)
106            .map(|k| start + (end - start) * k as f64 / (frames - 1) as f64)
107            .collect()
108    }
109}
110
111/// Each motion step's own.
112impl Target for MotionStudy {}
113
114/// Read whole.
115impl Kind for MotionStudy {
116    const NAME: &'static str = "motion";
117}
118
119/// What the target of a motion study is named, before the step's id.
120const PREFIX: &str = "motion:";
121
122impl HasElements<MotionStudy> for MotionStudy {
123    type Element<'a> = &'a MotionStudy;
124
125    fn element(&self, name: &str) -> GeopResult<&MotionStudy> {
126        whole(self, name)
127    }
128
129    fn names_target(name: &str) -> bool {
130        name.starts_with(PREFIX)
131    }
132}
133
134/// The motion study the step `step` makes.
135pub fn motion_study(step: &str) -> TargetReference<MotionStudy> {
136    TargetReference::new(format!("{PREFIX}{step}"))
137}
138
139/// Every motion study of the part `reader` reads, by the id of the step
140/// that made it, in the order the steps made them.
141pub fn motion_studies<'a>(reader: &Reader<'a>) -> Vec<(String, &'a MotionStudy)> {
142    (reader.targets_of::<MotionStudy>().into_iter())
143        .filter_map(|target| {
144            let step = target.name().strip_prefix(PREFIX)?.to_string();
145            let study = reader
146                .retrieve_element(&ElementReference::whole(target))
147                .ok()?;
148            Some((step, study))
149        })
150        .collect()
151}
geop_ops_model/placed/motion.rs

geop_ops_model/placed/
motion.rs
