/************************************************************************ 函数名 : OnSegment 功能描述: 判断点q是否在p1和p2的线段上(调试用) 输入参数: p1 线段端点1; p2 线段端点2; q 要判断的点; 输出参数: 无 返回值 : ture 点在线段上; false 点不在线段上 ************************************************************************/ bool ObstacleDetector::OnSegment(pcl::PointXYZINormal p1, pcl::PointXYZINormal p2, pcl::PointXYZINormal q) { // 在X_Z坐标系下,求向量q->p1,用点sta1表示 pcl::PointXYZINormal sta1; sta1.x = p1.x - q.x; sta1.z = p1.z - q.z; // 求向量q->p2,用点sta2表示 pcl::PointXYZINormal sta2; sta2.x = p2.x - q.x; sta2.z = p2.z - q.z; // 求向量sta1与sta2的叉积。意义:>0 sta1在sta2的顺时针方向; <0 sta1在sta2的逆时针方向; =0 sta1和sta2共线(同向或反向) float crossPro = sta1.x * sta2.z - sta2.x * sta1.z; // 求向量sta1与sta2的点积。意义:如果叉积为0,那么点积≤0,代表q点在在线段p1,p2上。 float dotPro = sta1.x * sta1.x + sta1.z * sta2.z; return (0 == Fcmp(crossPro)) && (0 >= Fcmp(dotPro)); }
/************************************************************************ 函数名 : Fcmp 功能描述: 三态函数,判断 x 在eps精度下的大小关系 输入参数: x 要判断的float值 输出参数: 无 返回值 : 0 x和0相等; -1 x小于0; 1 x大于0 ************************************************************************/ int ObstacleDetector::Fcmp(float x) { if(fabs(x) < 1e-6) { return 0; } else { return x < 0 ? -1 : 1; } }
/************************************************************************ 函数名 : InOctagon 功能描述: 判断点是否在多边形内(xz二维平面判断,使用射线法) 输入参数: pointTmp 要判断的点;sectionV 多边形的顶点;inYMin 切片的y值; inXMin 切片顶点的x轴最小值 inXMax 切片顶点的x轴最大值 inZMin 切片顶点的z轴最小值 inZMax 切片顶点的z轴最大值 输出参数: 无 返回值 : ture 点在多边形内; false 点不在多边形内 ************************************************************************/ bool ObstacleDetector::InOctagon(pcl::PointXYZINormal& pointTmp, std::vector<pcl::PointXYZINormal>& sectionV, float inYMin, float inXMin, float inXMax, float inZMin, float inZMax) { bool inOrNot = false; // 是否在切片之内 // 判断点是否在切片之后,考虑切片厚度 inYMin += CLOUD_SLICE_STEP; // 用原y轴信息比较筛选掉y轴不符合的点,再筛选掉x轴、z轴极值外的点 if(inYMin < pointTmp.normal_y && inXMin < pointTmp.x && inXMax > pointTmp.x && inZMin < pointTmp.z && inZMax > pointTmp.z) { // 投影后的八边形已变不规则八边形,适用射线法判断是否在图形内 pcl::PointXYZINormal p1, p2; //多边形一条边的两个顶点 int maxIndexShape = sectionV.size() - 1; for(int i=0, j = maxIndexShape; i <= maxIndexShape; j = i++) { p1 = sectionV[i]; p2 = sectionV[j]; #if 0 if(OnSegment(p1, p2, pointTmp)) { inOrNot = true; //点在多边形一条边上 break; } else { //前一个判断pointTmp.z在 p1和p2之间 //后一个判断被测点 在 射线与边交点 的左边 if( (0 < Fcmp(p1.z - pointTmp.z) != 0 < Fcmp(p2.z - pointTmp.z)) && 0 > Fcmp(pointTmp.x - (pointTmp.z - p1.z) * (p1.x - p2.x) / (p1.z - p2.z) - p1.x)) { inOrNot = !inOrNot; // 该点在线段左侧次数为奇数,则在多边性内部;如果为偶数,则在外部。(通过该点水平向右作射线,通过射线和多边形边界的交点数奇偶判断是否在多边形内部) } else { // 射线不与多边形相交。nothing } } #endif if((0 < Fcmp(p1.z - pointTmp.z)) != (0 < Fcmp(p2.z - pointTmp.z))) { // 如果pointTmp.z在 p1和p2之间 ,继续判断 // 计算pointTmp.x在线段p1->p2哪边:edgeV > 0, 在线段右边; =0 在线段上; <0 在线段左侧。 int edgeV = Fcmp(pointTmp.x - (pointTmp.z - p1.z) * (p1.x - p2.x) / (p1.z - p2.z) - p1.x); if(0 < edgeV) { // 通过该点水平向右作射线,肯定不和线段p1->p2相交。nothing } else if(0 > edgeV) { inOrNot = !inOrNot; // 该点在线段左侧次数为奇数,则在多边性内部;如果为偶数,则在外部。(通过该点水平向右作射线,通过射线和多边形边界的交点数奇偶判断是否在多边形内部) } else { // =0 , 在线段上 inOrNot = true; break; } } else { // 如果pointTmp.z不在 p1和p2之间。通过该点水平向右作射线,肯定不和线段p1->p2相交。nothing } } } else { // 点在切片前面,不考虑该点。nothing } return inOrNot; }
标签:p2,判断,多边形,pointTmp,线段,形状,p1,inOrNot From: https://www.cnblogs.com/gaara-zhang/p/18211553