地理围栏算法及C++实现

 

地理围栏一般是多边形，如何判断点在多边形内部呢？可以通过射线法来判断点是否在多边形内部。如下图所示，从该点出发沿着X轴画一条射线，依次判断该射线与每条边的交点，并统计交点个数，如果交点数为奇数，则在多边形内部（如图3个交点），如果焦点数是偶数，则在外部，射线法对凸和非凸多边形都适用，复杂度为O(N)，其它N是边数。

 

   当地理围栏多边形数目较少时，我们可以依次遍历每一个多边形（暴力遍历法），然后用射线法进行判断，这样效率也很高。代码如下：

  template<typename T>
  bool pointInPolygon(const T &point, const geometry_msgs::Polygon &polygon){
      int cross = 0;
      for (int i = 0, j = polygon.points.size()-1; i < polygon.points.size(); j = i++) {
          if ( ((polygon.points[i].y > point.y) != (polygon.points[j].y>point.y)) &&
              (point.x < (polygon.points[j].x-polygon.points[i].x) * (point.y-polygon.points[i].y) / (polygon.points[j].y-polygon.points[i].y) + polygon.points[i].x) ){
              cross++;
          }
      }
      return bool(cross % 2);
  }