distance.py

def pointPointDistance(p1, p2):
    '''
        point-to-point distance
        @param p1: first point as tuple
        @param p2: second point as tuple
        @return distance between two points as float
    '''
    return sum((a - b) ** 2 for a, b in zip(p1, p2)) ** 0.5


def pointLineDistance3D(p, seg, testSegmentEnds=False):
    '''
        minimum distance between a point and a line in 3D
        @param p: point
        @param seg: tuple of two points defining a line
        @return: tuple of minimum distance and projection of p on seg
    '''
    # assign passed tuples to coordinates
    x3, y3, z3 = p
    (x1, y1, z1), (x2, y2, z2) = seg

    # distance between line end points in each direction
    dx21 = x2 - x1
    dy21 = y2 - y1
    dz21 = z2 - z1

    # square of line length
    lensq21 = dx21 ** 2 + dy21 ** 2 + dz21 ** 2
    if lensq21 is 0:
        # we have a point!
        return None, pointPointDistance((x2, y2, z2), (x3, y3, z3))

    # projection of X3X1 on X2X1, normalized with length of X2X1
    u = (x3 - x1) * dx21 + (y3 - y1) * dy21 + (z3 - z1) * dz21
    u /= float(lensq21)

    # calculate projection of X3 on X2X1
    x = x1 + u * dx21
    y = y1 + u * dy21
    z = z1 + u * dz21

    if testSegmentEnds:
        if u < 0:
            x, y, z = x1, y1, z1
        elif u > 1:
            x, y, z = x2, y2, z2

    dx30 = x3 - x
    dy30 = y3 - y
    dz30 = z3 - z

    return (x, y, z), (dx30 ** 2 + dy30 ** 2 + dz30 ** 2) ** 0.5