def connected_test(q1, q2):
#n = len(samples)
#threshold = gamma * (math.log(n) / n) ** (1. / d)
threshold = max_distance
are_connected = (get_distance(q1, q2) <= threshold) and \
is_collision_free((q1, q2), obstacles)
if are_connected:
roadmap.append((q1, q2))
return are_connected
def distance_fn(q1, q2):
return scale_distance(get_distance(q1, q2))