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

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

给一个任意多边形，得到每条边上间隔一定距离所有的点
环境：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;
}