三维空间内两条直线的交点

https://blog.csdn.net/hunter_wwq/article/details/41044179

代码实现：

	float Xp,Yp,Zp;//	pos_P
    float Xq,Yq,Zq;//	pos_Q
    float Xr,Yr,Zr;//	pos_R

    Xp  =starting_P[0];
    Yp  =starting_P[1];
    Zp  =starting_P[2];
    Xq  =  center_Q[0];
    Yq  =  center_Q[1];
    Zq  =  center_Q[2];
    Xr  =  ending_R[0];
    Yr  =  ending_R[1];
    Zr  =  ending_R[2];
    //
    float n[3]={(Yq-Yp)*(Zr-Zq)-(Zq-Zp)*(Yr-Yq),
                (Xr-Xq)*(Zq-Zp)-(Xq-Xp)*(Zr-Zq),
                (Xq-Xp)*(Yr-Yq)-(Yq-Yp)*(Xr-Xq)};  // PQR面法向量


    float n1[3]={n[1]*(Zq-Zp)-n[2]*(Yq-Yp),
                 n[2]*(Xq-Xp)-n[0]*(Zq-Zp),
                 n[0]*(Yq-Yp)-n[1]*(Xq-Xp)};  //PQR面内 PQ直线的法向量

    float Xn1=(Xp+Xq)*0.5f;
    float Yn1=(Yp+Yq)*0.5f;
    float Zn1=(Zp+Zq)*0.5f;

    float n2[3]={n[1]*(Zr-Zq)-n[2]*(Yr-Yq),
                 n[2]*(Xr-Xq)-n[0]*(Zr-Zq),
                 n[0]*(Yr-Yq)-n[1]*(Xr-Xq)};	//PQR面内 QR直线的法向量

    float Xn2=(Xr+Xq)*0.5f;
    float Yn2=(Yr+Yq)*0.5f;
    float Zn2=(Zr+Zq)*0.5f;

    float K1;
    if  (n[0]==0 && n[1]==0 && n[2]!=0)
    {
        K1=(n2[1]*(Xn2-Xn1)-n2[0]*(Yn2-Yn1))/(n1[0]*n2[1]-n2[0]*n1[1]);//面1平行于xoy
    }else if(n[0]!=0 && n[1]==0 && n[2]==0)
    {
        K1=(n2[2]*(Yn2-Yn1)-n2[1]*(Zn2-Zn1))/(n2[2]*n1[1]-n2[1]*n1[2]);//面1平行于yoz
    }else if(n[0]==0 && n[1]!=0 && n[2]==0)
    {
        K1=(n2[2]*(Xn2-Xn1)-n2[0]*(Zn2-Zn1))/(n2[2]*n1[0]-n2[0]*n1[2]);//面1平行于xoz
    }else
    {
        K1=(n2[1]*(Xn2-Xn1)-n2[0]*(Yn2-Yn1))/(n1[0]*n2[1]-n2[0]*n1[1]);//面1不平行于基面
    }

    //	pos_C 圆心坐标
    float Xc=n1[0]*K1+Xn1;
    float Yc=n1[1]*K1+Yn1;
    float Zc=n1[2]*K1+Zn1;

    *Radius=sqrt(pow(Xc-Xp,2.f)+pow(Yc-Yp,2.f)+pow(Zc-Zp,2.f));  //半径R