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