deke_multipath/
reqpath.rs

1use deke_types::SRobotPath;
2
3use crate::error::{MultipathError, MultipathResult};
4
5/// A path that must be traversed exactly once, with the directions or variants
6/// the solver is free to choose between. Every variant collapses to a set of
7/// directed option paths; the solver picks exactly one option per `ReqPath`.
8pub enum ReqPath<const N: usize> {
9    /// Must be traversed in the given direction.
10    OneWay(SRobotPath<N, f64>),
11    /// May be traversed forwards or backwards; the solver picks the cheaper
12    /// orientation. The reverse is the waypoint-reversed path.
13    Reversible(SRobotPath<N, f64>),
14    /// Forwards and backwards are *distinct* pre-built realizations (e.g. a
15    /// different joint resolution for the return direction). The solver picks
16    /// one.
17    BothWays(SRobotPath<N, f64>, SRobotPath<N, f64>),
18    /// Arbitrary alternative realizations of the same required motion (e.g.
19    /// redundant linear-axis variants). Must be non-empty.
20    ManyWays(Vec<SRobotPath<N, f64>>),
21}
22
23/// One directed candidate for a required path. Its start is `path.first()` and
24/// its end is `path.last()`. `cluster` ties it back to the `ReqPath` it came
25/// from so the solver visits each required path exactly once.
26pub(crate) struct DirectedOption<const N: usize> {
27    pub path: SRobotPath<N, f64>,
28    pub cluster: usize,
29}
30
31/// Flatten the required paths into directed options tagged by cluster index.
32/// Returns the options and the cluster count. Errors if any `ManyWays` cluster
33/// is empty.
34pub(crate) fn expand<const N: usize>(
35    req_paths: &[ReqPath<N>],
36) -> MultipathResult<(Vec<DirectedOption<N>>, usize)> {
37    let mut options = Vec::new();
38    for (cluster, req) in req_paths.iter().enumerate() {
39        let before = options.len();
40        match req {
41            ReqPath::OneWay(p) => options.push(DirectedOption {
42                path: p.clone(),
43                cluster,
44            }),
45            ReqPath::Reversible(p) => {
46                options.push(DirectedOption {
47                    path: p.clone(),
48                    cluster,
49                });
50                options.push(DirectedOption {
51                    path: p.reversed(),
52                    cluster,
53                });
54            }
55            ReqPath::BothWays(a, b) => {
56                options.push(DirectedOption {
57                    path: a.clone(),
58                    cluster,
59                });
60                options.push(DirectedOption {
61                    path: b.clone(),
62                    cluster,
63                });
64            }
65            ReqPath::ManyWays(ps) => {
66                for p in ps {
67                    options.push(DirectedOption {
68                        path: p.clone(),
69                        cluster,
70                    });
71                }
72            }
73        }
74        if options.len() == before {
75            return Err(MultipathError::EmptyOptions(cluster));
76        }
77    }
78    Ok((options, req_paths.len()))
79}
deke_multipath/reqpath.rs

deke_multipath/
reqpath.rs
