simple_geom/
lib.rs

1#![allow(unused_macros)]
2#![allow(unused_imports)]
3
4#[cfg(test)]
5mod tests;
6
7#[derive(Clone, Copy, Debug)]
8pub struct Point2D {
9    x: f64,
10    y: f64,
11}
12#[derive(Clone, Copy, Debug)]
13pub struct Vector2D {
14    x: f64,
15    y: f64,
16}
17#[derive(Clone, Copy, Debug)]
18pub struct Segment2D {
19    p: Point2D,
20    e: Vector2D,
21}
22#[derive(Clone, Copy, Debug)]
23pub struct Line2D {
24    n: Vector2D,
25    a: f64,
26}
27#[derive(Clone, Copy, Debug)]
28pub enum SegmentIntersection {
29    None,
30    Point(Point2D),
31    Segment(Segment2D),
32}
33#[derive(Clone, Debug)]
34pub struct Polygon2D {
35    segments: Vec<Segment2D>,
36}
37#[derive(Clone, Copy, Debug, PartialEq, Eq)]
38pub enum PointLineRelation {
39    Left,
40    OnLine,
41    Right,
42}
43#[derive(Clone, Copy, Debug)]
44pub enum SegmentLineRelation {
45    Left,
46    LeftTouch,
47    OnLine,
48    Intersects(Point2D),
49    Right,
50    RightTouch,
51}
52
53impl Point2D {
54    const ORIGIN: Point2D = Point2D { x: 0., y: 0. };
55
56    pub fn new(x: f64, y: f64) -> Self {
57        Point2D { x, y }
58    }
59    pub fn origin() -> Self {
60        Self::ORIGIN
61    }
62    pub fn x(&self) -> f64 {
63        self.x
64    }
65    pub fn y(&self) -> f64 {
66        self.y
67    }
68    pub fn add(&self, v: &Vector2D) -> Self {
69        Point2D {
70            x: self.x + v.x,
71            y: self.y + v.y,
72        }
73    }
74    pub fn sub(&self, p0: &Point2D) -> Vector2D {
75        Vector2D {
76            x: self.x - p0.x,
77            y: self.y - p0.y,
78        }
79    }
80    pub fn to_vec(&self) -> Vector2D {
81        self.sub(&Self::ORIGIN)
82    }
83    pub fn distance_to(&self, p: &Point2D) -> f64 {
84        self.sub(p).len()
85    }
86}
87
88impl Vector2D {
89    pub fn new(x: f64, y: f64) -> Self {
90        Vector2D { x, y }
91    }
92    pub fn x(&self) -> f64 {
93        self.x
94    }
95    pub fn y(&self) -> f64 {
96        self.y
97    }
98    pub fn len(&self) -> f64 {
99        self.dot(self).sqrt()
100    }
101    pub fn add(&self, v: &Vector2D) -> Self {
102        Vector2D {
103            x: self.x + v.x,
104            y: self.y + v.y,
105        }
106    }
107    pub fn add_mut(&mut self, v: &Vector2D) -> &Self {
108        self.x += v.x;
109        self.y += v.y;
110        self
111    }
112    pub fn kmul(&self, k: f64) -> Self {
113        Vector2D {
114            x: k * self.x,
115            y: k * self.y,
116        }
117    }
118    pub fn dot(&self, v: &Vector2D) -> f64 {
119        self.x * v.x + self.y * v.y
120    }
121    pub fn cross(&self, v: &Vector2D) -> f64 {
122        self.x * v.y - self.y * v.x
123    }
124    pub fn unit(&self) -> Option<Self> {
125        if self.len() != 0.0 {
126            Some(self.kmul(1. / self.len()))
127        } else {
128            None
129        }
130    }
131    pub fn perpendicular(&self) -> Self {
132        Vector2D {
133            x: -self.y,
134            y: self.x,
135        }
136    }
137}
138
139impl Segment2D {
140    const EPS: f64 = 1e-8;
141
142    pub fn new(p: &Point2D, e: &Vector2D) -> Self {
143        Segment2D { p: *p, e: *e }
144    }
145    pub fn with_points(p0: &Point2D, p1: &Point2D) -> Self {
146        Segment2D {
147            p: p0.clone(),
148            e: p1.sub(p0),
149        }
150    }
151
152    pub fn intersect_segment(&self, o: &Segment2D) -> SegmentIntersection {
153        let (p00, p01) = (self.p, self.p.add(&self.e));
154        let (p10, p11) = (o.p, o.p.add(&o.e));
155        if f64::max(p00.x, p01.x) < f64::min(p10.x, p11.x) - Self::EPS
156            || f64::min(p00.x, p01.x) > f64::max(p10.x, p11.x) + Self::EPS
157            || f64::max(p00.y, p01.y) < f64::min(p10.y, p11.y) - Self::EPS
158            || f64::min(p00.y, p01.y) > f64::max(p10.y, p11.y) + Self::EPS
159        {
160            return SegmentIntersection::None;
161        }
162        /***
163         * Solve the equation:
164         * u * self.e.x - v * o.e.x = o.p.x - self.p.x
165         * u * self.e.y - v * o.e.y = o.p.y - self.p.y
166         ***/
167        let d = -self.e.x * o.e.y + self.e.y * o.e.x;
168        if d.abs() < Self::EPS {
169            return match self.e.x * (o.p.y - self.p.y) - self.e.y * (o.p.x - self.p.x) {
170                val if val.abs() > Self::EPS => SegmentIntersection::None,
171                _ => {
172                    let w = self.e.unit().unwrap();
173                    let (o0, o1) = (p10.sub(&p00).dot(&w), p11.sub(&p00).dot(&w));
174                    let (s0, s1) = (0., self.e.len());
175                    if o0 < s0 {
176                        assert!(o1 + Self::EPS > s0);
177                        if o1 < s1 {
178                            SegmentIntersection::Segment(Segment2D::with_points(&p00, &p11))
179                        } else {
180                            SegmentIntersection::Segment(self.clone())
181                        }
182                    } else if o1 < s0 {
183                        assert!(o0 + Self::EPS > s0);
184                        if o0 < s1 {
185                            SegmentIntersection::Segment(Segment2D::with_points(&p00, &p10))
186                        } else {
187                            SegmentIntersection::Segment(self.clone())
188                        }
189                    } else {
190                        if o1 > s1 {
191                            SegmentIntersection::Segment(Segment2D::with_points(&p10, &p01))
192                        } else if o0 > s1 {
193                            SegmentIntersection::Segment(Segment2D::with_points(&p11, &p01))
194                        } else {
195                            SegmentIntersection::Segment(o.clone())
196                        }
197                    }
198                }
199            };
200        }
201
202        match (-(o.p.x - self.p.x) * o.e.y + (o.p.y - self.p.y) * o.e.x) / d {
203            u if u < -Self::EPS || u > 1.0 + Self::EPS => SegmentIntersection::None,
204            u if u < Self::EPS => SegmentIntersection::Point(p00),
205            u if u > 1.0 - Self::EPS => SegmentIntersection::Point(p01),
206            u => {
207                let intersection_point = self.p.add(&self.e.kmul(u));
208                SegmentIntersection::Point(intersection_point)
209            }
210        }
211    }
212
213    pub fn intersect_line(&self, l: &Line2D) -> SegmentIntersection {
214        /***
215         * solve the equation (<*,*> - dot product)
216         * <self.p + t * self.e, l.n> = l.a
217         *
218         * t * <self.e, l.n> = l.a - <self.p, l.n>
219         ***/
220        let p_offset = l.a - l.n.dot(&self.p.to_vec());
221        if self.e.dot(&l.n).abs() < Self::EPS {
222            if p_offset.abs() < Self::EPS {
223                SegmentIntersection::Segment(self.clone())
224            } else {
225                SegmentIntersection::None
226            }
227        } else {
228            match p_offset / l.n.dot(&self.e) {
229                t if t < -Self::EPS || t > 1. + Self::EPS => SegmentIntersection::None,
230                t if t < Self::EPS => SegmentIntersection::Point(self.p),
231                t => SegmentIntersection::Point(self.p.add(&self.e.kmul(t))),
232            }
233        }
234    }
235}
236
237impl Line2D {
238    const EPS: f64 = 1e-8;
239    pub fn new(normal: Vector2D, a: f64) -> Self {
240        Line2D { n: normal, a }
241    }
242    pub fn with_points(p0: &Point2D, p1: &Point2D) -> Self {
243        let n = p1.sub(p0).perpendicular();
244        let a = n.dot(&p0.to_vec());
245        Line2D { n, a }
246    }
247    pub fn point_relation(&self, p: &Point2D) -> PointLineRelation {
248        let p_offset = self.n.dot(&p.to_vec());
249        if p_offset < self.a - Self::EPS {
250            PointLineRelation::Left
251        } else if p_offset < self.a + Self::EPS {
252            PointLineRelation::OnLine
253        } else {
254            PointLineRelation::Right
255        }
256    }
257    pub fn segment_relation(&self, s: &Segment2D) -> SegmentLineRelation {
258        let (p0, p1) = (s.p, s.p.add(&s.e));
259        match (self.point_relation(&p0), self.point_relation(&p1)) {
260            (a, b) if a == b => match a {
261                PointLineRelation::Left => SegmentLineRelation::Left,
262                PointLineRelation::Right => SegmentLineRelation::Right,
263                PointLineRelation::OnLine => SegmentLineRelation::OnLine,
264            },
265            (PointLineRelation::OnLine, PointLineRelation::Left)
266            | (PointLineRelation::Left, PointLineRelation::OnLine) => {
267                SegmentLineRelation::LeftTouch
268            }
269            (PointLineRelation::OnLine, PointLineRelation::Right)
270            | (PointLineRelation::Right, PointLineRelation::OnLine) => {
271                SegmentLineRelation::RightTouch
272            }
273            _ => match s.intersect_line(self) {
274                SegmentIntersection::Point(p) => SegmentLineRelation::Intersects(p),
275                _ => {
276                    panic!("unreachable branch");
277                }
278            },
279        }
280    }
281}
282
283impl Polygon2D {
284    pub fn new(segments: Vec<Segment2D>) -> Self {
285        assert!(segments.len() >= 3);
286        Polygon2D { segments }
287    }
288
289    pub fn with_points(points: &[Point2D]) -> Self {
290        let mut segments = Vec::new();
291        let n = points.len();
292        assert!(n >= 3);
293        for i in 0..n {
294            segments.push(Segment2D::with_points(&points[i], &points[(i + 1) % n]));
295        }
296        Polygon2D { segments }
297    }
298
299    pub fn len(&self) -> usize {
300        self.segments.len()
301    }
302
303    pub fn points(&self) -> impl Iterator<Item = Point2D> + '_ {
304        self.segments.iter().map(|s| s.p)
305    }
306
307    pub fn intersect_with_semiplane(&self, l: &Line2D, left_semiplane: bool) -> Option<Polygon2D> {
308        enum TouchOrder {
309            First,
310            Second,
311        }
312        let l = if !left_semiplane {
313            l.clone()
314        } else {
315            Line2D::new(l.n.kmul(-1.), -l.a)
316        };
317        let mut out_segments = Vec::new();
318        let mut mem_points = Vec::new();
319        let mut handle_touch = |p: Point2D, segments: &mut Vec<Segment2D>, order: TouchOrder| {
320            if mem_points.is_empty() {
321                mem_points.push(p);
322            } else {
323                let p0 = mem_points.pop().unwrap();
324                segments.push(match order {
325                    TouchOrder::First => Segment2D::with_points(&p, &p0),
326                    TouchOrder::Second => Segment2D::with_points(&p0, &p),
327                });
328            }
329        };
330
331        for s in &self.segments {
332            let (p0, p1) = (s.p, s.p.add(&s.e));
333            match l.segment_relation(s) {
334                SegmentLineRelation::Right
335                | SegmentLineRelation::RightTouch
336                | SegmentLineRelation::OnLine => { /* skip it */ }
337                SegmentLineRelation::Left => {
338                    out_segments.push(s.clone());
339                }
340                SegmentLineRelation::LeftTouch => match l.point_relation(&p0) {
341                    PointLineRelation::OnLine => {
342                        handle_touch(p0, &mut out_segments, TouchOrder::Second);
343                        out_segments.push(s.clone());
344                    }
345                    _ => {
346                        out_segments.push(s.clone());
347                        handle_touch(p1, &mut out_segments, TouchOrder::First);
348                    }
349                },
350                SegmentLineRelation::Intersects(p) => match l.point_relation(&p0) {
351                    PointLineRelation::Left => {
352                        out_segments.push(Segment2D::with_points(&p0, &p));
353                        handle_touch(p, &mut out_segments, TouchOrder::First);
354                    }
355                    PointLineRelation::Right => {
356                        handle_touch(p, &mut out_segments, TouchOrder::Second);
357                        out_segments.push(Segment2D::with_points(&p, &p1));
358                    }
359                    _ => {
360                        panic!("unreachable branch");
361                    }
362                },
363            }
364        }
365
366        if !out_segments.is_empty() {
367            assert!(out_segments.len() >= 3);
368            Some(Polygon2D::new(out_segments))
369        } else {
370            None
371        }
372    }
373
374    pub fn intersect_with_semiplane_mut(&mut self, l: &Line2D, less_mode: bool) -> bool {
375        match self.intersect_with_semiplane(l, less_mode) {
376            None => false,
377            Some(p) => {
378                self.segments = p.segments;
379                true
380            }
381        }
382    }
383
384    pub fn skip_short_edge(&self, tol: f64) -> Self {
385        let mut segments = Vec::new();
386        segments.push(self.segments[0]);
387        for s in &self.segments[1..] {
388            if s.e.len() < tol {
389                let last = segments.len() - 1;
390                segments[last].e.add_mut(&s.e);
391            } else {
392                segments.push(s.clone());
393            }
394        }
395        /*
396         * specific implementation for the case of near points
397         * we should to support invariant len(segments) >= 3
398         */
399        if segments.len() <= 2 && self.segments.len() >= 3 {
400            let p0 = segments[0].p;
401            let p1 = segments[0].p.add(&segments[0].e);
402
403            let mut opt_p = self.segments[1].p;
404            let mut opt_dist = opt_p.distance_to(&p0).min(opt_p.distance_to(&p1));
405            for i in 2..self.segments.len() {
406                let p = self.segments[i].p;
407                let dist = p.distance_to(&p0).min(p.distance_to(&p1));
408                if dist > opt_dist {
409                    opt_p = p;
410                    opt_dist = dist;
411                }
412            }
413            return Polygon2D::with_points(&[p0, opt_p, p1]);
414        }
415
416        Polygon2D { segments }
417    }
418}
simple_geom/lib.rs

simple_geom/
lib.rs
