ROS：：判断点是否在多边形区域内

ROS：：判断点是否在多边形区域内

原理为此链接介绍的射线法：链接

要求：多边形的边不能相交

#include <Eigen/Core>

const float EPS = 1e-6;
const float PI = 3.1415926535;

bool isInPolygon(float x,float y, geometry_msgs::Polygon& R)
{
    if(R.points.empty())
        return false;

    bool flag = false;
    Eigen::Vector2f Q(x,y);
    Eigen::Vector2f P1,P2; //多边形一条边的两个顶点

    size_t p_size = R.points.size();
    size_t loop_n = p_size - 1;
    for(size_t i=0;i<=loop_n;i++)
    {
        P1[0] = R.points[i].x;
        P1[1] = R.points[i].y;

        if(i==loop_n)
        {
            P2[0] = R.points[0].x;
            P2[1] = R.points[0].y;
        }
        else
        {
            P2[0] = R.points[i+1].x;
            P2[1] = R.points[i+1].y;
        }

        if(OnSegment(P1,P2,Q)) //点在多边形一条边上
        {
            return true;
        }

        if( (dcmp(P1[1]-Q[1])>0 != dcmp(P2[1]-Q[1])>0) &&
            (dcmp(Q[0] - (Q[1]-P1[1])*(P1[0]-P2[0])/(P1[1]-P2[1])-P1[0])<0) )
        {
            flag = !flag;;
        }
    }

    return flag;
}

int dcmp(float x)
{
    if(fabs(x)<EPS)
        return 0;
    else
        return x<0?-1:1;
}

bool OnSegment(const Eigen::Vector2f P1,
                       const Eigen::Vector2f P2,
                       Eigen::Vector2f Q)
{
    //向量1
    Eigen::Vector2f v1 = P1 - Q;
    //向量2
    Eigen::Vector2f v2 = P2 - Q;

    float v1Xv2 = v1[0]*v2[1] - v2[0]*v1[1];
    float v1_v2 = v1.transpose() *v2;

    return dcmp(v1Xv2)==0 && dcmp(v1_v2)<=0;
}