axioval_engine/
plan_region.rs

1//! Convex plan regions: stated areas of the floor plan that are not an
2//! object's body, such as the sector a door leaf sweeps.
3//!
4//! A region is a convex polygon in canonical metres, anticlockwise seen
5//! from above. Its geometry is exact as stated; a region approximating a
6//! curved area (a sector between an inscribed and a circumscribed polygon)
7//! is the caller's bracket, not the region's.
8
9use crate::{PlanRing, ProximityError};
10
11/// Tolerance on convexity, relative to the edge lengths multiplied.
12const CONVEXITY: f64 = 1.0e-12;
13
14/// A convex plan polygon, anticlockwise, in canonical metres.
15#[derive(Clone, Debug, PartialEq)]
16pub struct ConvexPlanRegion {
17    ring: PlanRing,
18}
19
20fn cross(o: [f64; 2], a: [f64; 2], b: [f64; 2]) -> f64 {
21    (a[0] - o[0]) * (b[1] - o[1]) - (a[1] - o[1]) * (b[0] - o[0])
22}
23
24fn length(a: [f64; 2], b: [f64; 2]) -> f64 {
25    (b[0] - a[0]).hypot(b[1] - a[1])
26}
27
28/// Distance from `point` to the segment `a`–`b`.
29fn segment_distance(point: [f64; 2], a: [f64; 2], b: [f64; 2]) -> f64 {
30    let (dx, dy) = (b[0] - a[0], b[1] - a[1]);
31    let squared = dx * dx + dy * dy;
32    let t = if squared == 0.0 {
33        0.0
34    } else {
35        (((point[0] - a[0]) * dx + (point[1] - a[1]) * dy) / squared).clamp(0.0, 1.0)
36    };
37    (point[0] - (a[0] + t * dx)).hypot(point[1] - (a[1] + t * dy))
38}
39
40impl ConvexPlanRegion {
41    /// A region from its vertices, in order and without repeating the
42    /// first. They must be finite, at least three, turn anticlockwise at
43    /// every vertex (collinear vertices are accepted) and enclose a positive
44    /// area.
45    pub fn try_new(ring: PlanRing) -> Result<Self, ProximityError> {
46        let count = ring.len();
47        if count < 3 || ring.iter().flatten().any(|c| !c.is_finite()) {
48            return Err(ProximityError::InvalidMeasurement);
49        }
50        let mut area = 0.0;
51        for index in 0..count {
52            let (a, b, c) = (
53                ring[index],
54                ring[(index + 1) % count],
55                ring[(index + 2) % count],
56            );
57            let scale = length(a, b) * length(b, c);
58            if cross(a, b, c) < -CONVEXITY * scale.max(f64::MIN_POSITIVE) {
59                return Err(ProximityError::InvalidMeasurement);
60            }
61            area += a[0] * b[1] - b[0] * a[1];
62        }
63        if area <= 0.0 {
64            return Err(ProximityError::InvalidMeasurement);
65        }
66        Ok(Self { ring })
67    }
68
69    /// The vertices, anticlockwise.
70    #[must_use]
71    pub fn ring(&self) -> &[[f64; 2]] {
72        &self.ring
73    }
74
75    /// The least and greatest corner of the region's plan box.
76    #[must_use]
77    pub fn bounds(&self) -> ([f64; 2], [f64; 2]) {
78        self.ring.iter().fold(
79            ([f64::INFINITY; 2], [f64::NEG_INFINITY; 2]),
80            |(low, high), [x, y]| {
81                (
82                    [low[0].min(*x), low[1].min(*y)],
83                    [high[0].max(*x), high[1].max(*y)],
84                )
85            },
86        )
87    }
88
89    /// The region's area in square metres.
90    #[must_use]
91    pub fn area_square_metres(&self) -> f64 {
92        let count = self.ring.len();
93        (0..count)
94            .map(|index| {
95                let (a, b) = (self.ring[index], self.ring[(index + 1) % count]);
96                a[0] * b[1] - b[0] * a[1]
97            })
98            .sum::<f64>()
99            / 2.0
100    }
101
102    fn edges(&self) -> impl Iterator<Item = ([f64; 2], [f64; 2])> + '_ {
103        let count = self.ring.len();
104        (0..count).map(move |index| (self.ring[index], self.ring[(index + 1) % count]))
105    }
106
107    /// Signed separation from `other`: the plan distance between the two
108    /// regions when they are apart, zero when they touch, and minus the
109    /// least depth one reaches into the other along an edge normal when
110    /// they overlap with positive area.
111    ///
112    /// Both are convex, so they are apart exactly when an edge normal of
113    /// one separates them, and their distance is then the least distance
114    /// from a vertex of one to an edge of the other.
115    #[must_use]
116    pub fn separation(&self, other: &Self) -> f64 {
117        let mut gap = f64::NEG_INFINITY;
118        for (region, against) in [(self, other), (other, self)] {
119            for (a, b) in region.edges() {
120                let edge = length(a, b);
121                if edge == 0.0 {
122                    continue;
123                }
124                // Outward normal of an anticlockwise edge.
125                let normal = [(b[1] - a[1]) / edge, (a[0] - b[0]) / edge];
126                let reach = |point: &[f64; 2]| {
127                    (point[0] - a[0]) * normal[0] + (point[1] - a[1]) * normal[1]
128                };
129                let nearest = against.ring.iter().map(reach).fold(f64::INFINITY, f64::min);
130                gap = gap.max(nearest);
131            }
132        }
133        if gap < 0.0 {
134            return gap;
135        }
136        let mut distance = f64::INFINITY;
137        for (region, against) in [(self, other), (other, self)] {
138            for point in &region.ring {
139                for (a, b) in against.edges() {
140                    distance = distance.min(segment_distance(*point, a, b));
141                }
142            }
143        }
144        distance
145    }
146}
147
148#[cfg(test)]
149mod tests {
150    use super::*;
151
152    fn square(x: f64, y: f64, side: f64) -> ConvexPlanRegion {
153        ConvexPlanRegion::try_new(vec![
154            [x, y],
155            [x + side, y],
156            [x + side, y + side],
157            [x, y + side],
158        ])
159        .unwrap()
160    }
161
162    #[test]
163    fn a_region_is_convex_and_anticlockwise() {
164        assert!(ConvexPlanRegion::try_new(vec![[0.0, 0.0], [1.0, 0.0]]).is_err());
165        // Clockwise.
166        assert!(
167            ConvexPlanRegion::try_new(vec![[0.0, 0.0], [0.0, 1.0], [1.0, 1.0], [1.0, 0.0]])
168                .is_err()
169        );
170        // A notch.
171        assert!(
172            ConvexPlanRegion::try_new(vec![
173                [0.0, 0.0],
174                [2.0, 0.0],
175                [1.0, 0.5],
176                [2.0, 2.0],
177                [0.0, 2.0]
178            ])
179            .is_err()
180        );
181        // Collinear vertices are accepted.
182        let collinear =
183            ConvexPlanRegion::try_new(vec![[0.0, 0.0], [1.0, 0.0], [2.0, 0.0], [1.0, 1.0]])
184                .unwrap();
185        assert!((collinear.area_square_metres() - 1.0).abs() < 1e-12);
186        assert_eq!(square(1.0, 2.0, 1.0).bounds(), ([1.0, 2.0], [2.0, 3.0]));
187    }
188
189    #[test]
190    fn separation_is_the_distance_apart_or_the_depth_of_overlap() {
191        let a = square(0.0, 0.0, 1.0);
192        assert!((a.separation(&square(3.0, 0.0, 1.0)) - 2.0).abs() < 1e-12);
193        // Corner to corner.
194        let diagonal = a.separation(&square(2.0, 2.0, 1.0));
195        assert!((diagonal - 2.0_f64.sqrt()).abs() < 1e-12);
196        assert!(a.separation(&square(1.0, 0.0, 1.0)).abs() < 1e-12);
197        assert!((a.separation(&square(0.75, 0.5, 1.0)) + 0.25).abs() < 1e-12);
198    }
199}
axioval_engine/plan_region.rs

axioval_engine/
plan_region.rs
