rgeometry/algorithms/visibility/
naive.rs

1use std::cmp::Ordering;
2
3use crate::data::{
4  Cursor, DirectedEdge, Direction, HalfLineSoS, IHalfLineLineSegmentSoS, Line, Point, Polygon,
5};
6use crate::{Intersects, Orientation, PolygonScalar};
7
8// Note: The description on wikipedia is quite bad.
9// Wikipedia: https://en.wikipedia.org/wiki/Visibility_polygon#Naive_algorithms
10// Star polygons: https://en.wikipedia.org/wiki/Star-shaped_polygon
11//
12// Note about SoS rays (aka HalfLineSoS):
13//   SoS rays lean either to the left or to the right. This means they always hit the sides
14//   of an edge rather than the vertices. For example:
15//
16//     a <--- ray
17//     |
18//     b
19//
20//   Here we have an edge ('a' to 'b') and a ray that points directly to 'a'. If the ray
21//   leans to the left then it will hit to the left of 'a'. That is, it'll hit between
22//   'a' and 'b' rather than hitting 'a' directly.
23//   If it leans to the right then it'll miss the 'a'-'b' edge altogether.
24//   SoS rays are essential for dealing correctly with colinear vertices.
25//
26//                 a
27//   SoS ray --->  |      => Crossing(CoLinear), ray completely blocked.
28//                 b
29//
30//   SoS ray --->  a      => Crossing(ClockWise), ray blocked on the right side.
31//                / \
32//               b   c
33//
34//               b   c
35//                \ /
36//   SoS ray --->  a      => Crossing(CounterClockWise), ray blocked on the left side.
37//
38//   SoS ray --->  a-b    => None, ray not blocked at all.
39//
40// Note about visibility polygons:
41//   Visibility polygons are always star-shaped: All vertices are visible from the point
42//   of origin. This means that we can find the visible vertices in any order we like and
43//   then sort them around the point of origin to generate the polygon.
44//
45// Algorithm overview:
46//   1. For each vertex in the polygon (including outer boundary and any holes):
47//     1.A Shoot a right-leaning SoS ray towards the vertex and record the intersection
48//         point closest to the origin. Note: The SoS ray might not hit the vertex.
49//     1.B Shoot a left-leaning SoS ray towards the vertex and record the intersection
50//         point closest to the origin. This point may or may not be the same as the point
51//         in step 1.A.
52//   2. Sort all the intersection points counter-clockwise around the point of origin.
53//   3. If two points are at the same angle then sort according to the leaning of the
54//      ray that generated the intersection point. Ie. a point from a left-leaning ray
55//      should be Ordering::Greater than a point from a right-leaning ray.
56//
57// Pseudo code:
58//   for vertex in poly.vertex {
59//     let left_intersections = ... use left-leaning SoS ray ...;
60//     let right_intersections = ... use right-leaning SoS ray ...;
61//     let left_closest = ...;
62//     let right_closest = ...;
63//     output.push( (SoS::CounterClockWise, left_closest));
64//     output.push( (SoS::ClockWise, right_closest));
65//   }
66//   output.sort_by(|a,b| origin.ccw_cmp_around(a,b).then(cmp by sos) );
67//   Polygon::new(output)
68//
69// Test cases:
70//
71//   Input            Output
72//
73//  /-----\ /----\  /-----\ /----\
74//  |     | |    |  |     | |    |
75//  |  x  \-/ /--/  |  x  \---\--/
76//  |         |     |         |
77//  \---------/     \---------/
78//
79//
80//  /-----\ /----\  /-----\ /----\
81//  |     | |    |  |     | |    |
82//  |  x  \-/    |  |  x  \------\
83//  |            |  |            |
84//  \------------/  \------------/
85//
86//  /---------\     /-\     /-\
87//  |   /-\   |     |  \   /  |
88//  |   \-/   |     |   \ /   |
89//  |    x    |     |    x    |
90//  \---------/     \---------/
91/// Naive alogrithn for calculating visibility polygon
92pub fn get_visibility_polygon<T>(point: &Point<T>, polygon: &Polygon<T>) -> Option<Polygon<T>>
93where
94  T: PolygonScalar,
95{
96  // FIXME: We want to iterate over all vertices, not just boundary vertices.
97  let mut vertices: Vec<Cursor<'_, T>> = polygon.iter_boundary().collect();
98  vertices.sort_by(|a, b| point.ccw_cmp_around(a, b));
99
100  let mut polygon_points = Vec::new();
101  for vertex in vertices {
102    let ray_sos = HalfLineSoS::new_through(point, vertex.point());
103    let mut right_intersection = NearestIntersection::new(point);
104    let mut left_intersection = NearestIntersection::new(point);
105
106    // FIXME: We want to iterate over all edges, not just boundary edges.
107    for edge in polygon.iter_boundary_edges() {
108      use IHalfLineLineSegmentSoS::*;
109      use Orientation::*;
110      match ray_sos.intersect(edge) {
111        None => (),
112        // CoLinear crosing blocks the ray both to the left and to the right
113        Some(Crossing(CoLinear)) => {
114          let isect = get_intersection(ray_sos, edge);
115          left_intersection.push(isect.clone());
116          right_intersection.push(isect);
117        }
118        // Ray blocked on the left side.
119        Some(Crossing(CounterClockWise)) => {
120          left_intersection.push(get_intersection_colinear(ray_sos, edge));
121        }
122        // Ray blocked on the right side.
123        Some(Crossing(ClockWise)) => {
124          right_intersection.push(get_intersection_colinear(ray_sos, edge));
125        }
126      }
127    }
128
129    match right_intersection.take() {
130      Some(interesection) => {
131        if point.cmp_distance_to(&interesection, &vertex) != Ordering::Less {
132          polygon_points.push(interesection);
133        }
134      }
135      None => return Option::None,
136    };
137    match left_intersection.take() {
138      Some(intersection) => {
139        if point.cmp_distance_to(&intersection, &vertex) != Ordering::Less {
140          polygon_points.push(intersection);
141        }
142      }
143      None => return Option::None,
144    };
145  }
146  polygon_points.dedup();
147
148  Some(Polygon::new(polygon_points).expect("Polygon Creation failed"))
149}
150
151fn get_intersection_colinear<T>(sos_line: HalfLineSoS<T>, edge: DirectedEdge<'_, T>) -> Point<T>
152where
153  T: PolygonScalar,
154{
155  let line: Line<T> = sos_line.into();
156  match Point::orient_along_direction(line.origin, line.direction, edge.src) {
157    Orientation::CoLinear => edge.src.clone(),
158    // edge.dst should be colinear with the ray
159    _ => edge.dst.clone(),
160  }
161}
162
163fn get_intersection<T>(sos_line: HalfLineSoS<T>, edge: DirectedEdge<'_, T>) -> Point<T>
164where
165  T: PolygonScalar,
166{
167  let segment_line = Line {
168    origin: edge.src,
169    direction: Direction::Through(edge.dst),
170  };
171  Line::from(sos_line)
172    .intersection_point(&segment_line)
173    .expect("LinesMustIntersect")
174}
175
176// Container for intersections that only store the nearest point to some origin.
177struct NearestIntersection<'a, T> {
178  origin: &'a Point<T>,
179  nearest_intersection: Option<Point<T>>,
180}
181
182impl<'a, T> NearestIntersection<'a, T>
183where
184  T: PolygonScalar,
185{
186  fn new(origin: &'a Point<T>) -> NearestIntersection<'a, T> {
187    NearestIntersection {
188      origin,
189      nearest_intersection: None,
190    }
191  }
192
193  fn push(&mut self, mut intersection: Point<T>) {
194    match self.nearest_intersection.as_mut() {
195      None => self.nearest_intersection = Some(intersection),
196      Some(previous) => {
197        if self.origin.cmp_distance_to(&intersection, previous) == Ordering::Less {
198          std::mem::swap(previous, &mut intersection);
199        }
200      }
201    }
202  }
203
204  fn take(self) -> Option<Point<T>> {
205    self.nearest_intersection
206  }
207}
208
209#[cfg(test)]
210mod naive_testing {
211  use super::*;
212
213  //     x
214  //  /---------\
215  //  |         |
216  //  |         |
217  //  \---------/
218  #[test]
219  fn point_outside_polygon() {
220    let point = Point::new([2, 8]);
221    let input_polygon = Polygon::new(vec![
222      Point::new([0, 0]),
223      Point::new([8, 0]),
224      Point::new([8, 6]),
225      Point::new([0, 6]),
226    ])
227    .unwrap();
228    let out_polygon = get_visibility_polygon(&point, &input_polygon);
229
230    assert!(out_polygon.is_none())
231  }
232  //  /-----\ /----\  /-----\ /----\
233  //  |     | |    |  |     | |    |
234  //  |  x  \-/ /--/  |  x  \---\--/
235  //  |         |     |         |
236  //  \---------/     \---------/
237  #[test]
238  fn case_1() {
239    let point = Point::new([2, 3]);
240    let input_polygon = Polygon::new(vec![
241      Point::new([0, 0]),
242      Point::new([8, 0]),
243      Point::new([8, 3]),
244      Point::new([10, 3]),
245      Point::new([10, 6]),
246      Point::new([6, 6]),
247      Point::new([6, 3]),
248      Point::new([4, 3]),
249      Point::new([4, 6]),
250      Point::new([0, 6]),
251    ])
252    .unwrap();
253    let out_test_points = [
254      Point::new([0, 0]),
255      Point::new([8, 0]),
256      Point::new([8, 3]),
257      Point::new([4, 3]),
258      Point::new([4, 6]),
259      Point::new([0, 6]),
260    ];
261    let out_polygon = get_visibility_polygon(&point, &input_polygon);
262    match out_polygon {
263      Some(polygon) => {
264        //ToDo: find a better way to compare polygons (maybe pairwise comparison of the points)
265        assert_eq!(out_test_points.len(), polygon.points.len())
266      }
267      None => panic!(),
268    }
269  }
270
271  //   Input            Output
272  //  /-----\ /----\  /-----\ /----\
273  //  |     | |    |  |     | |    |
274  //  |  x  \-/    |  |  x  \------\
275  //  |            |  |            |
276  //  \------------/  \------------/
277  #[test]
278  fn case_2() {
279    let point = Point::new([2, 3]);
280    let input_polygon = Polygon::new(vec![
281      Point::new([0, 0]),
282      Point::new([10, 0]),
283      Point::new([10, 6]),
284      Point::new([6, 6]),
285      Point::new([6, 3]),
286      Point::new([4, 3]),
287      Point::new([4, 6]),
288      Point::new([0, 6]),
289    ])
290    .unwrap();
291    let out_test_points = [
292      Point::new([0, 0]),
293      Point::new([10, 0]),
294      Point::new([10, 3]),
295      Point::new([4, 3]),
296      Point::new([4, 6]),
297      Point::new([0, 6]),
298    ];
299    let out_polygon = get_visibility_polygon(&point, &input_polygon);
300    match out_polygon {
301      Some(polygon) => {
302        assert_eq!(out_test_points.len(), polygon.points.len())
303      }
304      None => panic!(),
305    }
306  }
307
308  // test with rotating square
309  #[test]
310  fn test_rotating_square() {
311    use std::f64::consts::{FRAC_PI_2, PI};
312
313    fn get_point(theta: f64) -> Point<f64> {
314      Point::new([theta.sin(), theta.cos()])
315    }
316
317    for i in 0..100 {
318      let theta = PI / 2.0 * i as f64 / 100.0;
319      let points = vec![
320        get_point(theta),
321        get_point(theta - FRAC_PI_2),
322        get_point(theta - FRAC_PI_2 * 2.0),
323        get_point(theta - FRAC_PI_2 * 3.0),
324      ];
325
326      let point = Point::new([0.0, 0.0]);
327      let polygon = Polygon::new(points.clone()).unwrap();
328      let out_polygon = get_visibility_polygon(&point, &polygon).expect("get_visibility_polygon");
329      assert_eq!(points, out_polygon.points);
330    }
331  }
332}
rgeometry/algorithms/visibility/naive.rs

rgeometry/algorithms/visibility/
naive.rs
