Ejemplo n.º 1
0
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:]
Ejemplo n.º 2
0
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)
Ejemplo n.º 3
0
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)
Ejemplo n.º 4
0
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
Ejemplo n.º 5
0
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)
Ejemplo n.º 6
0
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
Ejemplo n.º 7
0
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)
    ]