求空间中2条线段的最短距离(用osg+C++写的)

2条线段的最短距离

float DistanceLineToLine( const osg::Vec3d& p1,const osg::Vec3d& p2,const osg::Vec3d& p3,const osg::Vec3d& p4 )
	{
		float distance;
		float x1 = p1.x(); //A点坐标(x1,y1,z1)
		float y1 = p1.y();
		float z1 = p1.z();
		float x2 = p2.x(); //B点坐标(x2,y2,z2)
		float y2 = p2.y();
		float z2 = p2.z();
		float x3 = p3.x(); //C点坐标(x3,y3,z3)
		float y3 = p3.y();
		float z3 = p3.z();
		float x4 = p4.x(); //D点坐标(x4,y4,z4)
		float y4 = p4.y();
		float z4 = p4.z();

		float a = (x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)+(z2-z1)*(z2-z1);
		float b = -((x2-x1)*(x4-x3)+(y2-y1)*(y4-y3)+(z2-z1)*(z4-z3));
		float c = -((x1-x2)*(x1-x3)+(y1-y2)*(y1-y3)+(z1-z2)*(z1-z3));

		float d = -((x2-x1)*(x4-x3)+(y2-y1)*(y4-y3)+(z2-z1)*(z4-z3));
		float e = (x4-x3)*(x4-x3)+(y4-y3)*(y4-y3)+(z4-z3)*(z4-z3);
		float f = -((x1-x3)*(x4-x3)+(y1-y3)*(y4-y3)+(z1-z3)*(z4-z3));

		if ((a*e-b*d)==0&&(b*d-a*e)==0) //平行
		{
			float d1 = (p1-p3).length();
			float d2 = (p1-p4).length();
			distance = (d1<d2)?d1:d2;
			return distance;
		}

		float s = (b*f-e*c)/(a*e-b*d);
		float t = (a*f-d*c)/(b*d-a*e);

		if(0<=s&&s<=1&&0<=t&&t<=1) //说明P点落在线段AB上,Q点落在线段CD上
		{
			//2条线段的公垂线段PQ;
			//P点坐标
			float X = x1+s*(x2-x1);
			float Y = y1+s*(y2-y1);
			float Z = z1+s*(z2-z1);
			//Q点坐标
			float U = x3+t*(x4-x3);
			float V = y3+t*(y4-y3);
			float W = z3+t*(z4-z3);
			osg::Vec3d P(X,Y,Z);
			osg::Vec3d Q(U,V,W);
			distance = (P-Q).length();
		}
		else
		{
			float d1 = DistancePointToLine(p3,p4,p1);
			float d2 = DistancePointToLine(p3,p4,p2);
			float d3 = DistancePointToLine(p1,p2,p3);
			float d4 = DistancePointToLine(p1,p2,p4);
			distance = (d1<d2)?d1:d2;
			distance = (distance<d3)?distance:d3;
			distance = (distance<d4)?distance:d4;
		}
		return distance;
	}

空间中一个点到空间中一条线段的最短距离

float DistancePointToLine( const osg::Vec3d& star, const osg::Vec3d& end,const osg::Vec3d& center )
{
float distance;
float x0 = center.x();//P点坐标(x0,y0,z0)
float y0 = center.y();
float z0 = center.z();
float x1 = star.x(); //A点坐标(x1,y1,z1)
float y1 = star.y();
float z1 = star.z();
float x2 = end.x();//B点坐标(x2,y2,z2)
float y2 = end.y();
float z2 = end.z();
float t = ((x1-x0)*(x1-x2)+(y1-y0)*(y1-y2)+(z1-z0)*(z1-z2))
      /((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2)+(z1-z2)*(z1-z2));
if (0<=t&&t<=1)//垂足Q点落在线段AB上
{
float X = x1+t*(x2-x1);
float Y = y1+t*(y2-y1);
float Z = z1+t*(z2-z1);
osg::Vec3d Q(X,Y,Z);
distance = (Q-center).length();
}
if (t<0) //垂足Q点不落在线段AB上,而是落在BA的延长线上
{
distance = (star-center).length();
}
if (t>1) //垂足Q点不落在线段AB上,而是落在AB的延长线上
{
distance = (end-center).length();
}
return distance;
}

数学公式,我直接上传了!地址:http://download.csdn.net/detail/liying426/5058179

  • 0
    点赞
  • 0
    收藏
  • 打赏
    打赏
  • 5
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
©️2022 CSDN 皮肤主题:大白 设计师:CSDN官方博客 返回首页
评论 5

打赏作者

liying426

你的鼓励将是我创作的最大动力

¥2 ¥4 ¥6 ¥10 ¥20
输入1-500的整数
余额支付 (余额:-- )
扫码支付
扫码支付:¥2
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值