一种求多边形的边上所有间隔一定距离的点坐标方法

一种求多边形的边上所有间隔一定距离的点坐标方法

给一个任意多边形，得到每条边上间隔一定距离所有的点
环境：ros 、c++
效果图：
点间隔10cm

代码如下：
geometry_msgs::Polygon 为多边形类型。
std::vector<geometry_msgs::Point32> &为要求的点存放的地方的引用。
首先定义geometry_msgs::Polygon 和std::vector<geometry_msgs::Point32> &points
类型数据，如：
geometry_msgs::Polygon polygon ;
std::vector<geometry_msgs::Point32> &points;
然后调用：
getPointFromPolygon(polygon,points);
在points中就得到要求的所有的点。

bool getPointFromPolygon(geometry_msgs::Polygon polygon, std::vector<geometry_msgs::Point32> &points)
{size_t point_size = polygon.points.size();for(size_t i=0;i<point_size-1;i++){getPointFromLine(polygon.points[i],polygon.points[i+1],points);}getPointFromLine(polygon.points[point_size-1],polygon.points[0],points);return true;
}bool getPointFromLine(geometry_msgs::Point32 point0, geometry_msgs::Point32 point1, std::vector<geometry_msgs::Point32> &points)
{double dist = 0.1; //点间隔距离double theta  = 0,incr_x = 0,incr_y = 0;if(fabs(point1.x-point0.x) < 0.0000001){theta = M_PI_2;}else{theta = atan2(point1.y-point0.y,point1.x-point0.x);}incr_x = fabs(dist*cos(theta));incr_y = fabs(dist*sin(theta));geometry_msgs::Point32 tem_point;double diff_x = point1.x - point0.x;double diff_y = point1.y - point0.y;size_t point_size = (size_t)( sqrt(diff_x*diff_x+diff_y*diff_y)/dist+0.5 );if(point_size < 1){points.push_back(point0);points.push_back(point1);return true;}double x_vec[point_size+2]={0};double y_vec[point_size+2]={0};if(point0.x < point1.x){for(size_t i=0;i<point_size;i++){x_vec[i] = point0.x + i*incr_x;}}else{for(size_t i=0;i<point_size;i++){x_vec[i] = point0.x - i*incr_x;}}if(point0.y < point1.y){for(size_t i=0;i<point_size;i++){y_vec[i] = point0.y + i*incr_y;}}else{for(size_t i=0;i<point_size;i++){y_vec[i] = point0.y - i*incr_y;}}for(size_t i=0;i<point_size;i++){tem_point.x = x_vec[i];tem_point.y = y_vec[i];points.push_back(tem_point);}return true;
}