一、背景

近期在阅读move_base源码costmap部分(感觉想玩转movebase导航，costmap必须理解呀)。读到两处点与多边形的相对位置关系。在此总结一下，分别是:

• intersects: 利用待测点向右引出的射线与多边形的交点数来确定相对位置。有的称 射线交叉算法，也有称PNPoly算法；
• ptInPolygon: 利用点和多边形的边的方向关系来确定相对位置；

二、intersects

1. 原理思路

以待测点引出射线穿过多边形，若点在多边形外，则射线与多边形交点数目一定为偶数(不考虑射线仅过多边形角点)；若点在多边形内，则射线与多边形交点数目一定为奇数；

2、代码分析

前提条件: 从待测点引出 向右的水平射线，与多边形相交进行交点数目判断。
函数位置: navigation/costmap_2d/src/costmap_math.cpp

/**
 * @brief 确定一个点 (testx,testy)是否在一个多边形顶点集polygon 内部。该函数使用的是射线交叉算法（Ray-Casting Algorithm）或PNPoly算法，该算法通过计算从点到无穷远的射线与多边形边的交点数量来判断点的内部性
 * @date 2025/01/14
 * @return 返回该点是否与多边形相交
**/
bool intersects(std::vector<geometry_msgs::Point>& polygon, float testx, float testy)
{
   
    
    
  // 记录交点的奇偶性，初始值为 false 此时表示无交点(交叉判断后也可以理解为偶数个交点)， true为奇数个交点
  bool c = false;
  int i, j, nvert = polygon.size();  // nvert 是多边形顶点的数量

  // 循环遍历多边形的每一条边，j 被初始化为最后一个顶点的索引，以便在循环中连接最后一个顶点和第一个顶点
  for (i = 0, j = nvert - 1; i < nvert; j = i++)
  {
   
    
    
    // yi 和 yj 分别是当前顶点和前一个顶点的 y 坐标
    // xi 和 xj 分别是当前顶点和前一个顶点的 x 坐标
    float yi = polygon[i].y, yj = polygon[j].y, xi = polygon[i].x, xj = polygon[j].x;

    // 判断当前边是否与测试点的 向右 引出的水平射线相交
    if (((yi > testy) != (yj > testy)) && (testx < (xj - xi) * (testy - yi) / (yj - yi) + xi))
    // 若以上条件都满足，说明射线与多边形的当前边相交，因此通过 c = !c 切换 c 的值，追踪交点的数量
      c = !c;
  }
  return c;
}

代码重点：

    // 判断当前边是否与测试点的 向右 引出的水平射线相交
    if (((yi > testy) != (yj > testy)) && (testx < (xj - xi) * (testy - yi) / (yj - yi) + xi))
    // 若以上条件都满足，说明射线与多边形的当前边相交，因此通过 c = !c 切换 c 的值，追踪交点的数量
      c = !c;

首先，(yi > testy) != (yj > testy) 表示检查当前边的两个端点是否分别在测试点的上下方(即两个端点的 y坐标相对于 testy 的位置是否不同)。如果返回 true，说明一端在上方，一端在下方，可能会有交点；若为false，则在线段的上方或者下方，说明不存在交点；
接着，分析条件判断 testx < (xj - xi) * (testy - yi) / (yj - yi) + xi

如下图所示，待测点$(x_t,y_t)$ 红点为待测点，$(x_0,y_0)$ 到$(x_3,$