判断点在形状里，点在线段上

/************************************************************************
函数名 :		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;
}