def extend_destination_left_border(lane_data):
"""
Extend the last section of the left border to a large size in order to find cross points with other lanes
:param lane_data: destination lane
:return: left border extended backwards
"""
return extend_vector(
lane_data['left_border'][:2]) + lane_data['left_border'][2:]
def extend_origin_left_border(lane_data, all_lanes):
"""
Extend the last section of the left border to a large size in order to find cross points with other lanes
:param lane_data: lane dictionary
:param all_lanes: list of dictionaries
:return: list of coordinates representing new extended border
"""
shorten_lane_for_crosswalk(lane_data, all_lanes)
return lane_data['left_shaped_border'][:-2] + extend_vector(
lane_data['left_shaped_border'][-2:], backward=False)
def construct_turn_arc_with_initial_angle(origin_border,
destination_border,
initial_angle=15.0,
number_of_points=12,
turn_direction=-1.0):
intersection_point, vector1, vector2 = get_turn_angle(
origin_border, destination_border)
if intersection_point is None:
return None
from_origin_to_intersection = great_circle_vec_check_for_nan(
intersection_point[1], intersection_point[0], vector1[1][1],
vector1[1][0])
from_destination_to_intersection = great_circle_vec_check_for_nan(
intersection_point[1], intersection_point[0], vector2[1][1],
vector2[1][0])
bearing1 = get_compass(vector1[1], vector1[0])
bearing2 = get_compass(vector2[0], vector2[1])
angle = ((turn_direction * (bearing2 - bearing1) + 360) % 360)
sin1 = math.sin(to_rad(180.0 - angle))
l_dest = from_destination_to_intersection
z = l_dest * sin1 / (sin1 + math.sin(to_rad(initial_angle)))
l_origin = z * math.cos(to_rad(initial_angle)) + (l_dest - z) * math.cos(
to_rad(180.0 - angle))
if from_origin_to_intersection > l_origin:
# Drive along the origin vector past the intersection
last_origin_point = extend_vector(vector1,
length=l_origin,
backward=False,
relative=False)[1]
else:
# Start turning immediately
last_origin_point = vector1[1]
azimuth = (get_compass(intersection_point, last_origin_point) -
initial_angle + 360) % 360
point = nv_frame.GeoPoint(latitude=last_origin_point[1],
longitude=last_origin_point[0],
degrees=True)
result, _azimuthb = point.geo_point(distance=100.0,
azimuth=azimuth,
degrees=True)
first_orin_point = (result.longitude_deg, result.latitude_deg)
angled_origin_border = [first_orin_point, last_origin_point]
return construct_turn_arc(angled_origin_border,
destination_border,
number_of_points=number_of_points,
turn_direction=turn_direction)
def construct_u_turn_arc(origin_border, destination_border, number_of_points=12):
"""
Construct a turn arc with the destination border
:param origin_border: list of coordinates
:param destination_border: list of coordinates
:param number_of_points: integer
:return: list of coordinates
"""
bearing1 = get_compass(origin_border[-2], origin_border[-1])
bearing2 = get_compass(destination_border[0], destination_border[1])
angle = abs(get_angle_between_bearings(bearing1, bearing2))
radius, landing_border = get_u_turn_radius_and_landing_border(origin_border, destination_border)
if radius > 50:
logger.debug('U-turn bearings %r, %r, angle %r' % (bearing1, bearing2, angle))
logger.warning('Radius is too large %r, landing border %r' % (radius, landing_border))
ob = cut_line_by_relative_distance(destination_border, 0.95)
radius, landing_border = get_u_turn_radius_and_landing_border(ob, destination_border)
logger.debug('Retry bearings %r, %r, angle %r' % (bearing1, bearing2, angle))
logger.debug('Adjusted radius %r, landing border %r' % (radius, landing_border))
else:
ob = origin_border
shift = [2.0 * radius * (math.sin(to_rad(angle / 2.0 * i / float(number_of_points)))) ** 2
for i in range(0, number_of_points + 1)
]
vec = [ob[-1], extend_vector(ob[-2:], length=30.0, backward=False, relative=True)[-1]]
vector = [extend_vector(vec,
length=radius * (math.sin(to_rad(angle * i / float(number_of_points)))),
backward=False
)[1]
for i in range(0, number_of_points + 1)
]
arc = [shift_by_bearing_and_distance(vector[i], shift[i], ob[-2:], bearing_delta=-90.0)
for i in range(0, number_of_points + 1)
]
return arc[:-1] + landing_border
def get_right_turn_border(origin_border, link_border, destination_border):
"""
Construct either left or right border for a right turn via link
:param origin_border: list of coordinates
:param link_border: list of coordinates
:param destination_border: list of coordinates
:return: list of coordinates
"""
last_segment = [origin_border[-2], origin_border[-1]]
extended = extend_vector(last_segment, length=1000.0, backward=False)
origin_line = geom.LineString(origin_border[:-1] + extended[1:])
extended_link = extend_both_sides_of_a_border(link_border)
link_line = geom.LineString(extended_link)
if not origin_line.intersects(link_line):
logger.error('Link border does not intersect the origin border')
return None
intersection_point1 = origin_line.intersection(link_line)
origin_line1 = cut_border_by_distance(
origin_line, origin_line.project(intersection_point1))[0]
ct1 = cut_border_by_distance(link_line,
link_line.project(intersection_point1))
if len(ct1) < 2:
logger.error('Link is too short')
return None
link_line1 = ct1[1]
destination_line = geom.LineString(destination_border)
if not destination_line.intersects(link_line1):
logger.error('Link border does not intersect the destination border')
return None
intersection_point2 = destination_line.intersection(link_line1)
line2 = cut_border_by_distance(
destination_line, destination_line.project(intersection_point2))[1]
return list(origin_line1.coords) + link_border[1:-1] + list(line2.coords)
def get_simulated_crosswalk(street_data, streets, width=1.8):
"""
Construct a simulated crosswalk
:param street_data: street dictionary
:param streets: list of dictionaries
:param width: float, crosswalk width in meters
:return: crosswalk dictionary
"""
if is_highway(street_data):
return None
logger.debug("Simulated crosswalk for %s" % street_data['name'])
right_border = shorten_border_for_crosswalk(street_data['right_border'],
street_data['name'],
streets,
crosswalk_width=3,
destination='to_intersection',
exclude_parallel=True,
max_reduction=130.0
)
left_border = shorten_border_for_crosswalk(street_data['left_border'],
street_data['name'],
streets,
crosswalk_width=3,
destination='to_intersection',
exclude_parallel=True,
max_reduction=130.0
)
right_border2 = shorten_border_for_crosswalk(street_data['right_border'],
street_data['name'],
streets,
crosswalk_width=3 + width,
destination='to_intersection',
exclude_parallel=True,
max_reduction=130.0
)
left_border2 = shorten_border_for_crosswalk(street_data['left_border'],
street_data['name'],
streets,
crosswalk_width=3 + width,
destination='to_intersection',
exclude_parallel=False,
max_reduction=130.0
)
vector = [right_border[-1], right_border2[-1]]
resized_vector = extend_vector(vector, length=width, backward=False)
vector2 = [get_closest_point(p, street_data['left_border']) for p in resized_vector]
# logger.debug("Vector %r" % vector)
# logger.debug("Resize %r" % resized_vector)
# logger.debug("Vecto2 %r" % vector2)
r_b = [right_border[-1], left_border[-1]]
r_l = [shift_by_bearing_and_distance(r_b[0], width, [right_border[-1], right_border[-2]],
bearing_delta=0.0),
shift_by_bearing_and_distance(r_b[-1], width, [left_border[-1], left_border[-2]],
bearing_delta=0.0)]
crosswalk = {
'lane_id': '1C',
'name': street_data['name'],
'simulated': 'yes',
# 'right_border': [vector[0], vector2[0]],
# 'left_border': [resized_vector[1], vector2[1]],
'right_border': r_b, # [right_border[-1], left_border[-1]],
'left_border': r_l, # [right_border2[-1], left_border2[-1]],
'median': shift_vector([right_border[-1], left_border[-1]], -width / 2.0),
'path_id': 0,
'path': [],
'lane_type': 'crosswalk',
'direction': 'undefined',
'nodes': [],
'nodes_coordinates': [],
'width': width,
'type': 'footway'
}
bearing = get_compass(crosswalk['right_border'][0], crosswalk['right_border'][-1])
crosswalk['bearing'] = bearing
crosswalk['compass'] = get_compass_rhumb(bearing),
crosswalk['length'] = get_border_length(crosswalk['median'])
if crosswalk['length'] > 50.0:
return None
if "id" in street_data:
crosswalk["street_id"] = street_data["id"]
logger.debug(
'Created crosswalk for street %s %s' % (street_data['name'], street_data['compass']))
return crosswalk
def construct_turn_arc(origin_border,
destination_border,
number_of_points=12,
turn_direction=-1.0,
lane=None):
"""
Construct a turn arc
:param origin_border: list of coordinates
:param destination_border: list of coordinates
:param number_of_points: integer
:param turn_direction: -1 if left turn otherwise 1
:return: list of coordinates
"""
intersection_point, vector1, vector2 = get_turn_angle(
origin_border, destination_border)
if intersection_point is None:
logger.debug('Origin %r' % origin_border)
logger.debug('Destin %r' % destination_border)
logger.debug('Cannot find intersection point')
return None
from_origin_to_intersection = great_circle_vec_check_for_nan(
intersection_point[1], intersection_point[0], vector1[1][1],
vector1[1][0])
from_destination_to_intersection = great_circle_vec_check_for_nan(
intersection_point[1], intersection_point[0], vector2[1][1],
vector2[1][0])
#logger.debug("Vectors 1 %r" % vector1)
#logger.debug("Vectors 2 %r" % vector2)
if lane is not None:
lane["left_vector"] = vector1
lane["right_vector"] = vector2
# logger.debug("Intersection point %r" % intersection_point)
bearing1 = get_compass(vector1[1], vector1[0])
bearing2 = get_compass(vector2[0], tuple(vector2[1]))
angle = ((turn_direction * (bearing2 - bearing1) + 360) % 360)
if from_origin_to_intersection < from_destination_to_intersection:
distance_to_starting_point = from_origin_to_intersection
dist_delta = 0.0
else:
distance_to_starting_point = from_destination_to_intersection
dist_delta = from_origin_to_intersection - from_destination_to_intersection
radius = distance_to_starting_point / math.tan(to_rad(angle / 2.0))
if radius < 0.0:
logger.error("Negative radius %r" % radius)
logger.debug("from_origin_to_intersection %r" %
from_origin_to_intersection)
logger.debug("from_destination_to_intersection %r" %
from_destination_to_intersection)
return None
else:
logger.debug("Radius %r" % radius)
shift = [
turn_direction * 2.0 * radius *
(math.sin(to_rad(angle / 2.0 * i / float(number_of_points))))**2
for i in range(0, number_of_points + 1)
]
vec = [origin_border[-1], intersection_point]
vector = [
extend_vector(vec,
length=dist_delta + radius *
(math.sin(to_rad(angle * i / float(number_of_points)))),
backward=False)[1]
for i in range(0, number_of_points + 1)
]
return [
shift_by_bearing_and_distance(vector[i],
shift[i],
vec,
bearing_delta=turn_direction * 90.0)
for i in range(0, number_of_points + 1)
]