1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
use std::cmp::Ordering;

use crate::data::{
  Cursor, DirectedEdge, Direction, HalfLineSoS, IHalfLineLineSegmentSoS, Line, Point, Polygon,
};
use crate::{Intersects, Orientation, PolygonScalar};

// Note: The description on wikipedia is quite bad.
// Wikipedia: https://en.wikipedia.org/wiki/Visibility_polygon#Naive_algorithms
// Star polygons: https://en.wikipedia.org/wiki/Star-shaped_polygon
//
// Note about SoS rays (aka HalfLineSoS):
//   SoS rays lean either to the left or to the right. This means they always hit the sides
//   of an edge rather than the vertices. For example:
//
//     a <--- ray
//     |
//     b
//
//   Here we have an edge ('a' to 'b') and a ray that points directly to 'a'. If the ray
//   leans to the left then it will hit to the left of 'a'. That is, it'll hit between
//   'a' and 'b' rather than hitting 'a' directly.
//   If it leans to the right then it'll miss the 'a'-'b' edge altogether.
//   SoS rays are essential for dealing correctly with colinear vertices.
//
//                 a
//   SoS ray --->  |      => Crossing(CoLinear), ray completely blocked.
//                 b
//
//   SoS ray --->  a      => Crossing(ClockWise), ray blocked on the right side.
//                / \
//               b   c
//
//               b   c
//                \ /
//   SoS ray --->  a      => Crossing(CounterClockWise), ray blocked on the left side.
//
//   SoS ray --->  a-b    => None, ray not blocked at all.
//
// Note about visibility polygons:
//   Visibility polygons are always star-shaped: All vertices are visible from the point
//   of origin. This means that we can find the visible vertices in any order we like and
//   then sort them around the point of origin to generate the polygon.
//
// Algorithm overview:
//   1. For each vertex in the polygon (including outer boundary and any holes):
//     1.A Shoot a right-leaning SoS ray towards the vertex and record the intersection
//         point closest to the origin. Note: The SoS ray might not hit the vertex.
//     1.B Shoot a left-leaning SoS ray towards the vertex and record the intersection
//         point closest to the origin. This point may or may not be the same as the point
//         in step 1.A.
//   2. Sort all the intersection points counter-clockwise around the point of origin.
//   3. If two points are at the same angle then sort according to the leaning of the
//      ray that generated the intersection point. Ie. a point from a left-leaning ray
//      should be Ordering::Greater than a point from a right-leaning ray.
//
// Pseudo code:
//   for vertex in poly.vertex {
//     let left_intersections = ... use left-leaning SoS ray ...;
//     let right_intersections = ... use right-leaning SoS ray ...;
//     let left_closest = ...;
//     let right_closest = ...;
//     output.push( (SoS::CounterClockWise, left_closest));
//     output.push( (SoS::ClockWise, right_closest));
//   }
//   output.sort_by(|a,b| origin.ccw_cmp_around(a,b).then(cmp by sos) );
//   Polygon::new(output)
//
// Test cases:
//
//   Input            Output
//
//  /-----\ /----\  /-----\ /----\
//  |     | |    |  |     | |    |
//  |  x  \-/ /--/  |  x  \---\--/
//  |         |     |         |
//  \---------/     \---------/
//
//
//  /-----\ /----\  /-----\ /----\
//  |     | |    |  |     | |    |
//  |  x  \-/    |  |  x  \------\
//  |            |  |            |
//  \------------/  \------------/
//
//  /---------\     /-\     /-\
//  |   /-\   |     |  \   /  |
//  |   \-/   |     |   \ /   |
//  |    x    |     |    x    |
//  \---------/     \---------/
/// Naive alogrithn for calculating visibility polygon
pub fn get_visibility_polygon<T>(point: &Point<T>, polygon: &Polygon<T>) -> Option<Polygon<T>>
where
  T: PolygonScalar,
{
  // FIXME: We want to iterate over all vertices, not just boundary vertices.
  let mut vertices: Vec<Cursor<'_, T>> = polygon.iter_boundary().collect();
  vertices.sort_by(|a, b| point.ccw_cmp_around(a, b));

  let mut polygon_points = Vec::new();
  for vertex in vertices {
    let ray_sos = HalfLineSoS::new_through(point, vertex.point());
    let mut right_intersection = NearestIntersection::new(point);
    let mut left_intersection = NearestIntersection::new(point);

    // FIXME: We want to iterate over all edges, not just boundary edges.
    for edge in polygon.iter_boundary_edges() {
      use IHalfLineLineSegmentSoS::*;
      use Orientation::*;
      match ray_sos.intersect(edge) {
        None => (),
        // CoLinear crosing blocks the ray both to the left and to the right
        Some(Crossing(CoLinear)) => {
          let isect = get_intersection(ray_sos, edge);
          left_intersection.push(isect.clone());
          right_intersection.push(isect);
        }
        // Ray blocked on the left side.
        Some(Crossing(CounterClockWise)) => {
          left_intersection.push(get_intersection_colinear(ray_sos, edge));
        }
        // Ray blocked on the right side.
        Some(Crossing(ClockWise)) => {
          right_intersection.push(get_intersection_colinear(ray_sos, edge));
        }
      }
    }

    match right_intersection.take() {
      Some(interesection) => {
        if point.cmp_distance_to(&interesection, &vertex) != Ordering::Less {
          polygon_points.push(interesection);
        }
      }
      None => return Option::None,
    };
    match left_intersection.take() {
      Some(intersection) => {
        if point.cmp_distance_to(&intersection, &vertex) != Ordering::Less {
          polygon_points.push(intersection);
        }
      }
      None => return Option::None,
    };
  }
  polygon_points.dedup();

  Some(Polygon::new(polygon_points).expect("Polygon Creation failed"))
}

