其思路是巧妙使用直线上两个点进行计算。
方法一:
如下图所示,在直线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