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 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)
]