fn get_intersection_colinear<T>(sos_line: HalfLineSoS<T>, edge: DirectedEdge<'_, T>) -> Point<T>
where
  T: PolygonScalar,
{
  let line: Line<T> = sos_line.into();
  match Point::orient_along_direction(line.origin, line.direction, edge.src) {
    Orientation::CoLinear => edge.src.clone(),
    // edge.dst should be colinear with the ray
    _ => edge.dst.clone(),
  }
}

fn get_intersection<T>(sos_line: HalfLineSoS<T>, edge: DirectedEdge<'_, T>) -> Point<T>
where
  T: PolygonScalar,
{
  let segment_line = Line {
    origin: edge.src,
    direction: Direction::Through(edge.dst),
  };
  Line::from(sos_line)
    .intersection_point(&segment_line)
    .expect("LinesMustIntersect")
}

// Container for intersections that only store the nearest point to some origin.
struct NearestIntersection<'a, T> {
  origin: &'a Point<T>,
  nearest_intersection: Option<Point<T>>,
}

impl<'a, T> NearestIntersection<'a, T>
where
  T: PolygonScalar,
{
  fn new(origin: &'a Point<T>) -> NearestIntersection<'a, T> {
    NearestIntersection {
      origin,
      nearest_intersection: None,
    }
  }

  fn push(&mut self, mut intersection: Point<T>) {
    match self.nearest_intersection.as_mut() {
      None => self.nearest_intersection = Some(intersection),
      Some(previous) => {
        if self.origin.cmp_distance_to(&intersection, previous) == Ordering::Less {
          std::mem::swap(previous, &mut intersection);
        }
      }
    }
  }

  fn take(self) -> Option<Point<T>> {
    self.nearest_intersection
  }
}

#[cfg(test)]
mod naive_testing {
  use super::*;

  //     x
  //  /---------\
  //  |         |
  //  |         |
  //  \---------/
  #[test]
  fn point_outside_polygon() {
    let point = Point::new([2, 8]);
    let input_polygon = Polygon::new(vec![
      Point::new([0, 0]),
      Point::new([8, 0]),
      Point::new([8, 6]),
      Point::new([0, 6]),
    ])
    .unwrap();
    let out_polygon = get_visibility_polygon(&point, &input_polygon);

    assert!(out_polygon.is_none())
  }
  //  /-----\ /----\  /-----\ /----\
  //  |     | |    |  |     | |    |
  //  |  x  \-/ /--/  |  x  \---\--/
  //  |         |     |         |
  //  \---------/     \---------/
  #[test]
  fn case_1() {
    let point = Point::new([2, 3]);
    let input_polygon = Polygon::new(vec![
      Point::new([0, 0]),
      Point::new([8, 0]),
      Point::new([8, 3]),
      Point::new([10, 3]),
      Point::new([10, 6]),
      Point::new([6, 6]),
      Point::new([6, 3]),
      Point::new([4, 3]),
      Point::new([4, 6]),
      Point::new([0, 6]),
    ])
    .unwrap();
    let out_test_points = vec![
      Point::new([0, 0]),
      Point::new([8, 0]),
      Point::new([8, 3]),
      Point::new([4, 3]),
      Point::new([4, 6]),
      Point::new([0, 6]),
    ];
    let out_polygon = get_visibility_polygon(&point, &input_polygon);
    match out_polygon {
      Some(polygon) => {
        //ToDo: find a better way to compare polygons (maybe pairwise comparison of the points)
        assert_eq!(out_test_points.len(), polygon.points.len())
      }
      None => panic!(),
    }
  }

  //   Input            Output
  //  /-----\ /----\  /-----\ /----\
  //  |     | |    |  |     | |    |
  //  |  x  \-/    |  |  x  \------\
  //  |            |  |            |
  //  \------------/  \------------/
  #[test]
  fn case_2() {
    let point = Point::new([2, 3]);
    let input_polygon = Polygon::new(vec![
      Point::new([0, 0]),
      Point::new([10, 0]),
      Point::new([10, 6]),
      Point::new([6, 6]),
      Point::new([6, 3]),
      Point::new([4, 3]),
      Point::new([4, 6]),
      Point::new([0, 6]),
    ])
    .unwrap();
    let out_test_points = vec![
      Point::new([0, 0]),
      Point::new([10, 0]),
      Point::new([10, 3]),
      Point::new([4, 3]),
      Point::new([4, 6]),
      Point::new([0, 6]),
    ];
    let out_polygon = get_visibility_polygon(&point, &input_polygon);
    match out_polygon {
      Some(polygon) => {
        assert_eq!(out_test_points.len(), polygon.points.len())
      }
      None => panic!(),
    }
  }

  // test with rotating square
  #[test]
  fn test_rotating_square() {
    use std::f64::consts::{FRAC_PI_2, PI};

    fn get_point(theta: f64) -> Point<f64> {
      Point::new([theta.sin(), theta.cos()])
    }

    for i in 0..100 {
      let theta = PI / 2.0 * i as f64 / 100.0;
      let points = vec![
        get_point(theta),
        get_point(theta - FRAC_PI_2),
        get_point(theta - FRAC_PI_2 * 2.0),
        get_point(theta - FRAC_PI_2 * 3.0),
      ];

      let point = Point::new([0.0, 0.0]);
      let polygon = Polygon::new(points.clone()).unwrap();
      let out_polygon = get_visibility_polygon(&point, &polygon).expect("get_visibility_polygon");
      assert_eq!(points, out_polygon.points);
    }
  }
}