线段相交（使用容差）

最新推荐文章于 2024-07-04 11:10:43 发布

zm147451753

最新推荐文章于 2024-07-04 11:10:43 发布

阅读量575

点赞数

分类专栏：计算几何文章标签： c++ 几何学线性代数

本文链接：https://blog.csdn.net/zm147451753/article/details/122195320

版权

计算几何专栏收录该内容

2 篇文章 0 订阅

订阅专栏

输入点 $p_1$ , $p_2$ , $p_3$ , $p_4$ ，最小容差dSmallestDisTol，距离容差dDisTol
输出vector<Point>

构造线段 $l_1 =(p_1,p_2)$ , $l_2 =(p_3,p_4)$ , $l_1$ 所在直线 $L 1$ , $l_2$ 所在直线 $L 2$
计算 $l_1, l_2$ 的距离dis (dis = Distance( $l_1$ , $l_2$ ))
如果dis>dDisTOl 没有交点，返回
判断线段 $l_1，l_2$ 是否平行

计算线段 $l_1$ 上的两点到直线 $L_2$ 的距离差d0 = fabs(Distance( $p_0$ , $L_2$ ) - Distance( $p_1$ , $L_2$ ) )
计算线段 $l_2$ 上的两点到直线 $L_1$ 的距离差d1 = fabs(Distance( $p_2$ , $L_1$ ) - Distance( $p_3$ , $L_1$ ) )
d0<dDisTol且d1<dDisTol则平行

平行跳到11；不平行，使用CGAL计算 $l_1,l_2$ 的交点，
如果存在交点 $p_0$ ,返回
不存在，计算 $l_1，l_2$ 的最近点对 $pntNst_0,pntNst_1$

pair<Point_3, Point_3> GetNearstPnt(Segment_3 segA, Segment_3 segB)
{
  Vector_3 u = segA.to_vector();
  Vector_3 v = segB.to_vector();
  Vector_3 w = segA.source() - segB.source();
  double a = CGAL::to_double(u * u), b = CGAL::to_double(u * v), c = CGAL::to_double(v * v), d = CGAL::to_double(u * w), e = CGAL::to_double(v * w);
  double dt = a * c - b * b;
  double sd = dt, td = dt, sn = 0.0, tn = 0.0;
  Point_3 pntClose[2];
  if (fabs(dt) < 1e-6)
  {
  	//两直线平行
  	sn = 0.0;    //在s上指定取s0
  	sd = 1.00;   //防止计算时除0错误

  	tn = e;      //按(公式3)求tc
  	td = c;
  }
  else
  {
  	sn = (b * e - c * d);
  	tn = (a * e - b * d);
  	if (sn < 0.0)
  	{
  		//最近点在s起点以外，同平行条件
  		sn = 0.0;
  		tn = e;
  		td = c;
  	}
  	else if (sn > sd)
  	{
  		//最近点在s终点以外(即sc>1,则取sc=1)
  		sn = sd;
  		tn = e + b; //按(公式3)计算
  		td = c;
  	}
  }
  if (tn < 0.0)
  {
  	//最近点在t起点以外
  	tn = 0.0;
  	if (-d < 0.0) //按(公式2)计算，如果等号右边小于0，则sc也小于零，取sc=0
  		sn = 0.0;
  	else if (-d > a) //按(公式2)计算，如果sc大于1，取sc=1
  		sn = sd;
  	else
  	{
  		sn = -d;
  		sd = a;
  	}
  }
  else if (tn > td)
  {
  	tn = td;
  	if ((-d + b) < 0.0)
  		sn = 0.0;
  	else if ((-d + b) > a)
  		sn = sd;
  	else
  	{
  		sn = (-d + b);
  		sd = a;
  	}
  }

  double sc = 0.0;
  double tc = 0.0;

  if (fabs(sn) < 1e-6)
  	sc = 0.0;
  else
  	sc = sn / sd;

  if (fabs(tn) < 1e-6)
  	tc = 0.0;
  else
  	tc = tn / td;

  pntClose[0] = segA.source() + sc * u;
  pntClose[1] = segB.source() + tc * v;
  return make_pair(pntClose[0], pntClose[1]);
}

判断点 $pntNst_0$ 是否在 $l_1$ ， $pntNst_1$ 是否在 $l_2$ ,判断函数如下

double GetPntScaleInSeg(const Segment_3& seg, Point_3 pnt)
{
	Vector_3 dir = seg.target() - seg.source();
	int inxMax = fabs(dir[0]) > fabs(dir[1]) ? 0 : 1; inxMax = fabs(dir[inxMax]) > fabs(dir[2]) ? inxMax : 2;
	return CGAL::to_double((pnt - seg.source())[inxMax]) / CGAL::to_double(dir[inxMax]);
}

bool IsPntInSeg(const Segment_3& seg, Point_3 pnt, double dTol)
{
	bool ret = true;
	if (VeLength(pnt - seg.source()) < dTol) return true;
	if (VeLength(pnt - seg.target()) < dTol) return true;
	double dDot = VeNormalTo(pnt - seg.source()) * VeNormalTo(pnt - seg.target());
	if (fabs(fabs(dDot) - 1) < dTol)
	{
		double dScale = GetPntScaleInSeg(seg, pnt);
		//判断交点是不是比较远，如果远，计算最近点对
		if (!(dScale <= 1 + dTol && dScale > -dTol))
		{
			ret = false;
		}
	}
	return ret;
}

如果满足条件8，则交点为 $pntNst_0+pntNst_1)/2$
否则判断为平行，跳到11
由于平行线之间距离<dDisTol所以肯定存在交点，对于 $l_1$ 上的2个点 $p_1,p_2$
在 $l_2$ 的投影点 $p_{12},p_{22}是否在线段l_2上$ ,如果在加入点 $p_i$ ,同理 $l_2$
$\vec{d_1} = \overrightarrow{l_1l_2}/|l_1l_2|$ ,
$\vec{d_2} = \overrightarrow{l_1l_2}/|l_1l_2|$
$ds1=(l_1.source() - l_2.source())*\vec{d_2}$
$ds2=(l_2.source() - l_1.source())*\vec{d_1}$
$de1=(l_1.end() - l_2.end())*\vec{d_2}$
$de2=(l_2.end() - l_1.end())*\vec{d_1}$
if $ds1>0 且ds1<|l_2|+ dSmallestDisTol$ 添加 $p_1$
if $ds2>0 且ds2<|l_1|+ dSmallestDisTol$ 添加 $p_3$
if $de1>0 且de1<|l_2|+ dSmallestDisTol$ 添加 $p_2$
if $de2>0 且de2<|l_1|+ dSmallestDisTol$ 添加 $p_4$