sophus_core/calculus/
region.rs

1use nalgebra::SVector;
2
3use crate::IsPoint;
4
5/// Floating-point interval
6#[derive(Debug, Copy, Clone, PartialEq)]
7pub struct Interval {
8    /// min and max of the interval
9    pub min_max: Option<(f64, f64)>,
10}
11
12/// Integer interval
13#[derive(Debug, Copy, Clone, PartialEq)]
14pub struct IInterval {
15    /// min and max of the interval
16    pub min_max: Option<(i64, i64)>,
17}
18
19/// Region - n-dimensional interval
20#[derive(Debug, Copy, Clone, PartialEq)]
21pub struct Region<const D: usize> {
22    /// min and max of the region
23    pub min_max: Option<(SVector<f64, D>, SVector<f64, D>)>,
24}
25
26/// Integer Region - n-dimensional interval
27#[derive(Debug, Copy, Clone, PartialEq)]
28pub struct IRegion<const D: usize> {
29    /// min and max of the region
30    pub min_max: Option<(SVector<i64, D>, SVector<i64, D>)>,
31}
32
33impl<const D: usize> IRegion<D> {
34    /// convert integer region to floating point region
35    pub fn to_region(&self) -> Region<D> {
36        if self.is_empty() {
37            return Region::empty();
38        }
39        // example: [2, 5] -> [1.5, 5.5]
40        Region::from_min_max(
41            self.min().cast() - SVector::repeat(0.5),
42            self.max().cast() + SVector::repeat(0.5),
43        )
44    }
45}
46
47/// Traits for regions
48pub trait IsRegion<const D: usize, P: IsPoint<D>>: core::marker::Sized {
49    /// create unbounded region
50    fn unbounded() -> Self;
51
52    /// create empty region
53    fn empty() -> Self;
54
55    /// create region from min and max values
56    fn from_min_max(min: P, max: P) -> Self;
57
58    /// create region from point
59    fn from_point(point: P) -> Self {
60        Self::from_min_max(point, point)
61    }
62
63    /// is region empty
64    fn is_empty(&self) -> bool;
65
66    /// is region degenerated
67    ///
68    /// A region is degenerated if it is not empty and min == max
69    fn is_degenerated(&self) -> bool;
70
71    /// is region proper
72    ///
73    /// A region is proper if it is not empty and not degenerated
74    fn is_proper(&self) -> bool {
75        !self.is_empty() && !self.is_degenerated()
76    }
77
78    /// is region unbounded
79    fn is_unbounded(&self) -> bool;
80
81    /// extend region to include point
82    fn extend(&mut self, point: &P);
83
84    /// min of the region
85    ///
86    /// panics if the region is empty
87    fn min(&self) -> P {
88        self.try_min().unwrap()
89    }
90
91    /// max of the region
92    ///
93    /// panics if the region is empty
94    fn max(&self) -> P {
95        self.try_max().unwrap()
96    }
97
98    /// return min of the region or None if the region is empty
99    fn try_min(&self) -> Option<P>;
100
101    /// return max of the region or None if the region is empty
102    fn try_max(&self) -> Option<P>;
103
104    /// clamp point to the region
105    fn clamp(&self, p: P) -> P;
106
107    /// intersect
108    fn intersect(self, other: Self) -> Self {
109        if other.is_empty() {
110            return other;
111        }
112        if self.is_empty() {
113            return self;
114        }
115
116        Self::from_min_max(self.clamp(other.min()), self.clamp(other.max()))
117    }
118
119    /// check if the region contains a point
120    fn contains(&self, p: P) -> bool {
121        if self.is_empty() {
122            return false;
123        }
124        self.min().is_less_equal(self.min()) && p.is_less_equal(self.max())
125    }
126
127    /// range of the region
128    fn range(&self) -> P;
129
130    /// mid of the region
131    fn mid(&self) -> P;
132}
133
134impl IsRegion<1, f64> for Interval {
135    fn unbounded() -> Self {
136        Self {
137            min_max: Option::Some((f64::NEG_INFINITY, f64::INFINITY)),
138        }
139    }
140
141    fn empty() -> Self {
142        Self {
143            min_max: Option::None,
144        }
145    }
146
147    fn from_min_max(min: f64, max: f64) -> Self {
148        Self {
149            min_max: Option::Some((min, max)),
150        }
151    }
152
153    fn is_empty(&self) -> bool {
154        self.min_max.is_none()
155    }
156
157    fn is_degenerated(&self) -> bool {
158        if self.is_empty() {
159            return false;
160        }
161        self.min() == self.max()
162    }
163
164    fn is_unbounded(&self) -> bool {
165        if self.is_empty() {
166            return false;
167        }
168        self.min() == f64::NEG_INFINITY && self.max() == f64::INFINITY
169    }
170
171    fn extend(&mut self, point: &f64) {
172        if self.is_empty() {
173            *self = Self::from_point(*point);
174        }
175        let (min, max) = (self.min().min(*point), self.max().max(*point));
176
177        *self = Self::from_min_max(min, max)
178    }
179
180    fn try_min(&self) -> Option<f64> {
181        Some(self.min_max?.0)
182    }
183
184    fn try_max(&self) -> Option<f64> {
185        Some(self.min_max?.1)
186    }
187
188    fn clamp(&self, p: f64) -> f64 {
189        p.clamp(self.min(), self.max())
190    }
191
192    fn range(&self) -> f64 {
193        if self.is_empty() {
194            return 0.0;
195        }
196        self.max() - self.min()
197    }
198
199    fn mid(&self) -> f64 {
200        self.min() + 0.5 * self.range()
201    }
202}
203
204impl<const D: usize> IsRegion<D, SVector<f64, D>> for Region<D> {
205    fn unbounded() -> Self {
206        let s: SVector<f64, D> = SVector::<f64, D>::smallest();
207        let l: SVector<f64, D> = SVector::<f64, D>::largest();
208        Self::from_min_max(s, l)
209    }
210
211    fn empty() -> Self {
212        Self {
213            min_max: Option::default(),
214        }
215    }
216
217    fn is_empty(&self) -> bool {
218        self.min_max.is_none()
219    }
220
221    fn is_degenerated(&self) -> bool {
222        if self.is_empty() {
223            return false;
224        }
225        self.min() == self.max()
226    }
227
228    fn is_unbounded(&self) -> bool {
229        if self.is_empty() {
230            return false;
231        }
232        self.min() == SVector::<f64, D>::smallest() && self.max() == SVector::<f64, D>::largest()
233    }
234
235    fn from_min_max(min: SVector<f64, D>, max: SVector<f64, D>) -> Self {
236        Self {
237            min_max: Option::Some((min, max)),
238        }
239    }
240
241    fn try_min(&self) -> Option<SVector<f64, D>> {
242        Some(self.min_max?.0)
243    }
244    fn try_max(&self) -> Option<SVector<f64, D>> {
245        Some(self.min_max?.1)
246    }
247
248    fn clamp(&self, p: SVector<f64, D>) -> SVector<f64, D> {
249        p.clamp(self.min(), self.max())
250    }
251
252    fn range(&self) -> SVector<f64, D> {
253        let p: SVector<f64, D>;
254        if self.is_empty() {
255            p = SVector::<f64, D>::zeros();
256            return p;
257        }
258        p = self.max() - self.min();
259        p
260    }
261
262    fn mid(&self) -> SVector<f64, D> {
263        self.min() + 0.5 * self.range()
264    }
265
266    fn extend(&mut self, point: &SVector<f64, D>) {
267        if self.is_empty() {
268            *self = Self::from_point(*point);
269        }
270        let (min, max) = self.min().inf_sup(point);
271
272        *self = Self::from_min_max(min, max)
273    }
274}
275
276impl IsRegion<1, i64> for IInterval {
277    fn unbounded() -> Self {
278        Self {
279            min_max: Option::Some((i64::MIN, i64::MAX)),
280        }
281    }
282
283    fn empty() -> Self {
284        Self {
285            min_max: Option::None,
286        }
287    }
288
289    fn from_min_max(min: i64, max: i64) -> Self {
290        Self {
291            min_max: Option::Some((min, max)),
292        }
293    }
294
295    fn is_empty(&self) -> bool {
296        self.min_max.is_none()
297    }
298
299    fn is_degenerated(&self) -> bool {
300        false
301    }
302
303    fn is_unbounded(&self) -> bool {
304        if self.is_empty() {
305            return false;
306        }
307        self.min() == i64::MIN && self.max() == i64::MAX
308    }
309
310    fn extend(&mut self, point: &i64) {
311        if self.is_empty() {
312            *self = Self::from_point(*point);
313        }
314        let (min, max) = (self.min().min(*point), self.max().max(*point));
315
316        *self = Self::from_min_max(min, max)
317    }
318
319    fn try_min(&self) -> Option<i64> {
320        Some(self.min_max?.0)
321    }
322
323    fn try_max(&self) -> Option<i64> {
324        Some(self.min_max?.1)
325    }
326
327    fn clamp(&self, p: i64) -> i64 {
328        p.clamp(self.min(), self.max())
329    }
330
331    fn range(&self) -> i64 {
332        if self.is_empty() {
333            return 0;
334        }
335        self.max() - self.min()
336    }
337
338    fn mid(&self) -> i64 {
339        self.min() + self.range() / 2
340    }
341}
342
343impl<const D: usize> IsRegion<D, SVector<i64, D>> for IRegion<D> {
344    fn unbounded() -> Self {
345        let s: SVector<i64, D> = SVector::<i64, D>::smallest();
346        let l: SVector<i64, D> = SVector::<i64, D>::largest();
347        Self::from_min_max(s, l)
348    }
349
350    fn empty() -> Self {
351        Self {
352            min_max: Option::default(),
353        }
354    }
355
356    fn is_empty(&self) -> bool {
357        self.min_max.is_none()
358    }
359
360    fn is_degenerated(&self) -> bool {
361        false
362    }
363
364    fn is_unbounded(&self) -> bool {
365        if self.is_empty() {
366            return false;
367        }
368        self.min() == SVector::<i64, D>::smallest() && self.max() == SVector::<i64, D>::largest()
369    }
370
371    fn from_min_max(min: SVector<i64, D>, max: SVector<i64, D>) -> Self {
372        Self {
373            min_max: Option::Some((min, max)),
374        }
375    }
376
377    fn try_min(&self) -> Option<SVector<i64, D>> {
378        Some(self.min_max?.0)
379    }
380    fn try_max(&self) -> Option<SVector<i64, D>> {
381        Some(self.min_max?.1)
382    }
383
384    fn clamp(&self, p: SVector<i64, D>) -> SVector<i64, D> {
385        p.clamp(self.min(), self.max())
386    }
387
388    fn range(&self) -> SVector<i64, D> {
389        let p: SVector<i64, D>;
390        if self.is_empty() {
391            p = SVector::<i64, D>::zeros();
392            return p;
393        }
394        p = self.max() - self.min() + SVector::<i64, D>::repeat(1);
395        p
396    }
397
398    fn mid(&self) -> SVector<i64, D> {
399        self.min() + self.range() / 2
400    }
401
402    fn extend(&mut self, point: &SVector<i64, D>) {
403        if self.is_empty() {
404            *self = Self::from_point(*point);
405        }
406        let (min, max) = self.min().inf_sup(point);
407
408        *self = Self::from_min_max(min, max)
409    }
410}
411
412#[cfg(test)]
413mod tests {
414
415    use super::*;
416
417    #[test]
418    fn region() {
419        let empty_f64 = Region::<2>::empty();
420        assert!(empty_f64.is_empty());
421        assert!(!empty_f64.is_degenerated());
422        assert!(!empty_f64.is_proper());
423        assert!(!empty_f64.is_unbounded());
424
425        let unbounded = Region::<2>::unbounded();
426        assert!(!unbounded.is_empty());
427        assert!(!unbounded.is_degenerated());
428        assert!(unbounded.is_proper());
429        assert!(unbounded.is_unbounded());
430
431        let one_i64 = IRegion::<2>::from_point(SVector::<i64, 2>::repeat(1));
432        assert!(!one_i64.is_empty());
433        assert!(!one_i64.is_degenerated());
434        assert!(one_i64.is_proper());
435        assert!(!one_i64.is_unbounded());
436
437        let two_f64 = Region::<2>::from_point(SVector::<f64, 2>::repeat(2.0));
438        assert!(!two_f64.is_empty());
439        assert!(two_f64.is_degenerated());
440        assert!(!two_f64.is_proper());
441        assert!(!two_f64.is_unbounded());
442    }
443
444    #[test]
445    fn interval() {
446        let empty_f64 = Interval::empty();
447        assert!(empty_f64.is_empty());
448        assert!(!empty_f64.is_degenerated());
449        assert!(!empty_f64.is_proper());
450        assert!(!empty_f64.is_unbounded());
451
452        let unbounded = Interval::unbounded();
453        assert!(!unbounded.is_empty());
454        assert!(!unbounded.is_degenerated());
455        assert!(unbounded.is_proper());
456        assert!(unbounded.is_unbounded());
457
458        let one_i64 = IRegion::<1>::from_point(SVector::<i64, 1>::repeat(1));
459        assert!(!one_i64.is_empty());
460        assert!(!one_i64.is_degenerated());
461        assert!(one_i64.is_proper());
462        assert!(!one_i64.is_unbounded());
463
464        let two_f64 = Interval::from_point(2.0);
465        assert!(!two_f64.is_empty());
466        assert!(two_f64.is_degenerated());
467        assert!(!two_f64.is_proper());
468        assert!(!two_f64.is_unbounded());
469    }
470}
sophus_core/calculus/region.rs

sophus_core/calculus/
region.rs
