tinympc_rs/
project.rs

1use nalgebra::{RealField, SMatrix, SVector};
2
3use crate::constraint::{Constraint, DynConstraint};
4
5/// Can project a series of points into their feasible region.
6pub trait Project<T, const N: usize, const H: usize> {
7    /// Applies the projection to a series of points, modifying them in place
8    fn project(&self, points: &mut SMatrix<T, N, H>);
9}
10
11impl<T, const N: usize, const H: usize> Project<T, N, H> for &dyn Project<T, N, H> {
12    fn project(&self, points: &mut SMatrix<T, N, H>) {
13        (**self).project(points);
14    }
15}
16
17impl<P: Project<T, N, H>, T, const N: usize, const H: usize> Project<T, N, H> for &P {
18    fn project(&self, points: &mut SMatrix<T, N, H>) {
19        (**self).project(points);
20    }
21}
22
23impl<T, const N: usize, const H: usize> Project<T, N, H> for () {
24    fn project(&self, mut _points: &mut SMatrix<T, N, H>) {}
25}
26
27impl<P: Project<T, N, H>, T, const N: usize, const H: usize, const NUM: usize> Project<T, N, H>
28    for [P; NUM]
29{
30    fn project(&self, points: &mut SMatrix<T, N, H>) {
31        for projector in self {
32            projector.project(points);
33        }
34    }
35}
36
37macro_rules! derive_tuple_project {
38    ($($project:ident: $number:tt),+) => {
39        impl<$($project: Project<T, N, H>),+, T, const N: usize, const H: usize> Project<T, N, H>
40            for ( $($project,)+ )
41        {
42            fn project(&self, points: &mut SMatrix<T, N, H>) {
43                $(
44                    self.$number.project(points);
45                )+
46            }
47        }
48    };
49}
50
51derive_tuple_project! {P0: 0}
52derive_tuple_project! {P0: 0, P1: 1}
53derive_tuple_project! {P0: 0, P1: 1, P2: 2}
54derive_tuple_project! {P0: 0, P1: 1, P2: 2, P3: 3}
55derive_tuple_project! {P0: 0, P1: 1, P2: 2, P3: 3, P4: 4}
56derive_tuple_project! {P0: 0, P1: 1, P2: 2, P3: 3, P4: 4, P5: 5}
57derive_tuple_project! {P0: 0, P1: 1, P2: 2, P3: 3, P4: 4, P5: 5, P6: 6}
58derive_tuple_project! {P0: 0, P1: 1, P2: 2, P3: 3, P4: 4, P5: 5, P6: 6, P7: 7}
59derive_tuple_project! {P0: 0, P1: 1, P2: 2, P3: 3, P4: 4, P5: 5, P6: 6, P7: 7, P8: 8}
60derive_tuple_project! {P0: 0, P1: 1, P2: 2, P3: 3, P4: 4, P5: 5, P6: 6, P7: 7, P8: 8, P9: 9}
61
62/// Extension trait for types implementing [`Project`] to convert it directly
63/// into a constraint with associated dual and slack variables.
64pub trait ProjectExt<T: RealField + Copy, const N: usize, const H: usize>:
65    Project<T, N, H> + Sized
66{
67    fn dynamic(&self) -> &dyn Project<T, N, H> {
68        self
69    }
70
71    fn constraint(&self) -> Constraint<T, &Self, N, H> {
72        Constraint::new(self)
73    }
74
75    fn dyn_constraint(&self) -> DynConstraint<'_, T, N, H> {
76        Constraint::new(self.dynamic())
77    }
78}
79
80impl<S: Project<T, N, H>, T: RealField + Copy, const N: usize, const H: usize> ProjectExt<T, N, H>
81    for S
82{
83}
84
85/// A box constraint that is constant throughout the horizon.
86pub struct Box<T, const N: usize> {
87    pub lower: SVector<Option<T>, N>,
88    pub upper: SVector<Option<T>, N>,
89}
90
91impl<T: RealField + Copy, const N: usize, const H: usize> Project<T, N, H> for Box<T, N> {
92    #[inline(always)]
93    fn project(&self, points: &mut SMatrix<T, N, H>) {
94        profiling::scope!("projector: Box");
95        let lower = self.lower.map(|x| x.unwrap_or(T::min_value().unwrap()));
96        let upper = self.upper.map(|x| x.unwrap_or(T::max_value().unwrap()));
97
98        for h in 0..H {
99            let mut column = points.column_mut(h);
100            for n in 0..N {
101                column[n] = column[n].clamp(lower[n], upper[n]);
102            }
103        }
104    }
105}
106
107/// A spherical constraint that is constant throughout the horizon
108#[derive(Debug, Copy, Clone)]
109pub struct Sphere<T, const N: usize> {
110    pub center: SVector<Option<T>, N>,
111    pub radius: T,
112}
113
114impl<T: RealField + Copy, const N: usize, const H: usize> Project<T, N, H> for Sphere<T, N> {
115    #[inline(always)]
116    fn project(&self, points: &mut SMatrix<T, N, H>) {
117        profiling::scope!("projector: Sphere");
118        // Special case, just snap the points to the center coordinate
119        if self.radius.is_zero() {
120            for h in 0..H {
121                let mut point = points.column_mut(h);
122                for n in 0..N {
123                    if let Some(center) = self.center[n] {
124                        point[n] = center
125                    }
126                }
127            }
128            return;
129        }
130
131        for h in 0..H {
132            let mut point = points.column_mut(h);
133
134            // Compute squared distance only for dimensions with defined centers
135            let mut squared_dist = T::zero();
136            let mut has_constraint = false;
137
138            for n in 0..N {
139                if let Some(center) = self.center[n] {
140                    has_constraint = true;
141                    let diff = point[n] - center;
142                    squared_dist += diff * diff;
143                }
144            }
145
146            // If no dimensions are constrained or point is within radius, skip
147            if !has_constraint || squared_dist <= self.radius * self.radius {
148                continue;
149            }
150
151            // Calculate scaling factor for projection
152            let dist = squared_dist.sqrt();
153            let scale = self.radius / dist;
154
155            // Apply scaling only to dimensions with defined centers
156            for n in 0..N {
157                if let Some(center) = self.center[n] {
158                    let diff = point[n] - center;
159                    point[n] = center + diff * scale;
160                }
161            }
162        }
163    }
164}
165
166/// An anti-spherical constraint that is constant throughout the horizon
167#[derive(Debug, Copy, Clone)]
168pub struct AntiSphere<T, const N: usize> {
169    pub center: SVector<Option<T>, N>,
170    pub radius: T,
171}
172
173impl<T: RealField + Copy, const N: usize, const H: usize> Project<T, N, H> for AntiSphere<T, N> {
174    #[inline(always)]
175    fn project(&self, points: &mut SMatrix<T, N, H>) {
176        // If radius is zero, the feasible region is everything.
177        if self.radius.is_zero() {
178            return;
179        }
180
181        for h in 0..H {
182            let mut point = points.column_mut(h);
183
184            // Compute squared distance only for dimensions with defined centers
185            let mut squared_dist = T::zero();
186            let mut has_constraint = false;
187
188            for n in 0..N {
189                if let Some(center) = self.center[n] {
190                    has_constraint = true;
191                    let diff = point[n] - center;
192                    squared_dist += diff * diff;
193                }
194            }
195
196            // If no dimensions are constrained or point is outside/on radius, it's valid
197            if !has_constraint || squared_dist >= self.radius * self.radius {
198                continue;
199            }
200
201            let dist = squared_dist.sqrt();
202
203            // The projection direction is undefined.
204            if dist.is_zero() {
205                for n in 0..N {
206                    if let Some(center) = self.center[n] {
207                        point[n] = center + self.radius;
208                        // Break after moving along the first available axis
209                        break;
210                    }
211                }
212                continue;
213            }
214
215            // Calculate scaling factor for projection
216            let scale = self.radius / dist;
217
218            // Apply scaling to push the point to the surface
219            for n in 0..N {
220                if let Some(center) = self.center[n] {
221                    let diff = point[n] - center;
222                    point[n] = center + diff * scale;
223                }
224            }
225        }
226    }
227}
228
229/// A half-space constraint that is constant throughout the horizon
230#[derive(Debug, Copy, Clone)]
231pub struct Affine<T, const N: usize> {
232    pub normal: SVector<T, N>,
233    pub distance: T,
234}
235
236impl<T: RealField + Copy, const N: usize, const H: usize> Project<T, N, H> for Affine<T, N> {
237    #[inline(always)]
238    fn project(&self, points: &mut SMatrix<T, N, H>) {
239        profiling::scope!("projector: Affine");
240        if self.normal.norm_squared().is_zero() {
241            return;
242        }
243        let normal = self.normal.normalize();
244
245        for h in 0..H {
246            let mut point = points.column_mut(h);
247            let dot = point.dot(&normal);
248
249            if dot > self.distance {
250                // Project onto the boundary: move point towards the plane
251                let correction = normal.scale(dot - self.distance);
252                point -= correction;
253            }
254        }
255    }
256}
tinympc_rs/project.rs

tinympc_rs/
project.rs
