simtk.org

Code: Select all

def mergeByDistance(pathPoints, minDistance, verbose=False):
    '''
    Merge path points with the neighbours in a sequence if they  are at a 
    distance less less than 'minDistance' from each other.

    Parameters
    ----------
    pathPoints : numpy.array(n, 3)
        x-, y-, and z-coordinates of the n points defining pathline.
    minDistance : float
        minimum Carthesian distance allowed of neighboring points to be 
        separated in a path.
    verbose : bool
        Swith to print points that are being merged.

    Returns
    -------
        numpy.array(n, 1)

    '''

    points = list()
    count = 0
    for i, point in enumerate(pathPoints):
        if i == 0:
            points.append(point)
        else:
            delta = point - points[-1]
            d = np.linalg.norm(delta)
            if d < minDistance:
                if verbose:
                    print('mergeByDistance: points {0:d} and {1:d} by distance'+
                          ' {2:f} < {3:f}.'.format(i-1, i, d, minDistance))
                points[-1] += 0.5*delta  # last point moves to halfway
                count += 1
            else:
                if verbose:
                    print('mergeByDistance: points {0:d} no change.'.format(i))
                points.append(point)
    print('mergeByDistance: {0:d} of {1:d} merged.'.format(count, i))

    return np.array(points)