toys/gps/docs/line2line.cpp

float DistanceLineToLine(const osg::Vec3d& p1, const osg::Vec3d& p2,
                         const osg::Vec3d& p3, const osg::Vec3d& p4)
{
    float distance;
    float x1 = p1.x(); // A点坐标（x1,y1,z1）
    float y1 = p1.y();
    float z1 = p1.z();
    float x2 = p2.x(); // B点坐标（x2,y2,z2）
    float y2 = p2.y();
    float z2 = p2.z();
    float x3 = p3.x(); // C点坐标（x3,y3,z3）
    float y3 = p3.y();
    float z3 = p3.z();
    float x4 = p4.x(); // D点坐标（x4,y4,z4）
    float y4 = p4.y();
    float z4 = p4.z();

    float a =
        (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1) + (z2 - z1) * (z2 - z1);
    float b = -((x2 - x1) * (x4 - x3) + (y2 - y1) * (y4 - y3) +
                (z2 - z1) * (z4 - z3));
    float c = -((x1 - x2) * (x1 - x3) + (y1 - y2) * (y1 - y3) +
                (z1 - z2) * (z1 - z3));

    float d = -((x2 - x1) * (x4 - x3) + (y2 - y1) * (y4 - y3) +
                (z2 - z1) * (z4 - z3));
    float e =
        (x4 - x3) * (x4 - x3) + (y4 - y3) * (y4 - y3) + (z4 - z3) * (z4 - z3);
    float f = -((x1 - x3) * (x4 - x3) + (y1 - y3) * (y4 - y3) +
                (z1 - z3) * (z4 - z3));

    if ((a * e - b * d) == 0 && (b * d - a * e) == 0) //平行
    {
        float d1 = (p1 - p3).length();
        float d2 = (p1 - p4).length();
        distance = (d1 < d2) ? d1 : d2;
        return distance;
    }

    float s = (b * f - e * c) / (a * e - b * d);
    float t = (a * f - d * c) / (b * d - a * e);

    if (0 <= s && s <= 1 && 0 <= t &&
        t <= 1) //说明P点落在线段AB上,Q点落在线段CD上
    {
        // 2条线段的公垂线段PQ;
        // P点坐标
        float X = x1 + s * (x2 - x1);
        float Y = y1 + s * (y2 - y1);
        float Z = z1 + s * (z2 - z1);
        // Q点坐标
        float U = x3 + t * (x4 - x3);
        float V = y3 + t * (y4 - y3);
        float W = z3 + t * (z4 - z3);
        osg::Vec3d P(X, Y, Z);
        osg::Vec3d Q(U, V, W);
        distance = (P - Q).length();
    }
    else
    {
        float d1 = DistancePointToLine(p3, p4, p1);
        float d2 = DistancePointToLine(p3, p4, p2);
        float d3 = DistancePointToLine(p1, p2, p3);
        float d4 = DistancePointToLine(p1, p2, p4);
        distance = (d1 < d2) ? d1 : d2;
        distance = (distance < d3) ? distance : d3;
        distance = (distance < d4) ? distance : d4;
    }
    return distance;
}

float DistancePointToLine(const osg::Vec3d& star, const osg::Vec3d& end,
                          const osg::Vec3d& center)
{
    float distance;
    float x0 = center.x(); // P点坐标（x0,y0,z0）
    float y0 = center.y();
    float z0 = center.z();
    float x1 = star.x(); // A点坐标（x1,y1,z1）
    float y1 = star.y();
    float z1 = star.z();
    float x2 = end.x(); // B点坐标（x2,y2,z2）
    float y2 = end.y();
    float z2 = end.z();
    float t =
        ((x1 - x0) * (x1 - x2) + (y1 - y0) * (y1 - y2) +
         (z1 - z0) * (z1 - z2)) /
        ((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2) + (z1 - z2) * (z1 - z2));
    if (0 <= t && t <= 1) //垂足Q点落在线段AB上
    {
        float X = x1 + t * (x2 - x1);
        float Y = y1 + t * (y2 - y1);
        float Z = z1 + t * (z2 - z1);
        osg::Vec3d Q(X, Y, Z);
        distance = (Q - center).length();
    }
    if (t < 0) //垂足Q点不落在线段AB上，而是落在BA的延长线上
    {
        distance = (star - center).length();
    }
    if (t > 1) //垂足Q点不落在线段AB上，而是落在AB的延长线上
    {
        distance = (end - center).length();
    }
    return distance;
}