三维空间内点到直线的距离计算公式

其思路是巧妙使用直线上两个点进行计算。

方法一：

如下图所示，在直线L上取两点与，则得到向量，与构成向量，根据下式计算得到两向量夹角。那么到直线L的距离为

方法二：

使用向量叉乘得到

基于方法一的代码如下：

double Point2Line3DSin(LinePara3D line, pcl::PointXYZ point)
{

	//直线的法向量(p,q,r)
	double p = line.LineNormal.normalX;
	double q = line.LineNormal.normalY;
	double r = line.LineNormal.normalZ;
	
	//直线上的2个点
	double x1 = line.point.x;
	double y1 = line.point.y;
	double z1 = line.point.z;

	double x2 = 1; //令第2个点的x=1
	double y2 = y1 - x1*q / p + q / p;
	double z2 = z1 - x1*r / p + r / p;
	
	//两个向量:参考博客中的绘图
	//p1p2
	double normal01_x = x2 - x1;
	double normal01_y = y2 - y1;
	double normal01_z = z2 - z1;

	//p1p0 (p0为待求点到直线距离的那个点)
	double normal02_x = point.x - x1;
	double normal02_y = point.y - y1;
	double normal02_z = point.z - z1;

	//求取两个向量的夹角:弧度
	double fenzi = normal01_x*normal02_x + normal01_y*normal02_y + normal01_z*normal02_z;
	double lengthN1 = sqrt(normal01_x*normal01_x + normal01_y*normal01_y + normal01_z*normal01_z);
	double lengthN2 = sqrt(normal02_x*normal02_x + normal02_y*normal02_y + normal02_z*normal02_z);
	double hudu = acos(fenzi / (lengthN1*lengthN2));


	//再求取点到直线的距离
	double ds = abs(lengthN2*sin(hudu));

	return ds;

}

基于方法二的代码如下：

double Point2Line3DVecproduct(LinePara3D line, pcl::PointXYZ point)
{
	//直线的法向量(p,q,r)
	double p = line.LineNormal.normalX;
	double q = line.LineNormal.normalY;
	double r = line.LineNormal.normalZ;

	//直线上的2个点
	double x_q = line.point.x;
	double y_q = line.point.y;
	double z_q = line.point.z;

	double x_b = 1; //令第2个点的x=1
	double y_b = y_q - x_q*q / p + q / p;
	double z_b = z_q - x_q*r / p + r / p;

	double x_o = point.x;
	double y_o = point.y;
	double z_o = point.z;

	//两个向量
	//oq
	double normal01_x = x_q - x_o;
	double normal01_y = y_q - y_o;
	double normal01_z = z_q - z_o;

	//ob
	double normal02_x = x_b - x_o;
	double normal02_y = y_b - y_o;
	double normal02_z = z_b - z_o;
	
	//oq与ob进行叉乘
	double chacheng_x = normal01_y*normal02_z - normal02_y*normal01_z;
	double chacheng_y = normal02_z*normal01_x - normal02_x*normal01_z;
	double chacheng_z = normal01_x*normal02_y - normal01_y*normal02_x;

	double chacheng_length = sqrt(chacheng_x*chacheng_x + chacheng_y*chacheng_y + chacheng_z*chacheng_z);
	double dx = x_q - x_b;
	double dy = y_q - y_b;
	double dz = z_q - z_b;

	double qb_length = sqrt(dx*dx+dy*dy+dz*dz);

	double ds = chacheng_length / qb_length;
	return ds;

}

测试代码如下：

void main()
{
	MyBoundary BounExample;

	LinePara3D line;
	line.LineNormal.normalX = 1;
	line.LineNormal.normalY = -4;
	line.LineNormal.normalZ = 2;

	line.point.x = 2;
	line.point.y = -1;
	line.point.z = 6;

	pcl::PointXYZ p;
	p.x = 4;
	p.y = 5;
	p.z = 6;

	double ds = BounExample.Point2Line3DSin(line, p);
	cout << "使用方法一计算得到的距离ds=" << ds << endl;

	double ds1 = BounExample.Point2Line3DVecproduct(line, p);
	cout << "使用方法二（叉乘）计算得到的距离" << ds1 << endl;

	system("pause");
}

 

具体参考matlab中推导：https://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html

参考博客：https://blog.csdn.net/lcfactorization/article/details/53285631