今天主要来讲一下这一部分的代码。就是判断点是否在一个区域内部的。也是Far_Planner里面用到的一个函数:
template <typename Point>
static bool PointInsideAPoly(const std::vector<Point>& poly, const Point& p) {
// By Randolph Franklin, https://www.eecs.umich.edu/courses/eecs380/HANDOUTS/PROJ2/InsidePoly.html
int i, j, c = 0;
int npol = poly.size();
if (npol < 3) {
if (FARUtil::IsDebug) ROS_WARN("FARUtil: The vertices number of a polygon is less than 3.");
return false;
}
// (y - y0) (x1 - x0) - (x - x0) (y1 - y0)>0
for (i = 0, j = npol-1; i < npol; j = i++) {
const Point vetex1 = poly[i];
const Point vetex2 = poly[j];
if ((((vetex1.y <= p.y) && (p.y < vetex2.y)) ||
((vetex2.y <= p.y) && (p.y < vetex1.y))) &&
(p.x <