def snap_points(self, points):
last_way = None
snapped_points = []
last_distance = None
# headings before snapping
headings = []
for i, p in enumerate(points):
if i < len(points) - 1:
next_point = points[i + 1]
headings.append(
Route.get_heading(p.y, p.x, next_point.y, next_point.x))
headings.append(headings[-1])
h_diff = []
for point_index in range(len(points)):
point = points[point_index]
heading = headings[point_index]
# if last_way is not None:
# projection = last_way.project(point)
# distance = last_way.distance(point)
# if 0 <= projection <= 1 and distance < 1.1 * last_distance:
# snapped_point = last_way.interpolate(projection)
# last_distance = distance
# snapped_points += [snapped_point]
# continue
last_way = self.get_closest_way_with_heading(point, heading)
# last_way = self.get_closest_way(point)
projection = last_way.project(point)
# last_distance = last_way.distance(point)
snapped_point = last_way.interpolate(projection)
h = WaySet.get_linestring_heading_at_projection(
last_way, projection)
h_diff += [headings[point_index] - h]
snapped_points += [snapped_point]
# plt.plot(h_diff)
# plt.show()
# headings after snapping
headings = []
for i, p in enumerate(snapped_points):
if i < len(snapped_points) - 1:
next_point = snapped_points[i + 1]
headings.append(
Route.get_heading(p.y, p.x, next_point.y, next_point.x))
headings.append(headings[-1])
return Route.from_points(snapped_points, headings)
