Fix & speed up max curvature calculation

[stklik]

The current curvature calculation is slow and relies on interation over a Python list. It also drops the last curvature value (due to a strange treatment of w in the range(len(nodes) - w).

The following code produces the same results (except for a handful of floating point issues in the 17th post-comma digit), but uses numpy instead of individual values. As you can see, structurally, the code still resembles the original (see _fast_define_circle), but is based on numpy array.

def _fast_define_circle(p1, p2, p3):
    """
    Returns the center and radius of the circle passing the given 3 points.
    In case the 3 points form a line, returns (None, infinity).
    """
    p1_0 = p1[:,0]
    p1_1 = p1[:,1]
    p2_0 = p2[:,0]
    p2_1 = p2[:,1]
    p3_0 = p3[:,0]
    p3_1 = p3[:,1]
    
    temp = p2_0 * p2_0 + p2_1 * p2_1
    bc = (p1_0 * p1_0 + p1_1 * p1_1 - temp) / 2
    cd = (temp - p3_0 * p3_0 - p3_1 * p3_1) / 2
    det = (p1_0 - p2_0) * (p2_1 - p3_1) - (p2_0 - p3_0) * (p1_1 - p2_1)
    
    radius = np.full(temp.shape, np.inf)
    det_fltr = np.abs(det) > 1.0e-6

    # Center of circle
    cx = (bc*(p2_1 - p3_1) - cd*(p1_1 - p2_1))[det_fltr] / det[det_fltr]
    cy = ((p1_0 - p2_0) * cd - (p2_0 - p3_0) * bc)[det_fltr] / det[det_fltr]

    radius[det_fltr] = np.sqrt((cx - p1_0[det_fltr])**2 + (cy - p1_1[det_fltr])**2)
        
    return radius

def fast_max_curvature(the_test, w=5, ORIGINAL_BEHAVIOUR=False):
    if not isinstance(the_test, list):
        nodes = the_test.interpolated_points
    else:
        nodes = the_test
    min_radius = np.inf
    
    np_nodes = np.array(nodes)
    # print(np_nodes)
    
    p1 = np_nodes[:-(w-1)]
    step = int((w-1) / 2)
    p2 = np_nodes[step:-step]
    p3 = np_nodes[w-1:]

    assert len(p1) == len(p2) == len(p3)
    
    radii = _fast_define_circle(p1, p2, p3)

    if ORIGINAL_BEHAVIOUR:   # NOTE: original code for some reason does not consider the last curvature value... it's strange! (I guess it's an maybe off-by-one error?) 
        curvature = 1 / radii[:-1].min()     
        return curvature
    else:  # fixed behaviour
        curvature = 1 / radii.min()     
        return curvature

NOTE the parameter ORIGINAL_BEHAVIOUR should eventually be dropped and the fixed behaviour adopted. It's currently here to prove that the results are the same.