三维空间两条直线的最短距离、最近点及C++算法实现

未经许可请勿转载


在双目视觉立体空间重建中，会根据两个相机中的物体图像坐标，求取给定三维坐标系的三维坐标，而可以根据物体
图像坐标、相机内参、给定坐标系的相机外参，求取相机光轴线的方程，从而实现立体重建，内外参、直线方程请执行
搜索学习，本文主要是解决在已知空间两直线求最短距离及最近点。如有疑问或错误，请与评论区指出，谢谢。
一、理论：
已知空间中两线段，如果它们无限变粗，判断是否相交。（主要讨论不在同一平面的情况）
线段AB，线段CD
问题的关键是求出这两条任意直线之间的最短距离，以及在这个距离上的两线最接近点坐标，判断该点是否在线段AB
和线段CD上。
首先将直线方程化为对称式，得到其方向向量n1=（a1,b1,c1),n2=(a2,b2,c2).
再将两向量叉乘得到其公垂向量N=（x,y,z）,在两直线上分别选取点A,B(任意)，得到向量AB，
求向量AB在向量N方向的投影即为两异面直线间的距离了（就是最短距离了）。
最短距离的求法：d=|向量N*向量AB|/|向量N|（上面是两向量的数量积，下面是取模）。
设交点为C,D，带入公垂线N的对称式中，又因为C,D两点分别满足一开始的直线方程，所以得到关于C（或D）的两个
连等方程，分别解出来就好了！
二、方程式
直线1：(平面式)
面e:A_1e*x+B_1e*y+C_1e*z+D_1e=0;
面f:A_1f*x+B_1f*y+C_1f*z+D_1f=0;
 
直线2：（平面式）
面e:A_2e*x+B_2e*y+C_2e*z+D_2e=0;
面f:A_2f*x+B_2f*y+C_2f*z+D_2f=0;
 
直线1的方向向量：（direction vector）
DV_1=(A_1e,B_1e,C_1e)X(A_1f,B_1f,C_1f)
=(B_1e*C_1f-B_1f*C_1e, C_1f*A_1e-A_1e*C_1f,A_1e*B_1f-A_1f*B_1e);
DV_2=(B_2e*C_2f-B_2f*C_2e, C_2f*A_2e-A_2e*C_2f,A_2e*B_2f-A_2f*B_2e);
 
在直线1、2上任取一点R1，R2






领Z_R1=1,Z_R2=1;
直线1带入Z_R 1有：
面e:A_1e*x+B_1e*y+C_1e*Z_R1+D_1e=0;
面f:A_1f*x+B_1f*y+C_1f*Z_R1+D_1f=0;

即：