def test_45_degree_variants_end_point(self): arc_45_deg = Arc(Point(0, 10), 0, 10, 45) self.assertAlmostEqual(arc_45_deg.end_point(), Point(7.07106781, 12.92893218)) arc_135_deg = Arc(Point(0, 0), 90, 10, 135) self.assertAlmostEqual(arc_135_deg.end_point(), Point(-17.07106781, 7.07106781)) arc_225_deg = Arc(Point(0, 0), 315, 10, 225) self.assertAlmostEqual(arc_225_deg.end_point(), Point(7.07106781, 17.07106781)) arc_315_deg = Arc(Point(0, 0), -45, 10, 315) self.assertAlmostEqual(arc_315_deg.end_point(), Point(-2.92893218, 7.07106781))
def test_includes_point(self): arc_90_deg = Arc(Point(0, 0), 90, 10, 90) self.assertTrue(arc_90_deg.includes_point(Point(0, 0))) self.assertTrue( arc_90_deg.includes_point(Point(-2.92893219, 7.07106781))) self.assertTrue(arc_90_deg.includes_point(Point(-10, 10))) arc_135_neg_deg = Arc(Point(0, 0), 90, 10, -135) self.assertTrue(arc_135_neg_deg.includes_point(Point(0, 0))) self.assertTrue( arc_135_neg_deg.includes_point(Point(2.92893219, 7.07106781))) self.assertTrue( arc_135_neg_deg.includes_point(Point(17.07106781, 7.07106781))) arc_270_deg = Arc(Point(-1, 0), 90, 3, 270) self.assertTrue(arc_270_deg.includes_point(Point(-4, -3))) null_arc = Arc(Point(0, 0), 80, 20, 0) self.assertTrue(null_arc.includes_point(Point(0, 0))) arc = Arc(Point(0, -1), 0, 1, 180) self.assertFalse(arc.includes_point(Point(0, 0))) self.assertFalse(arc.includes_point(Point(-1, 0))) arc = Arc(Point(1, 0), -90, 1, -180).counter_clockwise() self.assertTrue(arc.includes_point(Point(-1, 0))) self.assertTrue(arc.includes_point(Point(1, 0))) arc1 = Arc(Point(1, 0), 90, 1, 180).counter_clockwise() arc2 = Arc(Point(1, 0), -90, 1, -180).counter_clockwise() self.assertTrue(arc1.includes_point(arc2.start_point())) self.assertTrue(arc1.includes_point(arc2.end_point()))
def build_path_and_waypoints(cls, lane, mapped_centers): if lane.road_nodes_count() < 2: raise ValueError( "At least two nodes are required to build a geometry") road_nodes = list(lane.road_nodes()) is_circular = mapped_centers[road_nodes[0]].almost_equal_to( mapped_centers[road_nodes[-1]], 5) path = Path() waypoints = [] previous_node = road_nodes.pop(0) previous_point = mapped_centers[previous_node] nodes_count = len(road_nodes) for index, node in enumerate(road_nodes): point = mapped_centers[node] is_last_node = index + 1 == nodes_count if is_last_node: if is_circular: next_point = mapped_centers[road_nodes[0]] else: next_point = None else: next_point = mapped_centers[road_nodes[index + 1]] if path.is_empty(): previous_element_end_point = previous_point else: previous_element_end_point = path.element_at(-1).end_point() if next_point is None: element = LineSegment(previous_element_end_point, point) path.add_element(element) waypoints.append( cls._new_waypoint(lane, element, previous_node)) waypoints.append( cls._new_waypoint(lane, element, road_nodes[-1], False)) else: previous_vector = point - previous_point next_vector = next_point - point if previous_vector.is_collinear_with(next_vector): element = LineSegment(previous_element_end_point, point) path.add_element(element) waypoints.append( cls._new_waypoint(lane, element, previous_node)) if is_last_node: waypoints.append( cls._new_waypoint(lane, element, road_nodes[0], False)) else: inverted_previous_segment = LineSegment( point, previous_point) real_inverted_previous_segment = LineSegment( point, previous_element_end_point) next_segment = LineSegment(point, next_point) if is_last_node: real_next_segment = LineSegment( point, path.element_at(0).end_point()) delta = min(real_inverted_previous_segment.length(), real_next_segment.length(), 5) else: delta = min(real_inverted_previous_segment.length(), next_segment.length() / 2.0, 5) previous_segment_new_end_point = real_inverted_previous_segment.point_at_offset( delta) next_segment_new_start_point = next_segment.point_at_offset( delta) previous_segment = LineSegment( previous_element_end_point, previous_segment_new_end_point) # Try to avoid small segments if previous_segment.length() < 0.25: # `- 1e-10` to avoid length overflow due to floating point math new_delta = delta + previous_segment.length() - 1e-10 if next_segment.length() > new_delta: previous_segment_new_end_point = real_inverted_previous_segment.point_at_offset( new_delta) next_segment_new_start_point = next_segment.point_at_offset( new_delta) previous_segment = LineSegment( previous_element_end_point, previous_segment_new_end_point) angle_between_vectors = previous_vector.angle(next_vector) d2 = previous_segment_new_end_point.squared_distance_to( next_segment_new_start_point) cos = math.cos(math.radians(angle_between_vectors)) radius = math.sqrt(d2 / (2.0 * (1.0 - cos))) # If there should be no segment, just an arc. Use previous_element_end_point to # avoid rounding errors and make a perfect overlap if previous_segment.length() < 1e-8: if path.not_empty(): heading = path.element_at(-1).end_heading() else: heading = inverted_previous_segment.inverted( ).start_heading() connection_arc = Arc(previous_element_end_point, heading, radius, angle_between_vectors) path.add_element(connection_arc) waypoints.append( cls._new_waypoint(lane, connection_arc, node)) if not connection_arc.end_point().almost_equal_to( next_segment_new_start_point, 3): raise RuntimeError( "Expecting arc end {0} to match next segment entry point {1}" .format(connection_arc.end_point(), next_segment_new_start_point)) else: heading = inverted_previous_segment.inverted( ).start_heading() connection_arc = Arc(previous_segment_new_end_point, heading, radius, angle_between_vectors) path.add_element(previous_segment) waypoints.append( cls._new_waypoint(lane, previous_segment, previous_node)) path.add_element(connection_arc) waypoints.append( cls._new_waypoint(lane, connection_arc, node)) if not connection_arc.end_point().almost_equal_to( next_segment_new_start_point, 3): raise RuntimeError( "Expecting arc end {0} to match next segment entry point {1}" .format(connection_arc.end_point(), next_segment_new_start_point)) if is_last_node: if connection_arc.end_point().distance_to( path.element_at(1).start_point()) < 1e-8: path.remove_first_element() waypoints.pop(0) else: first_element = path.element_at(0) new_first_element = LineSegment( connection_arc.end_point(), first_element.end_point()) path.replace_first_element(new_first_element) waypoints[0] = cls._new_waypoint( lane, new_first_element, lane.road_nodes()[0]) waypoints.append( cls._new_waypoint(lane, connection_arc, road_nodes[0], False)) previous_node = node previous_point = mapped_centers[previous_node] return (path, waypoints)
def test_axis_aligned_arc_end_point(self): # Positive heading and angular length arc_90_deg = Arc(Point(0, 0), 90, 10, 90) self.assertAlmostEqual(arc_90_deg.end_point(), Point(-10, 10)) arc_180_deg = Arc(Point(0, 0), 90, 10, 180) self.assertAlmostEqual(arc_180_deg.end_point(), Point(-20, 0)) arc_270_deg = Arc(Point(0, 0), 90, 10, 270) self.assertAlmostEqual(arc_270_deg.end_point(), Point(-10, -10)) arc_360_deg = Arc(Point(0, 0), 90, 10, 360) self.assertAlmostEqual(arc_360_deg.end_point(), Point(0, 0)) # Positive heading and negative angular length arc_minus_90_deg = Arc(Point(0, 0), 90, 10, -90) self.assertAlmostEqual(arc_minus_90_deg.end_point(), Point(10, 10)) arc_minus_180_deg = Arc(Point(0, 0), 90, 10, -180) self.assertAlmostEqual(arc_minus_180_deg.end_point(), Point(20, 0)) arc_minus_270_deg = Arc(Point(0, 0), 90, 10, -270) self.assertAlmostEqual(arc_minus_270_deg.end_point(), Point(10, -10)) arc_minus_360_deg = Arc(Point(0, 0), 90, 10, -360) self.assertAlmostEqual(arc_minus_360_deg.end_point(), Point(0, 0)) # Negative heading and positive angular length arc_minus_90_deg = Arc(Point(0, 0), -90, 10, 90) self.assertAlmostEqual(arc_minus_90_deg.end_point(), Point(10, -10)) arc_minus_180_deg = Arc(Point(0, 0), -90, 10, 180) self.assertAlmostEqual(arc_minus_180_deg.end_point(), Point(20, 0)) arc_minus_270_deg = Arc(Point(0, 0), -90, 10, 270) self.assertAlmostEqual(arc_minus_270_deg.end_point(), Point(10, 10)) arc_minus_360_deg = Arc(Point(0, 0), -90, 10, 360) self.assertAlmostEqual(arc_minus_360_deg.end_point(), Point(0, 0)) # Negative heading and angular length arc_90_deg = Arc(Point(0, 0), -90, 10, -90) self.assertAlmostEqual(arc_90_deg.end_point(), Point(-10, -10)) arc_180_deg = Arc(Point(0, 0), -90, 10, -180) self.assertAlmostEqual(arc_180_deg.end_point(), Point(-20, 0)) arc_270_deg = Arc(Point(0, 0), -90, 10, -270) self.assertAlmostEqual(arc_270_deg.end_point(), Point(-10, 10)) arc_360_deg = Arc(Point(0, 0), -90, 10, -360) self.assertAlmostEqual(arc_360_deg.end_point(), Point(0, 0))
def test_null_arc_end_point(self): null_arc = Arc(Point(0, 0), 90, 10, 0) self.assertAlmostEqual(null_arc.end_point(), Point(0, 0))
def test_end_point_after_counter_clockwise(self): arc = Arc(Point(1, 0), -90, 1, -180).counter_clockwise() self.assertAlmostEqual(arc.end_point(), Point(1, 0))