求解空间中两线段之间的最小距离，并求出最小距离对应的两个点

bool IsEqual(double d1, double d2)
{
    if (fabs(d1 - d2) < 1e-7)
        return true;
    return false;
}
//判断两线段之间的最小距离，并求出相距最小距离的连个点  
//输入线段1的两个端点，输入线段2的两个端点，输出最小距离，最小距离在线段1上的点，最小距离在线段2上的点，
int SqureDistanceSegmentToSegment(double douLine1Point1[3], double douLine1Point2[3], double douLine2Point1[3], double douLine2Point2[3],double *douMinDis, double douMinPoint1[3], double douMinPoint2[3])
{
    // 解析几何通用解法，可以求出点的位置，判断点是否在线段上
    // 算法描述：设两条无限长度直线s、t,起点为s0、t0，方向向量为u、v
    // 最短直线两点：在s1上为s0+sc*u，在t上的为t0+tc*v
    // 记向量w为(s0+sc*u)-(t0+tc*v),记向量w0=s0-t0
    // 记a=u*u，b=u*v，c=v*v，d=u*w0，e=v*w0——(a)；
    // w与两线段分别垂直时最短，所存在u*w=0,v*w=0；　
    // 由于u*w=、v*w=0，将w=-tc*v+w0+sc*u带入前两式得：
    // (u*u)*sc - (u*v)*tc = -u*w0  (公式2)
    // (v*u)*sc - (v*v)*tc = -v*w0  (公式3)
    // 再将前式(a)带入可得sc=(be-cd)/(ac-b2)、tc=(ae-bd)/(ac-b2)——（b）
    // 注意到ac-b2=|u|2|v|2-(|u||v|cosq)2=(|u||v|sinq)2不小于0
    // 所以可以根据公式（b）判断sc、tc符号和sc、tc与1的关系即可分辨最近点是否在线段内
    // 当ac-b2=0时，(公式2)(公式3)独立，表示两条直线平行。可令sc=0单独解出tc
    // 最终距离d（L1、L2）=|（P0-Q0)+[(be-cd)*u-(ae-bd)v]/(ac-b2)|
    
    double x1, y1, z1, x2, y2, z2, x3, y3, z3, x4, y4, z4;
    x1 = douLine1Point1[0];
    y1 = douLine1Point1[1];
    z1 = douLine1Point1[2];

    x2 = douLine1Point2[0];
    y2 = douLine1Point2[1];
    z2 = douLine1Point2[2];

    x3 = douLine2Point1[0];
    y3 = douLine2Point1[1];
    z3 = douLine2Point1[2];

    x4 = douLine2Point2[0];
    y4 = douLine2Point2[1];
    z4 = douLine2Point2[2];

    double ux = x2 - x1;
    double uy = y2 - y1;
    double uz = z2 - z1;

    double vx = x4 - x3;
    double vy = y4 - y3;
    double vz = z4 - z3;

    double wx = x1 - x3;
    double wy = y1 - y3;
    double wz = z1 - z3;

    double a = (ux * ux + uy * uy + uz * uz); //u*u
    double b = (ux * vx + uy * vy + uz * vz); //u*v
    double c = (vx * vx + vy * vy + vz * vz); //v*v
    double d = (ux * wx + uy * wy + uz * wz); //u*w 
    double e = (vx * wx + vy * wy + vz * wz); //v*w
    double dt = a * c - b * b;

    double sd = dt;
    double td = dt;

    double sn = 0.0;//sn = be-cd
    double tn = 0.0;//tn = ae-bd

    if (IsEqual(dt, 0.0))
    {
        //两直线平行
        sn = 0.0;    //在s上指定取s0
        sd = 1.00;   //防止计算时除0错误

        tn = e;      //按(公式3)求tc
        td = c;
    }
    else
    {
        sn = (b * e - c * d);
        tn = (a * e - b * d);
        if (sn < 0.0)
        {
            //最近点在s起点以外，同平行条件
            sn = 0.0;
            tn = e;
            td = c;
        }
        else if (sn > sd)
        {
            //最近点在s终点以外(即sc>1,则取sc=1)
            sn = sd;
            tn = e + b; //按(公式3)计算
            td = c;
        }
    }
    if (tn < 0.0)
    {
        //最近点在t起点以外
        tn = 0.0;
        if (-d < 0.0) //按(公式2)计算，如果等号右边小于0，则sc也小于零，取sc=0
            sn = 0.0;
        else if (-d > a) //按(公式2)计算，如果sc大于1，取sc=1
            sn = sd;
        else
        {
            sn = -d;
            sd = a;
        }
    }
    else if (tn > td)
    {
        tn = td;
        if ((-d + b) < 0.0)
            sn = 0.0;
        else if ((-d + b) > a)
            sn = sd;
        else
        {
            sn = (-d + b);
            sd = a;
        }
    }

    double sc = 0.0;
    double tc = 0.0;

    if (IsEqual(sn, 0.0))
        sc = 0.0;
    else
        sc = sn / sd;

    if (IsEqual(tn, 0.0))
        tc = 0.0;
    else
        tc = tn / td;

    double dx = wx + (sc * ux) - (tc * vx);
    double dy = wy + (sc * uy) - (tc * vy);
    double dz = wz + (sc * uz) - (tc * vz);
    *douMinDis = sqrt(dx * dx + dy * dy + dz * dz);
    
    douMinPoint1[0] = x1 + (sc * ux);
    douMinPoint1[1] = y1 + (sc * uy);
    douMinPoint1[2] = z1 + (sc * uz);
    douMinPoint2[0] = x3 + (tc * vx);
    douMinPoint2[1] = y3 + (tc * vy);
    douMinPoint2[2] = z3 + (tc * vz);

    return 0;
}

 原文链接 ：https://blog.csdn.net/qq_45097594/article/details/103969664