@@ -233,20 +233,55 @@ Eigen::Vector3f cross(Eigen::Vector3f v1, Eigen::Vector3f v2)
233233 return v1.cross (v2);
234234}
235235
236+ bool on_segment (const Eigen::Vector2f &c1, const Eigen::Vector2f &c2, const Eigen::Vector2f &p) {
237+ // Given three collinear points c1, c2, p, the function checks if
238+ // point p lies on line segment c1 <-> c2
239+
240+ float x_down, x_up, y_down, y_up;
241+ x_down = fminf (c1.coeff (0 ), c2.coeff (0 ));
242+ x_up = fmaxf (c1.coeff (0 ), c2.coeff (0 ));
243+ y_down = fminf (c1.coeff (1 ), c2.coeff (1 ));
244+ y_up = fmaxf (c1.coeff (1 ), c2.coeff (1 ));
245+ return (x_down <= p.coeff (0 ) && p.coeff (0 ) <= x_up && y_down <= p.coeff (1 ) && p.coeff (1 ) <= y_up);
246+ }
236247
237- int is_inside_2d_polygon (const Eigen::Vector2f &p,
248+ inline int is_left (const Eigen::Vector2f &P0, const Eigen::Vector2f &P1, const Eigen::Vector2f &P2)
249+ {
250+ // isLeft(): tests if a point is Left|On|Right of an infinite line.
251+ // on the line is defined as within epsilon
252+ // Input: three points P0, P1, and P2
253+ // Return: >0 for P2 left of the line through P0 and P1
254+ // =0 for P2 on the line
255+ // <0 for P2 right of the line
256+ // See: Algorithm 1 "Area of Triangles and Polygons"
257+ float test_value = (
258+ (P1.coeff (0 ) - P0.coeff (0 )) * (P2.coeff (1 ) - P0.coeff (1 ))
259+ - (P2.coeff (0 ) - P0.coeff (0 )) * (P1.coeff (1 ) - P0.coeff (1 ))
260+ );
261+
262+ if (fabsf (test_value) < libroom_eps)
263+ return 0 ;
264+ else if (test_value > 0 )
265+ return 1 ;
266+ else
267+ return -1 ;
268+ }
269+
270+ int is_inside_2d_polygon (const Eigen::Vector2f &p_test,
238271 const Eigen::Matrix<float ,2 ,Eigen::Dynamic> &corners)
239272{
240273 /*
241- Checks if a given point is inside a given polygon in 2D.
274+ Checks if a given point is inside a given polygon in 2D using the winding
275+ number method.
242276
243277 This function checks if a point (defined by its coordinates) is inside
244278 a polygon (defined by an array of coordinates of its corners) by counting
245- the number of intersections between the borders and a segment linking
246- the given point with a computed point outside the polygon.
247- A boolean is also returned to indicate if a point is on a border of the
248- polygon (the point is still considered inside), which can be useful for
249- limit cases computations.
279+ the number of left intersections between the edges and a half-line starting from
280+ the test point.
281+
282+ This is Dave Sunday's winding number algorithm described here:
283+ http://profs.ic.uff.br/~anselmo/cursos/CGI/slidesNovos/Inclusion%20of%20a%20Point%20in%20a%20Polygon.pdf
284+ It was modified to explicitely check for points on the edges of the polygon.
250285
251286 p: (array size 2) coordinates of the point
252287 corners: (array size 2xN, N>2) coordinates of the corners of the polygon
@@ -257,67 +292,35 @@ int is_inside_2d_polygon(const Eigen::Vector2f &p,
257292 0 : the point is inside
258293 1 : the point is on the boundary
259294 */
260-
261- bool is_inside = false ; // initialize point not in the polygon
262- int c1c2p, c1c2p0, pp0c1, pp0c2;
263295 int n_corners = corners.cols ();
264-
265- // find a point outside the polygon
266- int i_min;
267- corners.row (0 ).minCoeff (&i_min);
268- Eigen::Vector2f p_out;
269- p_out.resize (2 );
270- p_out.coeffRef (0 ) = corners.coeff (0 ,i_min) - 1 ;
271- p_out.coeffRef (1 ) = p.coeff (1 );
272-
273- // Now count intersections
296+ int wn = 0 ; // the winding number counter
297+ // loop through all edges of the polygon
274298 for (int i = 0 , j = n_corners-1 ; i < n_corners ; j=i++)
275299 {
276-
277- // Check first if the point is on the segment
278- // We count the border as inside the polygon
279- c1c2p = ccw3p (corners.col (i), corners.col (j), p);
280- if (c1c2p == 0 )
300+ if (ccw3p (corners.col (j), corners.col (i), p_test) == 0 && on_segment (corners.col (j), corners.col (i), p_test))
281301 {
282- // Here we know that p is co-linear with the two corners
283- float x_down, x_up, y_down, y_up;
284- x_down = fminf (corners.coeff (0 ,i), corners.coeff (0 ,j));
285- x_up = fmaxf (corners.coeff (0 ,i), corners.coeff (0 ,j));
286- y_down = fminf (corners.coeff (1 ,i), corners.coeff (1 ,j));
287- y_up = fmaxf (corners.coeff (1 ,i), corners.coeff (1 ,j));
288- if (x_down <= p.coeff (0 ) && p.coeff (0 ) <= x_up && y_down <= p.coeff (1 ) && p.coeff (1 ) <= y_up)
289- return 1 ;
302+ // point is on the edge, we consider it inside
303+ return 1 ;
290304 }
291-
292- // Now check intersection with standard method
293- c1c2p0 = ccw3p (corners.col (i), corners.col (j), p_out);
294- if (c1c2p == c1c2p0) // no intersection
295- continue ;
296-
297- pp0c1 = ccw3p (p, p_out, corners.col (i));
298- pp0c2 = ccw3p (p, p_out, corners.col (j));
299- if (pp0c1 == pp0c2) // no intersection
300- continue ;
301-
302- // at this point we are sure there is an intersection
303-
304- // the second condition takes care of horizontal edges and intersection on vertex
305- float c_max = fmaxf (corners.coeff (1 ,i), corners.coeff (1 ,j));
306- if (p.coeff (1 ) + libroom_eps < c_max)
307- {
308- is_inside = !is_inside;
305+ if (corners.coeff (1 ,j) <= p_test.coeff (1 )) { // start y <= P.y
306+ if (corners.coeff (1 ,i) > p_test.coeff (1 )) { // an upward crossing
307+ if (is_left (corners.col (j), corners.col (i), p_test) > 0 ) // P left of edge
308+ ++wn; // have a valid up intersect
309+ }
310+ } else { // start y > P.y (no test needed)
311+ if (corners.coeff (1 , i) <= p_test.coeff (1 )) { // a downward crossing
312+ if (is_left (corners.col (j), corners.col (i), p_test) < 0 ) // P right of edge
313+ --wn; // have a valid down intersect
314+ }
309315 }
310-
311316 }
312317
313- // for a odd number of intersections, the point is in the polygon
314- if (is_inside)
315- return 0 ; // point strictly inside
318+ if (wn == 0 )
319+ return -1 ;
316320 else
317- return - 1 ; // point is outside
321+ return 0 ;
318322}
319323
320-
321324float area_2d_polygon (const Eigen::Matrix<float , 2 , Eigen::Dynamic> &corners)
322325{
323326 /*
0 commit comments