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