35 #ifndef _POLYGON_SIMPLIFICATION_CPP_
36 #define _POLYGON_SIMPLIFICATION_CPP_
57 double prev_x=-999999, prev_y=-999999;
58 ROS_WARN(
"polygon_simplification: polygon is not simple. Printing all vertexes");
61 for (PS_CGALPolygon_2::Vertex_const_iterator j = p->vertices_begin(); j != p->vertices_end(); ++ j )
68 PS_CGALPolygon_2::Vertex_const_iterator begin = p->vertices_begin();
69 PS_CGALPolygon_2::Vertex_const_iterator end = --p->vertices_end();
70 if ( begin->x() == end->x() && begin->y() == end->y())
72 ROS_WARN(
"Final and start point are equal. Erasing final point");
77 for (PS_CGALPolygon_2::Vertex_const_iterator j = p->vertices_begin(); j != p->vertices_end(); ++ j )
81 if (prev_x==j->x() && prev_y==j->y())
83 ROS_INFO(
"Erasing vertex %d",tt);
89 prev_x = CGAL::to_double(j->x());
90 prev_y = CGAL::to_double(j->y());
96 prev_x = CGAL::to_double(j->x());
97 prev_y = CGAL::to_double(j->y());
106 ROS_WARN(
"offset polygon %d: result of fix: %d",(
int)p->size(), p->is_simple());
109 for (PS_CGALPolygon_2::Vertex_const_iterator j = p->vertices_begin(); j != p->vertices_end(); ++ j )
115 if (p->is_simple()) keep_trying=0;
126 if(!p->is_counterclockwise_oriented())
128 ROS_INFO(
"Offset Polygon: ch (%d points) is not counterclockwise oriented. Reversing orientation",(
int)p->size());
129 p->reverse_orientation();
130 ROS_INFO(
"Result Polygon: ch (%d points)",(
int)p->size());
134 ROS_INFO(
"Removing collinear points... %d", (
int)p->size());
141 PS_CGALPolygon_2::Vertex_const_iterator j = p->vertices_begin();
142 double x2 = CGAL::to_double((*j).x());
143 double y2 = CGAL::to_double((*j).y());
146 double x1 = CGAL::to_double((*j).x());
147 double y1 = CGAL::to_double((*j).y());
150 for (; j != p->vertices_end(); ++ j )
152 double m1 = (y2-y1)/(x2-x1);
153 double m2 = ( CGAL::to_double((*j).y()) - y1)/( CGAL::to_double((*j).x())-x1);
155 if (fabs(m1-m2)<0.01)
166 x1= CGAL::to_double((*j).x());
167 y1= CGAL::to_double((*j).y());
171 ROS_INFO(
"End removing collinear points... %d", (
int)p->size());
Defines the class polygon simplification.
~class_polygon_simplification(void)
int simplify_polygon(PS_CGALPolygon_2 *p)
CGAL::Polygon_2< PS_CGALKernel > PS_CGALPolygon_2
class_polygon_simplification(void)