缩小矩形后的坐标点

缩小一个矩形后的坐标点

在一个坐标系中给出一个矩形的四个顶点坐标，求将这个矩形缩小一定范围后的坐标。
矩形要求：四个定点按顺时针或者逆时针排列P0，P1，P2，P3。

具体操作步骤

1、旋转

以P0为旋转点，以P0指向P1向量的角度为旋转角度旋转，角度为atan((P1.y-P0.y),(P1.x-P0.x))
注：逆时针旋转乘上正角度，顺时针旋转乘上负角度值。
旋转后的点计做：R_P0,R_P1,R_P2,R_P3.

2、计算旋转后缩小后的坐标

根据P0指向P2向量角度的正负分两种情况计算。在R_P0,R_P1,R_P2,R_P3的坐标值直接相加减即可。

3、反向旋转

将R_P0,R_P1,R_P2,R_P3反向旋转回来即可。

代码实现

void shrinkPolygon(geometry_msgs::Polygon &out, double shrink_meter)
{
  const geometry_msgs::Polygon in = out;
  geometry_msgs::Polygon rotate_polygon;
  rotate_polygon.points.resize(4);
  geometry_msgs::Polygon rotate_shrink_polygon;
  rotate_shrink_polygon.points.resize(4);

  geometry_msgs::Point32 tem_point32;
  geometry_msgs::Point32 rotate_point = in.points[0];
  double theta = atan2((in.points[1].y - in.points[0].y),(in.points[1].x - in.points[0].x))*(-1.0);
  for(size_t i = 0;i < 4;i++)
  {
    tem_point32 = in.points[i];
    rotate_polygon.points[i].x = (tem_point32.x-rotate_point.x)*cos(theta) - (tem_point32.y-rotate_point.y)*sin(theta) + rotate_point.x;
    rotate_polygon.points[i].y = (tem_point32.x-rotate_point.x)*sin(theta) + (tem_point32.y-rotate_point.y)*cos(theta) + rotate_point.y;
  }

  double theta_p02 = atan2((rotate_polygon.points[2].y - rotate_polygon.points[0].y),(rotate_polygon.points[2].x - rotate_polygon.points[0].x));

  if(theta_p02>0)
  {
    rotate_shrink_polygon.points[0].x = rotate_polygon.points[0].x + shrink_meter;
    rotate_shrink_polygon.points[0].y = rotate_polygon.points[0].y + shrink_meter;

    rotate_shrink_polygon.points[1].x = rotate_polygon.points[1].x - shrink_meter;
    rotate_shrink_polygon.points[1].y = rotate_polygon.points[1].y + shrink_meter;

    rotate_shrink_polygon.points[2].x = rotate_polygon.points[2].x - shrink_meter;
    rotate_shrink_polygon.points[2].y = rotate_polygon.points[2].y - shrink_meter;

    rotate_shrink_polygon.points[3].x = rotate_polygon.points[3].x + shrink_meter;
    rotate_shrink_polygon.points[3].y = rotate_polygon.points[3].y - shrink_meter;
  }
  else
  {
    rotate_shrink_polygon.points[0].x = rotate_polygon.points[0].x + shrink_meter;
    rotate_shrink_polygon.points[0].y = rotate_polygon.points[0].y - shrink_meter;

    rotate_shrink_polygon.points[1].x = rotate_polygon.points[1].x - shrink_meter;
    rotate_shrink_polygon.points[1].y = rotate_polygon.points[1].y - shrink_meter;

    rotate_shrink_polygon.points[2].x = rotate_polygon.points[2].x - shrink_meter;
    rotate_shrink_polygon.points[2].y = rotate_polygon.points[2].y + shrink_meter;

    rotate_shrink_polygon.points[3].x = rotate_polygon.points[3].x + shrink_meter;
    rotate_shrink_polygon.points[3].y = rotate_polygon.points[3].y + shrink_meter;
  }

  for (size_t i = 0; i < 4; i++)
  {
    tem_point32 = rotate_shrink_polygon.points[i];
    out.points[i].x = (tem_point32.x-rotate_point.x)*cos(-theta) - (tem_point32.y-rotate_point.y)*sin(-theta) + rotate_point.x;
    out.points[i].y = (tem_point32.x-rotate_point.x)*sin(-theta) + (tem_point32.y-rotate_point.y)*cos(-theta) + rotate_point.y;
  }
}

out.points包含矩形的四个顶点坐标，shrink_meter为缩小多少米。