jagua_rs/geometry/fail_fast/
sp_surrogate.rs

1use crate::geometry::Transformation;
2use crate::geometry::convex_hull;
3use crate::geometry::fail_fast::{piers, pole};
4use crate::geometry::geo_traits::{Transformable, TransformableFrom};
5use crate::geometry::primitives::Circle;
6use crate::geometry::primitives::Edge;
7use crate::geometry::primitives::SPolygon;
8use itertools::Itertools;
9use serde::{Deserialize, Serialize};
10
11use anyhow::Result;
12
13#[derive(Clone, Debug)]
14/// Surrogate representation of a [`SPolygon`] - a 'light-weight' representation that
15/// is fully contained in the original [`SPolygon`].
16/// Used for *fail-fast* collision detection.
17pub struct SPSurrogate {
18    /// Set of [poles](pole::generate_surrogate_poles)
19    pub poles: Vec<Circle>,
20    /// Set of [piers](piers::generate_piers)
21    pub piers: Vec<Edge>,
22    /// Indices of the vertices in the [`SPolygon`] that form the convex hull
23    pub convex_hull_indices: Vec<usize>,
24    /// The area of the convex hull of the [`SPolygon`].
25    pub convex_hull_area: f32,
26    /// The configuration used to generate the surrogate
27    pub config: SPSurrogateConfig,
28}
29
30impl SPSurrogate {
31    /// Creates a new [`SPSurrogate`] from a [`SPolygon`] and a configuration.
32    /// Expensive operations are performed here!
33    pub fn new(simple_poly: &SPolygon, config: SPSurrogateConfig) -> Result<Self> {
34        let convex_hull_indices = convex_hull::convex_hull_indices(simple_poly);
35        let convex_hull_points = convex_hull_indices
36            .iter()
37            .map(|&i| simple_poly.vertices[i])
38            .collect_vec();
39        let convex_hull_area = SPolygon::calculate_area(&convex_hull_points);
40        let poles = pole::generate_surrogate_poles(simple_poly, &config.n_pole_limits)?;
41        let n_ff_poles = usize::min(config.n_ff_poles, poles.len());
42        let relevant_poles_for_piers = &poles[0..n_ff_poles]; //poi + all poles that will be checked during fail fast are relevant for piers
43        let piers =
44            piers::generate_piers(simple_poly, config.n_ff_piers, relevant_poles_for_piers)?;
45
46        Ok(Self {
47            poles,
48            piers,
49            convex_hull_indices,
50            convex_hull_area,
51            config,
52        })
53    }
54
55    #[must_use]
56    pub fn ff_poles(&self) -> &[Circle] {
57        &self.poles[0..self.config.n_ff_poles]
58    }
59
60    #[must_use]
61    pub fn ff_piers(&self) -> &[Edge] {
62        &self.piers
63    }
64}
65
66impl Transformable for SPSurrogate {
67    fn transform(&mut self, t: &Transformation) -> &mut Self {
68        //destructuring pattern used to ensure that the code is updated accordingly when the struct changes
69        let Self {
70            convex_hull_indices: _,
71            poles,
72            piers,
73            convex_hull_area: _,
74            config: _,
75        } = self;
76
77        //transform poles
78        for c in poles.iter_mut() {
79            c.transform(t);
80        }
81
82        //transform piers
83        for p in piers.iter_mut() {
84            p.transform(t);
85        }
86
87        self
88    }
89}
90
91impl TransformableFrom for SPSurrogate {
92    fn transform_from(&mut self, reference: &Self, t: &Transformation) -> &mut Self {
93        debug_assert!(self.poles.len() == reference.poles.len());
94        debug_assert!(self.piers.len() == reference.piers.len());
95
96        //destructuring pattern used to ensure that the code is updated accordingly when the struct changes
97        let Self {
98            convex_hull_indices: _,
99            poles,
100            piers,
101            convex_hull_area: _,
102            config: _,
103        } = self;
104
105        for (pole, ref_pole) in poles.iter_mut().zip(reference.poles.iter()) {
106            pole.transform_from(ref_pole, t);
107        }
108
109        for (pier, ref_pier) in piers.iter_mut().zip(reference.piers.iter()) {
110            pier.transform_from(ref_pier, t);
111        }
112
113        self
114    }
115}
116
117/// maximum number of definable pole limits, increase if needed
118const N_POLE_LIMITS: usize = 3;
119
120/// Configuration of the [`SPSurrogate`](crate::geometry::fail_fast::SPSurrogate) generation
121#[derive(Serialize, Deserialize, Clone, Copy, Debug, PartialEq)]
122pub struct SPSurrogateConfig {
123    ///Limits on the number of poles to be generated at different coverage levels.
124    ///For example: [(100, 0.0), (20, 0.75), (10, 0.90)]:
125    ///While the coverage is below 75% the generation will stop at 100 poles.
126    ///If 75% coverage with 20 or more poles the generation will stop.
127    ///If 90% coverage with 10 or more poles the generation will stop.
128    pub n_pole_limits: [(usize, f32); N_POLE_LIMITS],
129    ///Number of poles to test during fail-fast
130    pub n_ff_poles: usize,
131    ///number of piers to test during fail-fast
132    pub n_ff_piers: usize,
133}
134
135impl SPSurrogateConfig {
136    #[must_use]
137    pub fn none() -> Self {
138        Self {
139            n_pole_limits: [(0, 0.0); N_POLE_LIMITS],
140            n_ff_poles: 0,
141            n_ff_piers: 0,
142        }
143    }
144}
jagua_rs/geometry/fail_fast/sp_surrogate.rs

jagua_rs/geometry/fail_fast/
sp_surrogate.rs
