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            convex_hull_indices,
48            poles,
49            piers,
50            convex_hull_area,
51            config,
52        })
53    }
54
55    pub fn ff_poles(&self) -> &[Circle] {
56        &self.poles[0..self.config.n_ff_poles]
57    }
58
59    pub fn ff_piers(&self) -> &[Edge] {
60        &self.piers
61    }
62}
63
64impl Transformable for SPSurrogate {
65    fn transform(&mut self, t: &Transformation) -> &mut Self {
66        //destructuring pattern used to ensure that the code is updated accordingly when the struct changes
67        let Self {
68            convex_hull_indices: _,
69            poles,
70            piers,
71            convex_hull_area: _,
72            config: _,
73        } = self;
74
75        //transform poles
76        poles.iter_mut().for_each(|c| {
77            c.transform(t);
78        });
79
80        //transform piers
81        piers.iter_mut().for_each(|p| {
82            p.transform(t);
83        });
84
85        self
86    }
87}
88
89impl TransformableFrom for SPSurrogate {
90    fn transform_from(&mut self, reference: &Self, t: &Transformation) -> &mut Self {
91        debug_assert!(self.poles.len() == reference.poles.len());
92        debug_assert!(self.piers.len() == reference.piers.len());
93
94        //destructuring pattern used to ensure that the code is updated accordingly when the struct changes
95        let Self {
96            convex_hull_indices: _,
97            poles,
98            piers,
99            convex_hull_area: _,
100            config: _,
101        } = self;
102
103        for (pole, ref_pole) in poles.iter_mut().zip(reference.poles.iter()) {
104            pole.transform_from(ref_pole, t);
105        }
106
107        for (pier, ref_pier) in piers.iter_mut().zip(reference.piers.iter()) {
108            pier.transform_from(ref_pier, t);
109        }
110
111        self
112    }
113}
114
115/// maximum number of definable pole limits, increase if needed
116const N_POLE_LIMITS: usize = 3;
117
118/// Configuration of the [`SPSurrogate`](crate::geometry::fail_fast::SPSurrogate) generation
119#[derive(Serialize, Deserialize, Clone, Copy, Debug, PartialEq)]
120pub struct SPSurrogateConfig {
121    ///Limits on the number of poles to be generated at different coverage levels.
122    ///For example: [(100, 0.0), (20, 0.75), (10, 0.90)]:
123    ///While the coverage is below 75% the generation will stop at 100 poles.
124    ///If 75% coverage with 20 or more poles the generation will stop.
125    ///If 90% coverage with 10 or more poles the generation will stop.
126    pub n_pole_limits: [(usize, f32); N_POLE_LIMITS],
127    ///Number of poles to test during fail-fast
128    pub n_ff_poles: usize,
129    ///number of piers to test during fail-fast
130    pub n_ff_piers: usize,
131}
132
133impl SPSurrogateConfig {
134    pub fn none() -> Self {
135        Self {
136            n_pole_limits: [(0, 0.0); N_POLE_LIMITS],
137            n_ff_poles: 0,
138            n_ff_piers: 0,
139        }
140    }
141}
jagua_rs/geometry/fail_fast/sp_surrogate.rs

jagua_rs/geometry/fail_fast/
sp_surrogate.rs
