def sample_path_points(path_points, path_sample_spacing):
path_edges = util.to_pairs(path_points)
sampled_path_points, sampled_arc_lengths, last_arc_length = \
sample_edges(path_edges, path_sample_spacing)
last_point = path_points[-1]
sampled_arc_lengths.append(last_arc_length)
sampled_path_points.append(last_point)
sampled_path_points_array = np.array([np.array(point) for point
in sampled_path_points])
sampled_arc_lengths_array = np.array(sampled_arc_lengths)
return [sampled_path_points_array, sampled_arc_lengths_array]
def build_extended_quintic(s, x):
"""
Extends quint() to allow for high numbers (> 5) of waypoints to be
interpolated, without running into ill-conditioning problems.
"""
num_intervals = len(s) - 1
boundary_indices = get_boundaries_indices(num_intervals)
if len(boundary_indices) == 0:
first_derivative_initial_value = 0
first_derivative_final_value = 0
partitions_quintic_coeffs = quint(s, x, first_derivative_initial_value,
first_derivative_final_value)
else:
boundary_first_derivatives = []
initial_first_derivative = 0
boundary_first_derivatives.append(initial_first_derivative)
for i in boundary_indices:
boundary_first_derivative = ((x[i + 1] - x[i - 1]) /
(s[i + 1] - s[i - 1]))
boundary_first_derivatives.append(boundary_first_derivative)
final_first_derivative = 0
boundary_first_derivatives.append(final_first_derivative)
boundary_indices_pairs = util.to_pairs(boundary_indices)
s_partitions = []
for boundary_index_pair in boundary_indices_pairs:
boundary_index_a, boundary_index_b = boundary_index_pair
s_partition = s[joined_index_a : joined_index_b + 1]
s_partitions.append(s_partition)
x_partitions = []
for boundary_index_pair in boundary_indices_pairs:
boundary_index_a, boundary_index_b = boundary_index_pair
x_partition = x[joined_index_a : joined_index_b + 1]
x_partitions.append(x_partition)
partitions_quintic_coeffs = []
for i in range(len(joined_indices) - 1):
partition_quintic_coeff = quint(s_partitions[i],
x_partitions[i],
boundary_first_derivatives[i],
boundary_first_derivatives[i + 1])
partitions_quintic_coeffs.append(partition_quintic_coeff)
return partitions_quintic_coeffs
def compute_total_distance(self, geospatials):
geospatial_edges = util.to_pairs(geospatials)
geospatial_vectors = [edge[1] - edge[0] for edge in geospatial_edges]
edge_lengths = np.linalg.norm(geospatial_vectors, axis=1)
total_distance = np.sum(edge_lengths)
return total_distance
def build_tube_edges(self, tube_points):
tube_points_pairs = util.to_pairs(tube_points)
tube_edges = [TubeEdge(pair[0], pair[1]) for pair in tube_points_pairs]
return tube_edges
