sdf.py 文件源码

def find_zero_crossing_quadratic(x1, y1, x2, y2, x3, y3, eps = 1.0):
        """ Find zero crossing using quadratic approximation along 1d line"""
        # compute coords along 1d line
        v = x2 - x1
        v = v / np.linalg.norm(v)
        if v[v!=0].shape[0] == 0:
            logging.error('Difference is 0. Probably a bug')

        t1 = 0
        t2 = (x2 - x1)[v!=0] / v[v!=0]
        t2 = t2[0]
        t3 = (x3 - x1)[v!=0] / v[v!=0]
        t3 = t3[0]

        # solve for quad approx
        x1_row = np.array([t1**2, t1, 1])
        x2_row = np.array([t2**2, t2, 1])
        x3_row = np.array([t3**2, t3, 1])
        X = np.array([x1_row, x2_row, x3_row])
        y_vec = np.array([y1, y2, y3])
        try:
            w = np.linalg.solve(X, y_vec)
        except np.linalg.LinAlgError:
            logging.error('Singular matrix. Probably a bug')
            return None

        # get positive roots
        possible_t = np.roots(w)
        t_zc = None
        for i in range(possible_t.shape[0]):
            if possible_t[i] >= 0 and possible_t[i] <= 10 and not np.iscomplex(possible_t[i]):
                t_zc = possible_t[i]

        # if no positive roots find min
        if np.abs(w[0]) < 1e-10:
            return None

        if t_zc is None:
            t_zc = -w[1] / (2 * w[0])

        if t_zc < -eps or t_zc > eps:
            return None

        x_zc = x1 + t_zc * v
        return x_zc