tinympc_rs/
project.rs

1use core::ops::Range;
2
3use nalgebra::{RealField, SMatrix, SVector, SVectorViewMut, convert};
4
5use crate::constraint::{Constraint, DynConstraint};
6
7/// Can project a single point into its feasible region.
8pub trait ProjectSingle<T, const D: usize> {
9    /// Apply the projection to a single point.
10    fn project_single(&self, point: SVectorViewMut<T, D>);
11}
12
13impl<T, const D: usize> ProjectSingle<T, D> for &dyn ProjectSingle<T, D> {
14    fn project_single(&self, point: SVectorViewMut<T, D>) {
15        (**self).project_single(point);
16    }
17}
18
19impl<P: ProjectSingle<T, D>, T, const D: usize> ProjectSingle<T, D> for &P {
20    fn project_single(&self, point: SVectorViewMut<T, D>) {
21        (**self).project_single(point);
22    }
23}
24
25impl<T, const D: usize> ProjectSingle<T, D> for () {
26    fn project_single(&self, mut _point: SVectorViewMut<T, D>) {}
27}
28
29macro_rules! derive_tuple_project {
30    ($($project:ident: $number:tt),+) => {
31        impl<$($project: ProjectSingle<T, D>),+, T, const D: usize> ProjectSingle<T, D>
32            for ( $($project,)+ )
33        {
34            fn project_single(&self, mut point: SVectorViewMut<T, D>) {
35                $(
36                    self.$number.project_single(point.as_view_mut());
37                )+
38            }
39        }
40    };
41}
42
43derive_tuple_project! {P0: 0}
44derive_tuple_project! {P0: 0, P1: 1}
45derive_tuple_project! {P0: 0, P1: 1, P2: 2}
46derive_tuple_project! {P0: 0, P1: 1, P2: 2, P3: 3}
47derive_tuple_project! {P0: 0, P1: 1, P2: 2, P3: 3, P4: 4}
48derive_tuple_project! {P0: 0, P1: 1, P2: 2, P3: 3, P4: 4, P5: 5}
49derive_tuple_project! {P0: 0, P1: 1, P2: 2, P3: 3, P4: 4, P5: 5, P6: 6}
50derive_tuple_project! {P0: 0, P1: 1, P2: 2, P3: 3, P4: 4, P5: 5, P6: 6, P7: 7}
51derive_tuple_project! {P0: 0, P1: 1, P2: 2, P3: 3, P4: 4, P5: 5, P6: 6, P7: 7, P8: 8}
52derive_tuple_project! {P0: 0, P1: 1, P2: 2, P3: 3, P4: 4, P5: 5, P6: 6, P7: 7, P8: 8, P9: 9}
53
54pub trait ProjectSingleExt<T: RealField + Copy, const D: usize>:
55    ProjectSingle<T, D> + Sized
56{
57    fn time_fixed(self) -> time::Fixed<Self> {
58        time::Fixed::new(self)
59    }
60
61    fn time_ranged(self, range: Range<usize>) -> time::Ranged<Self> {
62        time::Ranged::new(self, range)
63    }
64
65    fn dim_lift<const N: usize>(self, indices: [usize; D]) -> dim::Lift<Self, D, N> {
66        dim::Lift::new(indices, self)
67    }
68}
69
70impl<P: ProjectSingle<T, D>, T: RealField + Copy, const D: usize> ProjectSingleExt<T, D> for P {}
71
72/// Can project a multiple points into their feasible region.
73pub trait ProjectMulti<T, const D: usize, const H: usize> {
74    fn project_multi(&self, points: &mut SMatrix<T, D, H>);
75}
76
77impl<P: ProjectMulti<T, D, H>, T, const D: usize, const H: usize> ProjectMulti<T, D, H> for &P {
78    fn project_multi(&self, points: &mut SMatrix<T, D, H>) {
79        (**self).project_multi(points);
80    }
81}
82
83impl<T, const D: usize, const H: usize> ProjectMulti<T, D, H> for &dyn ProjectMulti<T, D, H> {
84    fn project_multi(&self, points: &mut SMatrix<T, D, H>) {
85        (**self).project_multi(points);
86    }
87}
88
89impl<T, const D: usize, const H: usize> ProjectMulti<T, D, H> for () {
90    fn project_multi(&self, _points: &mut SMatrix<T, D, H>) {}
91}
92
93macro_rules! derive_tuple_project_multi {
94    ($($project:ident: $number:tt),+) => {
95        impl<$($project: ProjectMulti<T, D, H>),+, T, const D: usize, const H: usize> ProjectMulti<T, D, H>
96            for ( $($project,)+ )
97        {
98            fn project_multi(&self, points: &mut SMatrix<T, D, H>) {
99                $(
100                    self.$number.project_multi(points);
101                )+
102            }
103        }
104    };
105}
106
107derive_tuple_project_multi! {P0: 0}
108derive_tuple_project_multi! {P0: 0, P1: 1}
109derive_tuple_project_multi! {P0: 0, P1: 1, P2: 2}
110derive_tuple_project_multi! {P0: 0, P1: 1, P2: 2, P3: 3}
111derive_tuple_project_multi! {P0: 0, P1: 1, P2: 2, P3: 3, P4: 4}
112derive_tuple_project_multi! {P0: 0, P1: 1, P2: 2, P3: 3, P4: 4, P5: 5}
113derive_tuple_project_multi! {P0: 0, P1: 1, P2: 2, P3: 3, P4: 4, P5: 5, P6: 6}
114derive_tuple_project_multi! {P0: 0, P1: 1, P2: 2, P3: 3, P4: 4, P5: 5, P6: 6, P7: 7}
115derive_tuple_project_multi! {P0: 0, P1: 1, P2: 2, P3: 3, P4: 4, P5: 5, P6: 6, P7: 7, P8: 8}
116derive_tuple_project_multi! {P0: 0, P1: 1, P2: 2, P3: 3, P4: 4, P5: 5, P6: 6, P7: 7, P8: 8, P9: 9}
117
118pub trait ProjectMultiExt<T: RealField + Copy, const D: usize, const H: usize>:
119    ProjectMulti<T, D, H> + Sized
120{
121    fn dynamic(&self) -> &dyn ProjectMulti<T, D, H> {
122        self
123    }
124
125    fn constraint(&self) -> Constraint<T, &Self, D, H> {
126        Constraint::new(self)
127    }
128
129    fn dyn_constraint(&self) -> DynConstraint<'_, T, D, H> {
130        Constraint::new(self)
131    }
132
133    fn constraint_owned(self) -> Constraint<T, Self, D, H> {
134        Constraint::new(self)
135    }
136}
137
138impl<P: ProjectMulti<T, D, H>, T: RealField + Copy, const D: usize, const H: usize>
139    ProjectMultiExt<T, D, H> for P
140{
141}
142
143/// A box projection
144#[derive(Debug, Copy, Clone)]
145pub struct Box<T, const N: usize> {
146    pub lower: SVector<T, N>,
147    pub upper: SVector<T, N>,
148}
149
150impl<T: RealField + Copy, const N: usize> ProjectSingle<T, N> for Box<T, N> {
151    fn project_single(&self, mut point: SVectorViewMut<T, N>) {
152        for n in 0..N {
153            point[n] = point[n].clamp(self.lower[n], self.upper[n]);
154        }
155    }
156}
157
158/// A spherical projection
159#[derive(Debug, Copy, Clone)]
160pub struct Sphere<T, const D: usize> {
161    pub center: SVector<T, D>,
162    pub radius: T,
163}
164
165impl<T: RealField + Copy, const N: usize> ProjectSingle<T, N> for Sphere<T, N> {
166    fn project_single(&self, mut point: SVectorViewMut<T, N>) {
167        if self.radius <= T::zero() {
168            point.copy_from(&self.center);
169        } else {
170            let diff = &point - self.center;
171            let dist = diff.norm();
172
173            if dist > self.radius {
174                let scale = self.radius / dist;
175                point.copy_from(&self.center);
176                point.axpy(scale, &diff, T::one());
177            }
178        }
179    }
180}
181
182/// An anti-spherical projection
183#[derive(Debug, Copy, Clone)]
184pub struct AntiSphere<T, const N: usize> {
185    pub center: SVector<T, N>,
186    pub radius: T,
187}
188
189impl<T: RealField + Copy, const N: usize> ProjectSingle<T, N> for AntiSphere<T, N> {
190    fn project_single(&self, mut point: SVectorViewMut<T, N>) {
191        if self.radius > T::zero() {
192            let diff = &point - self.center;
193            let dist = diff.norm();
194
195            if dist < self.radius {
196                let scale = self.radius / dist;
197                point.copy_from(&(self.center + diff * scale));
198            }
199        }
200    }
201}
202
203/// A half-space projection
204#[derive(Debug, Copy, Clone)]
205pub struct Affine<T, const N: usize> {
206    normal: SVector<T, N>,
207    distance: T,
208}
209
210impl<T: RealField + Copy, const D: usize> Default for Affine<T, D> {
211    fn default() -> Affine<T, D> {
212        Affine {
213            normal: SVector::identity(),
214            distance: T::zero(),
215        }
216    }
217}
218
219impl<T: RealField + Copy, const D: usize> Affine<T, D> {
220    /// Create a new [`Affine`] projector with default values.
221    #[must_use]
222    pub fn new() -> Affine<T, D> {
223        Affine::default()
224    }
225
226    /// Set the axis along the center of the cone.
227    ///
228    /// # Panics
229    ///
230    /// If the provided vector has no magnitude.
231    #[must_use]
232    pub fn normal(mut self, normal: impl Into<SVector<T, D>>) -> Self {
233        self.normal = normal.into();
234        assert!(self.normal.norm() > convert(1e-9));
235        self.normal = self.normal.normalize();
236        self
237    }
238
239    /// Set the affine projectors offset distance
240    #[must_use]
241    pub fn distance(mut self, distance: T) -> Self {
242        self.distance = distance;
243        self
244    }
245}
246
247impl<T: RealField + Copy, const N: usize> ProjectSingle<T, N> for Affine<T, N> {
248    fn project_single(&self, mut point: SVectorViewMut<T, N>) {
249        let dot = point.dot(&self.normal);
250        if dot > self.distance {
251            point -= self.normal.scale(dot - self.distance);
252        }
253    }
254}
255
256/// A circular cone projection
257#[derive(Debug, Clone)]
258pub struct CircularCone<T: RealField + Copy, const D: usize> {
259    vertex: SVector<T, D>,
260    axis: SVector<T, D>,
261    mu: T,
262}
263
264impl<T: RealField + Copy, const D: usize> Default for CircularCone<T, D> {
265    fn default() -> Self {
266        CircularCone {
267            vertex: SVector::zeros(),
268            axis: SVector::identity(),
269            mu: nalgebra::convert(1.0),
270        }
271    }
272}
273
274impl<T: RealField + Copy, const D: usize> CircularCone<T, D> {
275    /// Create a new [`CircularCone`] projector with default values.
276    #[must_use]
277    pub fn new() -> CircularCone<T, D> {
278        CircularCone::default()
279    }
280
281    /// Set the axis along the center of the cone.
282    #[must_use]
283    pub fn axis(mut self, axis: impl Into<SVector<T, D>>) -> Self {
284        self.mut_axis(axis);
285        self
286    }
287
288    /// Set the axis along the center of the cone.
289    pub fn mut_axis(&mut self, axis: impl Into<SVector<T, D>>) {
290        self.axis = axis.into().normalize();
291    }
292
293    /// Set the coordinate of the cones vertex / tip.
294    #[must_use]
295    pub fn vertex(mut self, vertex: impl Into<SVector<T, D>>) -> Self {
296        self.mut_vertex(vertex);
297        self
298    }
299
300    /// Set the coordinate of the cones vertex / tip.
301    pub fn mut_vertex(&mut self, vertex: impl Into<SVector<T, D>>) {
302        self.vertex = vertex.into();
303    }
304
305    /// Set the `µ` value, or the "aperture" of the cone.
306    #[must_use]
307    pub fn mu(mut self, mu: T) -> Self {
308        self.mut_mu(mu);
309        self
310    }
311
312    /// Set the `µ` value, or the "aperture" of the cone.
313    pub fn mut_mu(&mut self, mu: T) {
314        self.mu = mu.max(T::zero());
315    }
316}
317
318impl<T: RealField + Copy, const D: usize> ProjectSingle<T, D> for CircularCone<T, D> {
319    fn project_single(&self, mut point: SVectorViewMut<T, D>) {
320        // Translate by the tip to get vector v
321        let v = &point - self.vertex;
322
323        // Decompose v into parallel and orthogonal components
324        let s_n = v.dot(&self.axis);
325        let s_v = v - self.axis.scale(s_n);
326
327        // The radial distance
328        let a = s_v.norm();
329
330        // Inside feasible region, do nothing
331        if a <= self.mu * s_n {
332        }
333        // Inside polar cone, project to tip
334        else if (a * self.mu <= -s_n) || a.is_zero() {
335            point.copy_from(&self.vertex);
336        }
337        // Outside both, project onto boundary
338        else {
339            let alpha = (self.mu * a + s_n) / (T::one() + self.mu * self.mu);
340            point.copy_from(&((self.axis + s_v * self.mu / a) * alpha + self.vertex));
341        }
342    }
343}
344
345/// Module for dimension modifiers for projectors
346pub mod dim {
347    use core::marker::PhantomData;
348
349    use nalgebra::{RealField, SVector, SVectorViewMut};
350
351    use crate::ProjectSingle;
352
353    /// Lift the projection into a higher dimensional space.
354    #[derive(Debug, Clone)]
355    pub struct Lift<P, const D: usize, const N: usize> {
356        indices: [usize; D],
357        pub projector: P,
358        _p: PhantomData<[(); N]>,
359    }
360
361    impl<P, const D: usize, const N: usize> Lift<P, D, N> {
362        /// Lift the provided projector into a higher-dimensional space
363        ///
364        /// # Panics
365        ///
366        /// If any of the provided indeces exceed the higher dimension of `N`
367        pub fn new(indices: [usize; D], projector: P) -> Self {
368            assert!(indices.iter().all(|e| e < &N));
369            Self {
370                indices,
371                projector,
372                _p: PhantomData,
373            }
374        }
375    }
376
377    impl<P: ProjectSingle<T, D>, T: RealField + Copy, const D: usize, const N: usize>
378        ProjectSingle<T, N> for Lift<P, D, N>
379    {
380        fn project_single(&self, mut point: SVectorViewMut<T, N>) {
381            let mut sub_point: SVector<T, D> = SVector::zeros();
382            for i in 0..D {
383                sub_point[i] = point[self.indices[i]];
384            }
385
386            self.projector.project_single(sub_point.as_view_mut());
387
388            for i in 0..D {
389                point[self.indices[i]] = sub_point[i];
390            }
391        }
392    }
393}
394
395/// Module for time modifiers for projectors
396pub mod time {
397    use core::ops::Range;
398
399    use nalgebra::{RealField, SMatrix};
400
401    use crate::{ProjectMulti, ProjectSingle};
402
403    /// Apply the projector `P` to be fixed throughout the entire horizon.
404    pub struct Fixed<P> {
405        pub projector: P,
406    }
407
408    impl<P> Fixed<P> {
409        pub fn new(projector: P) -> Fixed<P> {
410            Fixed { projector }
411        }
412    }
413
414    impl<P: ProjectSingle<T, D>, T: RealField + Copy, const D: usize, const H: usize>
415        ProjectMulti<T, D, H> for Fixed<P>
416    {
417        fn project_multi(&self, points: &mut SMatrix<T, D, H>) {
418            for mut column in points.column_iter_mut() {
419                self.projector.project_single(column.as_view_mut());
420            }
421        }
422    }
423
424    /// Apply the projector `P` to be fixed across a range of the horizon.
425    pub struct Ranged<P> {
426        pub projector: P,
427        pub range: Range<usize>,
428    }
429
430    impl<P> Ranged<P> {
431        pub fn new(projector: P, range: Range<usize>) -> Ranged<P> {
432            Ranged { projector, range }
433        }
434    }
435
436    impl<P: ProjectSingle<T, D>, T: RealField + Copy, const D: usize, const H: usize>
437        ProjectMulti<T, D, H> for Ranged<P>
438    {
439        fn project_multi(&self, points: &mut SMatrix<T, D, H>) {
440            for mut column in points
441                .column_iter_mut()
442                .take(self.range.end)
443                .skip(self.range.start)
444            {
445                self.projector.project_single(column.as_view_mut());
446            }
447        }
448    }
449
450    /// Yields a projector for a given index in the time horizon
451    pub struct Func<F> {
452        func: F,
453    }
454
455    impl<F> Func<F> {
456        pub fn new(func: F) -> Func<F> {
457            Func { func }
458        }
459    }
460
461    impl<
462        P: ProjectSingle<T, D>,
463        T: RealField + Copy,
464        F: Fn(usize) -> P,
465        const D: usize,
466        const H: usize,
467    > ProjectMulti<T, D, H> for Func<F>
468    {
469        fn project_multi(&self, points: &mut SMatrix<T, D, H>) {
470            for (index, mut column) in points.column_iter_mut().enumerate() {
471                (self.func)(index).project_single(column.as_view_mut());
472            }
473        }
474    }
475}
tinympc_rs/project.rs

tinympc_rs/
project.rs
