求空间内两条直线的最近距离以及最近点的坐标(C++)

关键词：空间几何

用途：总有地方会用到吧

文章类型：C++函数展示

@Author：VShawn(singlex@foxmail.com)

@Date:2016-11-19

@Lab: CvLab202@CSU

 

对于空间内的两条直线A，B我假设他们一定不平行，也不相交。若已知A上两点A1、A2，B上两点B1、B2，那么可以很容易得到两条直线的方程。

然后调用函数即可解得结果：

下面提供了一个用于解决这个问题的类：

#include <math.h>

//用于求解两条空间直线的最近距离，以及他们最近的两点坐标
//author @vshawn
//url:http://www.cnblogs.com/singlex/p/6091659.html
//date:2016-11-22
class GetDistanceOf2linesIn3D
{
public:
	//输入直线A的两个点，以便获得A的方程
	void SetLineA(double A1x, double A1y, double A1z, double A2x, double A2y, double A2z)
	{
		a1_x = A1x;
		a1_y = A1y;
		a1_z = A1z;

		a2_x = A2x;
		a2_y = A2y;
		a2_z = A2z;
	}

	//输入直线B的两个点，以便获得B的方程
	void SetLineB(double B1x, double B1y, double B1z, double B2x, double B2y, double B2z)
	{
		b1_x = B1x;
		b1_y = B1y;
		b1_z = B1z;

		b2_x = B2x;
		b2_y = B2y;
		b2_z = B2z;
	}

	//用SetLineA、SetLineB输入A、B方程后
	//调用本函数解出结果
	void GetDistance()
	{
		//方法来自：http://blog.csdn.net/pi9nc/article/details/11820545

		double d1_x = a2_x - a1_x;
		double d1_y = a2_y - a1_y;
		double d1_z = a2_z - a1_z;

		double d2_x = b2_x - b1_x;
		double d2_y = b2_y - b1_y;
		double d2_z = b2_z - b1_z;

		double e_x = b1_x - a1_x;
		double e_y = b1_y - a1_y;
		double e_z = b1_z - a1_z;


		double cross_e_d2_x, cross_e_d2_y, cross_e_d2_z;
		cross(e_x, e_y, e_z, d2_x, d2_y, d2_z, cross_e_d2_x, cross_e_d2_y, cross_e_d2_z);
		double cross_e_d1_x, cross_e_d1_y, cross_e_d1_z;
		cross(e_x, e_y, e_z, d1_x, d1_y, d1_z, cross_e_d1_x, cross_e_d1_y, cross_e_d1_z);
		double cross_d1_d2_x, cross_d1_d2_y, cross_d1_d2_z;
		cross(d1_x, d1_y, d1_z, d2_x, d2_y, d2_z, cross_d1_d2_x, cross_d1_d2_y, cross_d1_d2_z);

		double t1, t2;
		t1 = dot(cross_e_d2_x, cross_e_d2_y, cross_e_d2_z, cross_d1_d2_x, cross_d1_d2_y, cross_d1_d2_z);
		t2 = dot(cross_e_d1_x, cross_e_d1_y, cross_e_d1_z, cross_d1_d2_x, cross_d1_d2_y, cross_d1_d2_z);
		double dd = norm(cross_d1_d2_x, cross_d1_d2_y, cross_d1_d2_z);
		t1 /= dd*dd;
		t2 /= dd*dd;

		//得到最近的位置
		PonA_x = (a1_x + (a2_x - a1_x)*t1);
		PonA_y = (a1_y + (a2_y - a1_y)*t1);
		PonA_z = (a1_z + (a2_z - a1_z)*t1);

		PonB_x = (b1_x + (b2_x - b1_x)*t2);
		PonB_y = (b1_y + (b2_y - b1_y)*t2);
		PonB_z = (b1_z + (b2_z - b1_z)*t2);

		distance = norm(PonB_x - PonA_x, PonB_y - PonA_y, PonB_z - PonA_z);
	}



	double PonA_x;//两直线最近点之A线上的点的x坐标
	double PonA_y;//两直线最近点之A线上的点的y坐标
	double PonA_z;//两直线最近点之A线上的点的z坐标
	double PonB_x;//两直线最近点之B线上的点的x坐标
	double PonB_y;//两直线最近点之B线上的点的y坐标
	double PonB_z;//两直线最近点之B线上的点的z坐标
	double distance;//两直线距离
private:
	//直线A的第一个点
	double a1_x ;
	double a1_y ;
	double a1_z ;
	//直线A的第二个点
	double a2_x ;
	double a2_y ;
	double a2_z ;

	//直线B的第一个点
	double b1_x;
	double b1_y ;
	double b1_z ;

	//直线B的第二个点
	double b2_x;
	double b2_y ;
	double b2_z ;


	//点乘
	double dot(double ax, double ay, double az, double bx, double by, double bz) { return ax*bx + ay*by + az*bz; }
	//向量叉乘得到法向量，最后三个参数为输出参数
	void cross(double ax, double ay, double az, double bx, double by, double bz, double& x, double& y, double& z)
	{ 
		x = ay*bz - az*by;
		y = az*bx - ax*bz;
		z = ax*by - ay*bx;
	}
	//向量取模
	double norm(double ax, double ay, double az) { return sqrt(dot(ax, ay, az, ax, ay, az)); }
};

使用方法： 

		GetDistanceOf2linesIn3D g;//初始化
		g.SetLineA(a1.x, a1.y, a1.z, a2.x, a2.y, a2.z);//输入直线A上的两个点坐标
		g.SetLineB(b1.x, b1.y, b1.z, b2.x, b2.y, b2.z);//输入直线B上的两个点坐标
		g.GetDistance();//计算距离
		double d = g.distance;//获得距离
		double x = g.PonA_x;//获得AB间最近的两点中A线上的点的x坐标
		double y = g.PonA_y;//获得AB间最近的两点中A线上的点的y坐标
		double z = g.PonA_z;//获得AB间最近的两点中A线上的点的z坐标

　

 

下载：

Onedrive:https://1drv.ms/u/s!AmjBsmo7u4eFmDpYL9FSBJurEQQ7

Git:https://github.com/vshawn/Shawn_pose_estimation_by_opencv/blob/master/include/GetDistanceOf2linesIn3D.h