"""Quadratic- and CubicBezierCurves."""
import functools
from dataclasses import dataclass
from typing import Sequence, Union
import numpy as np
import simulation.utils.road.sections.type as road_section_type
from simulation.utils.geometry import Line, Point
from simulation.utils.road.sections.road_section import RoadSection
[docs]def _read_point(p: Union[Point, Sequence[float]]) -> np.ndarray:
"""Get numpy array from point or coordinate sequence."""
try:
return p.to_numpy()[:2]
except AttributeError:
return np.array(p)
[docs]def _compute_cubic_bezier(t, p0, p1, p2, p3):
c0 = (1 - t) * p0 + t * p1
c1 = (1 - t) * p1 + t * p2
c2 = (1 - t) * p2 + t * p3
d0 = (1 - t) * c0 + t * c1
d1 = (1 - t) * c1 + t * c2
x = (1 - t) * d0 + t * d1
return x
[docs]def add_quad_bezier_points(p0, p1, p2, p3):
return [Point(_compute_cubic_bezier(t, p0, p1, p2, p3)) for t in np.linspace(0, 1, 100)]
[docs]@dataclass
class BezierCurve(RoadSection):
p1: Union["Point", Sequence[float]] = None
"""Control point 1."""
p2: Union["Point", Sequence[float]] = None
"""Control point 2."""
def __post_init__(self):
assert self.p1 is not None, f"Missing first point for {self.__class__}."
assert self.p1.y == 0, f"Y-Value of first point hast to be zero. Not {self.p1.y}!"
"""Y-Value of first control point has to be zero to have a gradient of zero at the start.
If this is not the case the bezier curve does not fit to the previous road section.
"""
assert self.p2 is not None, f"Missing second point for {self.__class__}."
self._p0 = _read_point(Point(0, 0))
"""First point p0 is always set to (0, 0) and can not be changed."""
self._p1 = _read_point(self.p1)
self._p2 = _read_point(self.p2)
super().__post_init__()
[docs]@dataclass
class QuadBezier(BezierCurve):
"""Quadratic bezier curve, defined by two control points.
Args:
p1 (Union[Point, Sequence[float]]): Control point 1.
Y-Value has to be zero in order to match the gradient
of the last section at the start.
p2 (Union[Point, Sequence[float]]): Control point 2.
"""
TYPE = road_section_type.QUAD_BEZIER
@functools.cached_property
def middle_line(self) -> Line:
points = []
t = 0
while t <= 1:
c0 = (1 - t) * self._p0 + t * self._p1
c1 = (1 - t) * self._p1 + t * self._p2
x = (1 - t) * c0 + t * c1
points.append(Point(*x))
t += 0.01
t = 1
c0 = (1 - t) * self._p0 + t * self._p1
c1 = (1 - t) * self._p1 + t * self._p2
x = (1 - t) * c0 + t * c1
points.append(Point(*x))
return self.transform * Line(points)
[docs]@dataclass
class CubicBezier(BezierCurve):
"""Cubic bezier curve, defined by three control points.
Args:
p1 (Union[Point, Sequence[float]]): Control point 1.
p2 (Union[Point, Sequence[float]]): Control point 2.
p3 (Union[Point, Sequence[float]]): Control point 3.
"""
TYPE = road_section_type.CUBIC_BEZIER
p3: Union[Point, Sequence[float]] = None
"""Control point 3."""
def __post_init__(self):
assert self.p3 is not None, "Missing third point for CubicBezier."
self._p3 = _read_point(self.p3)
super().__post_init__()
@property
def middle_line(self) -> Line:
"""Line: Middle line of the road section."""
points = add_quad_bezier_points(self._p0, self._p1, self._p2, self._p3)
return self.transform * Line(points)