jagua_rs/geometry/fail_fast/
sp_surrogate.rs

1use crate::geometry::Transformation;
2use crate::geometry::convex_hull;
3use crate::geometry::fail_fast::pole;
4use crate::geometry::geo_traits::{Transformable, TransformableFrom};
5use crate::geometry::primitives::Circle;
6use crate::geometry::primitives::SPolygon;
7use itertools::Itertools;
8use serde::{Deserialize, Serialize};
9
10use anyhow::{Result, ensure};
11
12#[derive(Clone, Debug)]
13/// Surrogate representation of a [`SPolygon`] - a 'light-weight' representation that
14/// is fully contained in the original [`SPolygon`].
15/// Used for *fail-fast* collision detection.
16pub struct SPSurrogate {
17    /// Interior circles used for collision screening and overlap estimation.
18    pub poles: Vec<Circle>,
19    /// Indices of the polygon vertices forming its counterclockwise convex hull.
20    /// The owning polygon remaps these when reflection reverses its vertex storage.
21    pub convex_hull_indices: Vec<usize>,
22    /// The area of the convex hull of the [`SPolygon`].
23    pub convex_hull_area: f32,
24    /// The configuration used to generate the surrogate
25    pub config: SPSurrogateConfig,
26    /// Length of the pole prefix selected by [`SPSurrogateConfig::ff_pole_area_ratio`].
27    n_ff_poles: usize,
28    /// Whether the owning polygon's vertex storage is reversed relative to the one
29    /// this surrogate was generated from, which determines `convex_hull_indices`.
30    hull_reversed: bool,
31}
32
33impl SPSurrogate {
34    /// Creates a new [`SPSurrogate`] from a [`SPolygon`] and a configuration.
35    /// Expensive operations are performed here!
36    pub fn new(simple_poly: &SPolygon, config: SPSurrogateConfig) -> Result<Self> {
37        ensure!(
38            (0.0..=1.0).contains(&config.ff_pole_area_ratio),
39            "fail-fast pole area ratio must be between 0.0 and 1.0"
40        );
41        let convex_hull_indices = convex_hull::convex_hull_indices(simple_poly);
42        let convex_hull_points = convex_hull_indices
43            .iter()
44            .map(|&i| simple_poly.vertices[i])
45            .collect_vec();
46        let convex_hull_area = SPolygon::calculate_area(&convex_hull_points);
47        let poles = pole::generate_surrogate_poles(simple_poly, &config.n_pole_limits)?;
48        let n_ff_poles = if config.ff_pole_area_ratio == 0.0 {
49            0
50        } else {
51            let target_area = simple_poly.area * config.ff_pole_area_ratio;
52            let mut covered_area = 0.0;
53            poles
54                .iter()
55                .position(|pole| {
56                    covered_area += pole.area();
57                    covered_area >= target_area
58                })
59                .map_or(poles.len(), |i| i + 1)
60        };
61
62        Ok(Self {
63            poles,
64            convex_hull_indices,
65            convex_hull_area,
66            config,
67            n_ff_poles,
68            hull_reversed: false,
69        })
70    }
71
72    /// Returns the smallest generated pole prefix that reaches the configured fail-fast coverage.
73    #[must_use]
74    pub fn ff_poles(&self) -> &[Circle] {
75        &self.poles[..self.n_ff_poles]
76    }
77
78    /// Remaps `convex_hull_indices` after the owning polygon reversed its `n_vertices` vertices.
79    pub(crate) fn reverse_vertex_order(&mut self, n_vertices: usize) {
80        self.convex_hull_indices.reverse();
81        for i in &mut self.convex_hull_indices {
82            *i = n_vertices - 1 - *i;
83        }
84        self.hull_reversed = !self.hull_reversed;
85    }
86
87    /// Whether `convex_hull_indices` refer to vertex storage reversed since generation.
88    pub(crate) fn hull_reversed(&self) -> bool {
89        self.hull_reversed
90    }
91}
92
93impl Transformable for SPSurrogate {
94    fn transform(&mut self, t: &Transformation) -> &mut Self {
95        //destructuring pattern used to ensure that the code is updated accordingly when the struct changes
96        let Self {
97            convex_hull_indices: _,
98            poles,
99            convex_hull_area: _,
100            config: _,
101            n_ff_poles: _,
102            hull_reversed: _,
103        } = self;
104
105        //transform poles
106        for c in poles.iter_mut() {
107            c.transform(t);
108        }
109
110        self
111    }
112}
113
114impl TransformableFrom for SPSurrogate {
115    fn transform_from(&mut self, reference: &Self, t: &Transformation) -> &mut Self {
116        debug_assert_eq!(self.poles.len(), reference.poles.len());
117
118        //destructuring pattern used to ensure that the code is updated accordingly when the struct changes
119        let Self {
120            convex_hull_indices: _,
121            poles,
122            convex_hull_area: _,
123            config: _,
124            n_ff_poles: _,
125            hull_reversed: _,
126        } = self;
127
128        for (pole, ref_pole) in poles.iter_mut().zip(reference.poles.iter()) {
129            pole.transform_from(ref_pole, t);
130        }
131
132        self
133    }
134}
135
136/// maximum number of definable pole limits, increase if needed
137const N_POLE_LIMITS: usize = 3;
138
139/// Configuration of the [`SPSurrogate`](crate::geometry::fail_fast::SPSurrogate) generation
140#[derive(Serialize, Deserialize, Clone, Copy, Debug, PartialEq)]
141pub struct SPSurrogateConfig {
142    ///Limits on the number of poles to be generated at different coverage levels.
143    ///For example: [(100, 0.0), (20, 0.75), (10, 0.90)]:
144    ///While the coverage is below 75% the generation will stop at 100 poles.
145    ///If 75% coverage with 20 or more poles the generation will stop.
146    ///If 90% coverage with 10 or more poles the generation will stop.
147    pub n_pole_limits: [(usize, f32); N_POLE_LIMITS],
148    /// Fraction of the polygon area that fail-fast poles should cover.
149    ///
150    /// The smallest prefix whose combined non-overlapping area reaches this ratio is selected.
151    /// `0.0` disables pole checks. If the generated poles do not reach the requested coverage, all
152    /// of them are checked.
153    pub ff_pole_area_ratio: f32,
154}
155
156impl SPSurrogateConfig {
157    #[must_use]
158    pub fn none() -> Self {
159        Self {
160            n_pole_limits: [(0, 0.0); N_POLE_LIMITS],
161            ff_pole_area_ratio: 0.0,
162        }
163    }
164}
jagua_rs/geometry/fail_fast/sp_surrogate.rs

jagua_rs/geometry/fail_fast/
sp_surrogate.rs
