ROS：：向量法外扩多边形

ROS：：向量法外扩多边形

将多边形区域的灭个边向外移动一定距离然后组成的新多边形

如上图所示针对每一个顶点分别计算外扩后的新顶点，

c为当前点，p为前一个点，n为下一个点

分两种情况：1、pn中点在多边形内部，2、pn中点在多边形外部

判断一个点是否在多边形内部请参考：点是否在多边形内

情况1：pn的中点在多边形外部

计算如图所示：

多边形区域：R，计算当前点c的外扩点，上一个点为p，下一个点为n，o为pn的中点。

L = 外扩距离。向量v1,v2.
v1,v2的夹角为angle
n_v1 = v1.normalized(); //单位向量
n_v2 = v2.normalized(); //单位向量
|v1|=|v2|=L/sin(amgle)=s
v3 = c_new-c = s*(n_v1+n_v2)
c_new = s*(n_v1+n_v2)+c

情况2：pn的中点在多边形内部

计算如图所示：

对比情况1可得出：
v1 *= -1;
v2 *= -1;
既可得出情况2中的v1,v2,其他计算不变。

参考代码

#include <Eigen/Core>

void expandPolygon(geometry_msgs::Polygon &in, geometry_msgs::Polygon &out)
{
    size_t ps = in.points.size();
    if(ps < 3)
    {
        out = in;
        return;
    }

    out.points.resize(ps);

    float L = robot_radius_;

    for(size_t i=0;i<ps;i++)
    {
        Eigen::Vector2f c,p,n;
        c[0] = in.points[i].x;
        c[1] = in.points[i].y;

        if(i==0)
        {
            p[0] = in.points.back().x;
            p[1] = in.points.back().y;

            n[0] = in.points[i+1].x;
            n[1] = in.points[i+1].y;
        }
        else if(i==ps-1)
        {
            p[0] = in.points[i-1].x;
            p[1] = in.points[i-1].y;

            n[0] = in.points[0].x;
            n[1] = in.points[0].y;
        }
        else
        {
            p[0] = in.points[i-1].x;
            p[1] = in.points[i-1].y;

            n[0] = in.points[i+1].x;
            n[1] = in.points[i+1].y;
        }
        Eigen::Vector2f v1 = c-p;
        Eigen::Vector2f v2 = c-n;
        Eigen::Vector2f v12_centre = (p+n)/2.0;
        if(isInPolygon(v12_centre[0],v12_centre[1],in)) //pn中点在多边形内部
        {
            Eigen::Vector2f n_v1 = v1.normalized(); //单位向量
            Eigen::Vector2f n_v2 = v2.normalized(); //单位向量
            float angle = std::acos( (n_v1[0]*n_v2[0]+n_v1[1]*n_v2[1]) );
            float s = L/std::sin(angle);
            Eigen::Vector2f c_new = s*(n_v1+n_v2)+c;
            out.points[i].x = c_new[0];
            out.points[i].y = c_new[1];
        }
        else pn中点在多边形外部
        {
            v1 *= -1;
            v2 *= -1;
            Eigen::Vector2f n_v1 = v1.normalized(); //单位向量
            Eigen::Vector2f n_v2 = v2.normalized(); //单位向量
            float angle = std::acos( (n_v1[0]*n_v2[0]+n_v1[1]*n_v2[1]) );
            float s = L/std::sin(angle);
            Eigen::Vector2f c_new = s*(n_v1+n_v2)+c;
            out.points[i].x = c_new[0];
            out.points[i].y = c_new[1];
        }
    }
}