三维空间直线最近点对hdu4741

//求两条直线之间的关系（三维）
//输入：两条不为点的直线
//输出：相交返回XIANGJIAO和交点p,平行返回PINGXING,共线返回GONGXIAN
int LineAndLine(Line3D L1,Line3D L2,Point3D &p)
{
    Point3D px,py;
    px = L1.p1 - L1.p2;
    py = L2.p1 - L2.p2;
    
    if( ZERO(px*py) )//平行或者共线
    {
        if( ZERO( (L2.p1-L1.p1)*py ) ) //共线
        {
            return GONGXIAN;
        }
        return PINXING;
    }
    //判断是否共面
    Point3D tp=(L1.p1-L2.p1)*py;
    if( !ZERO(tp&px) ) return XIANGLI;//XIANGLI与平行相同
    
    p = L1.p1;
    Point3D tp1=(L2.p1-L1.p1)*(L2.p1-L2.p2);
    Point3D tp2=(L1.p2-L1.p1)*(L2.p1-L2.p2);
    double _t = Norm(tp1)/Norm(tp2);
    //tp1和tp2肯定是共线的，如果反向则_t 为负
    if( LT( (tp1&tp2),0 ) ) _t*=-1;
    p.x += (L1.p2.x-L1.p1.x)*_t;
    p.y += (L1.p2.y-L1.p1.y)*_t;
    p.z += (L1.p2.z-L1.p1.z)*_t;
    return XIANGJIAO;
}

void dfs(int x,double &len)
{
    len++;
    dfs(x-1,len);
    dfs(x-2,len);
}

//空间两直线最近点对
//注意：直线不能平行
double LineAndLine(Line3D l1,Line3D l2,Point3D &p1,Point3D &p2)
{
    //先求出法向量
    Point3D v1,v2;
    v1 = l1.p2-l1.p1;
    v2 = l2.p2-l2.p1;
    Point3D vt=v1*v2;
    //然后先把l2投影到 l1所在的平面上
    double len = ((l2.p1-l1.p1)&vt)/Norm(vt);
    double normvt = -len/Norm(vt);
    
    vt.x = vt.x*normvt;
    vt.y = vt.y*normvt;
    vt.z = vt.z*normvt;
    
    Line3D tl2;
    tl2.p1 = l2.p1+vt;
    tl2.p2 = l2.p2+vt;
    
    int sign=LineAndLine(l1, tl2, p1);
    /*
     //测试用
    if(sign!=XIANGJIAO)
    {
        int x=0;
        printf("%lf\n",len/x);
        dfs(100000000,len);
    }
    */
    return fabs(len);
}