public float pointLineSegmentDistance(PVector l1, PVector l2, PVector p)
{
    double x2 = l2.x - l1.x;
    double y2 = l2.y - l1.y;
    double z2 = l2.z - l1.z;

    double px = p.x - l1.x;
    double py = p.y - l1.y;
    double pz = p.z - l1.z;

    double dotprod = px * x2 + py * y2 + pz * z2;
    double projlenSq;

    if (dotprod <= 0.0) projlenSq = 0.0;
    else
    {
        px = x2 - px;
        py = y2 - py;
        pz = z2 - pz;
        dotprod = px * x2 + py * y2 + pz * z2;

        if (dotprod <= 0.0) projlenSq = 0.0;
        else projlenSq = dotprod * dotprod / (x2 * x2 + y2 * y2 + z2 * z2);
    }

    double lenSq = px * px + py * py + pz * pz - projlenSq;

    if (lenSq < 0) lenSq = 0;

    return sqrt((float)lenSq);
}