縮小矩形後的座標點

縮小一個矩形後的座標點

在一個座標系中給出一個矩形的四個頂點座標，求將這個矩形縮小一定範圍後的座標。
矩形要求：四個定點按順時針或者逆時針排列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爲縮小多少米。