计算空间中点到直线的距离

最近在做一个项目,已知三维空间中的一个点和构成直线的两个点的三维坐标,需要计算空间中点到直线的距离。
网上查了查,pcl有自带的计算函数,pcl::sqrPointToLineDistance如下,需要提供…方向?
计算空间中点到直线的距离_第1张图片
好吧,反正我是不会搞。
后来又在网上查了查资料,发现可以用海伦公式,通过已知三角形三个边的长度计算三角形的面积!问题就非常简单了。
计算空间中点到直线的距离_第2张图片
这样就可以先计算三个点之间的距离,作为三角形三个边的长度。然后利用三角形的面积公式:S = (h*a)/2计算空间中点到直线的距离啦。
感觉比网上求叉积的方式要容易些,但是具体会出现哪些问题,还没有深入探讨。
代码如下(我这里需要计算的是点到房间四条边的距离,仅做参考):

double Point_2_4line(Point cluster_pt,edge_pt component_info)
{
	vector<edge_pt> lines;
	//计算点到每条边界线的距离并筛选出最小距离
	double distance = 1000;
	for (size_t i = 0; i < lines.size(); ++i)
	{
		//利用海伦公式计算三角形面积,然后通过三角形面积算距离
		double delt_x_a = cluster_pt.x() - lines[i].pt[0].x();
		double delt_y_a = cluster_pt.y() - lines[i].pt[0].y();
		double delt_z_a = cluster_pt.z() - lines[i].pt[0].z();
		double delt_x_b = cluster_pt.x() - lines[i].pt[1].x();
		double delt_y_b = cluster_pt.y() - lines[i].pt[1].y();
		double delt_z_b = cluster_pt.z() - lines[i].pt[1].z();
		double delt_x_c = lines[i].pt[1].x() - lines[i].pt[0].x();
		double delt_y_c = lines[i].pt[1].y() - lines[i].pt[0].y();
		double delt_z_c = lines[i].pt[1].z() - lines[i].pt[0].z();
		double A = sqrt(pow(delt_x_a,2) + pow(delt_y_a, 2) + pow(delt_z_a, 2));
		double B = sqrt(pow(delt_x_b, 2) + pow(delt_y_b, 2) + pow(delt_z_b, 2));
		double C = sqrt(pow(delt_x_c, 2) + pow(delt_y_c, 2) + pow(delt_z_c, 2));
		double p = (A + B + C) / 2;
		double area = sqrt(p*(p-A)*(p-B)*(p-C));
		double dis = 2 * area / C;
		if (dis < distance)
		{
			distance = dis;
		}
	}



	return distance;
}

你可能感兴趣的:(点云数据处理,c++)