findNearestPointOfTwoRays.cpp

    static vec findNearestPoint(vector<vec> line1, vector<vec>line2)
    {
        //vec p_0 = {line1[0](0), line1[0](1), line1[0](2)};
        //vec q_0 = {line2[0](0), line2[0](1), line2[0](2)};
        vec p_0 = line1[0];
        vec q_0 = line2[0];
        vec u = line1[1];
        vec v = line2[1];


        vec w_0 = p_0 - q_0;
        double s_c = (dot(u,v)*dot(v,w_0)-dot(u,w_0))/(1-dot(u,v)*dot(u,v));
        double t_c = (dot(v,w_0)-dot(u,v)*dot(u, w_0))/(1-dot(u, v)*dot(u, v));
        //cout<<"s_c="<<s_c<<endl;

        vec p_s = q_0 + t_c * v;
        vec q_c = p_0 + s_c * u;
        return (p_s + q_c)/2;

    }