[utils] add explicit spiral and circular arc helpers, and allow non-90deg bends
This commit is contained in:
parent
f4df8e0553
commit
833f5dd159
2 changed files with 215 additions and 46 deletions
|
|
@ -15,7 +15,7 @@ from ..utils import (
|
||||||
rotation_matrix_2d,
|
rotation_matrix_2d,
|
||||||
)
|
)
|
||||||
from ..file.utils import tmpfile
|
from ..file.utils import tmpfile
|
||||||
from ..utils.curves import bezier
|
from ..utils.curves import bezier, circular_arc, euler_bend, euler_spiral
|
||||||
from ..error import PatternError
|
from ..error import PatternError
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -148,6 +148,53 @@ def test_bezier_accepts_exact_weight_count() -> None:
|
||||||
assert_allclose(samples, [[0, 0], [2 / 3, 2 / 3], [1, 1]], atol=1e-10)
|
assert_allclose(samples, [[0, 0], [2 / 3, 2 / 3], [1, 1]], atol=1e-10)
|
||||||
|
|
||||||
|
|
||||||
|
def _endpoint_tangent(xy: numpy.ndarray) -> float:
|
||||||
|
dxy = xy[-1] - xy[-2]
|
||||||
|
return numpy.arctan2(dxy[1], dxy[0])
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.mark.parametrize(
|
||||||
|
('switchover_angle', 'total_angle'),
|
||||||
|
[
|
||||||
|
(pi / 8, pi / 4),
|
||||||
|
(pi / 8, pi / 2),
|
||||||
|
(pi / 4, pi),
|
||||||
|
],
|
||||||
|
)
|
||||||
|
def test_euler_bend_supports_total_angle(switchover_angle: float, total_angle: float) -> None:
|
||||||
|
xy = euler_bend(switchover_angle, num_points=2000, total_angle=total_angle)
|
||||||
|
|
||||||
|
assert_allclose(xy[0], [0, 0], atol=1e-12)
|
||||||
|
assert_allclose(_endpoint_tangent(xy), -total_angle, atol=1e-3)
|
||||||
|
|
||||||
|
|
||||||
|
def test_euler_bend_180_degrees_with_90_degree_circular_middle() -> None:
|
||||||
|
xy = euler_bend(pi / 4, num_points=2000, total_angle=pi)
|
||||||
|
|
||||||
|
assert_allclose(_endpoint_tangent(xy), -pi, atol=1e-3)
|
||||||
|
assert abs(xy[-1][0]) < 1e-3
|
||||||
|
assert xy[-1][1] < 0
|
||||||
|
|
||||||
|
|
||||||
|
def test_euler_bend_rejects_too_large_switchover_angle() -> None:
|
||||||
|
with pytest.raises(PatternError, match='total_angle / 2'):
|
||||||
|
euler_bend(pi / 2, total_angle=pi / 2)
|
||||||
|
|
||||||
|
|
||||||
|
def test_euler_spiral_and_circular_arc_helpers_match_endpoint_tangent() -> None:
|
||||||
|
xy_spiral = euler_spiral(pi / 4, num_points=1000)
|
||||||
|
assert_allclose(_endpoint_tangent(xy_spiral), -pi / 4, atol=1e-3)
|
||||||
|
|
||||||
|
xy_arc = circular_arc(
|
||||||
|
10,
|
||||||
|
pi / 2,
|
||||||
|
num_points=1000,
|
||||||
|
start_angle=_endpoint_tangent(xy_spiral),
|
||||||
|
start=xy_spiral[-1],
|
||||||
|
)
|
||||||
|
assert_allclose(_endpoint_tangent(xy_arc), -3 * pi / 4, atol=2e-3)
|
||||||
|
|
||||||
|
|
||||||
def test_deferred_dict_accessors_resolve_values_once() -> None:
|
def test_deferred_dict_accessors_resolve_values_once() -> None:
|
||||||
calls = 0
|
calls = 0
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -3,11 +3,7 @@ from numpy.typing import ArrayLike, NDArray
|
||||||
from numpy import pi
|
from numpy import pi
|
||||||
|
|
||||||
from ..error import PatternError
|
from ..error import PatternError
|
||||||
|
from .vertices import remove_duplicate_vertices
|
||||||
try:
|
|
||||||
from numpy import trapezoid
|
|
||||||
except ImportError:
|
|
||||||
from numpy import trapz as trapezoid # type:ignore
|
|
||||||
|
|
||||||
|
|
||||||
def bezier(
|
def bezier(
|
||||||
|
|
@ -53,70 +49,196 @@ def bezier(
|
||||||
return qq
|
return qq
|
||||||
|
|
||||||
|
|
||||||
|
def _integrate_tangent(
|
||||||
|
qq: NDArray[numpy.float64],
|
||||||
|
theta: NDArray[numpy.float64],
|
||||||
|
num_points: int,
|
||||||
|
) -> NDArray[numpy.float64]:
|
||||||
|
dx = numpy.cos(theta)
|
||||||
|
dy = numpy.sin(theta)
|
||||||
|
|
||||||
|
dq = qq[-1] / (qq.size - 1)
|
||||||
|
ix = numpy.zeros(qq.size)
|
||||||
|
iy = numpy.zeros(qq.size)
|
||||||
|
ix[1:] = numpy.cumsum((dx[:-1] + dx[1:]) / 2) * dq
|
||||||
|
iy[1:] = numpy.cumsum((dy[:-1] + dy[1:]) / 2) * dq
|
||||||
|
|
||||||
|
qq_target = numpy.linspace(0, qq[-1], num_points)
|
||||||
|
x_target = numpy.interp(qq_target, qq, ix)
|
||||||
|
y_target = numpy.interp(qq_target, qq, iy)
|
||||||
|
|
||||||
|
return numpy.stack((x_target, y_target), axis=1)
|
||||||
|
|
||||||
|
|
||||||
|
def euler_spiral(
|
||||||
|
switchover_angle: float,
|
||||||
|
num_points: int = 200,
|
||||||
|
*,
|
||||||
|
start_angle: float = 0.0,
|
||||||
|
start: ArrayLike = (0.0, 0.0),
|
||||||
|
reverse: bool = False,
|
||||||
|
) -> NDArray[numpy.float64]:
|
||||||
|
"""
|
||||||
|
Generate one Euler bend transition segment.
|
||||||
|
|
||||||
|
Positive angles bend clockwise, matching `euler_bend()`. When `reverse` is
|
||||||
|
`False`, curvature ramps from zero to the switchover curvature. When
|
||||||
|
`reverse` is `True`, curvature ramps from the switchover curvature to zero.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
switchover_angle: Tangent angle change across the Euler segment, in radians.
|
||||||
|
num_points: Number of points in the curve.
|
||||||
|
start_angle: Tangent angle at the first point.
|
||||||
|
start: First point of the segment.
|
||||||
|
reverse: If `True`, generate the exit segment of an Euler bend.
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
`[[x0, y0], ...]` for the curve.
|
||||||
|
"""
|
||||||
|
if num_points < 0:
|
||||||
|
raise PatternError(f'num_points must be non-negative, got {num_points}')
|
||||||
|
if switchover_angle < 0:
|
||||||
|
raise PatternError(f'switchover_angle must be non-negative, got {switchover_angle}')
|
||||||
|
if num_points == 0:
|
||||||
|
return numpy.empty((0, 2))
|
||||||
|
|
||||||
|
start = numpy.asarray(start, dtype=float)
|
||||||
|
if start.shape != (2,):
|
||||||
|
raise PatternError(f'start must be a 2D point; got shape {start.shape}')
|
||||||
|
|
||||||
|
if switchover_angle == 0:
|
||||||
|
return numpy.tile(start, (num_points, 1))
|
||||||
|
|
||||||
|
resolution = 100000
|
||||||
|
ll_max = numpy.sqrt(2 * switchover_angle)
|
||||||
|
qq = numpy.linspace(0, ll_max, resolution)
|
||||||
|
if reverse:
|
||||||
|
theta = start_angle - (ll_max * qq - qq * qq / 2)
|
||||||
|
else:
|
||||||
|
theta = start_angle - qq * qq / 2
|
||||||
|
|
||||||
|
return _integrate_tangent(qq, theta, num_points) + start
|
||||||
|
|
||||||
|
|
||||||
|
def circular_arc(
|
||||||
|
radius: float,
|
||||||
|
arc_angle: float,
|
||||||
|
num_points: int = 200,
|
||||||
|
*,
|
||||||
|
start_angle: float = 0.0,
|
||||||
|
start: ArrayLike = (0.0, 0.0),
|
||||||
|
) -> NDArray[numpy.float64]:
|
||||||
|
"""
|
||||||
|
Generate a clockwise circular arc.
|
||||||
|
|
||||||
|
Args:
|
||||||
|
radius: Arc radius.
|
||||||
|
arc_angle: Clockwise tangent angle change across the arc, in radians.
|
||||||
|
num_points: Number of points in the curve, excluding the start point.
|
||||||
|
start_angle: Tangent angle at the start point.
|
||||||
|
start: Point where the arc starts.
|
||||||
|
|
||||||
|
Returns:
|
||||||
|
`[[x0, y0], ...]` for the arc, excluding `start`.
|
||||||
|
"""
|
||||||
|
if num_points < 0:
|
||||||
|
raise PatternError(f'num_points must be non-negative, got {num_points}')
|
||||||
|
if radius <= 0:
|
||||||
|
raise PatternError(f'radius must be positive, got {radius}')
|
||||||
|
if arc_angle < 0:
|
||||||
|
raise PatternError(f'arc_angle must be non-negative, got {arc_angle}')
|
||||||
|
if num_points == 0:
|
||||||
|
return numpy.empty((0, 2))
|
||||||
|
|
||||||
|
start = numpy.asarray(start, dtype=float)
|
||||||
|
if start.shape != (2,):
|
||||||
|
raise PatternError(f'start must be a 2D point; got shape {start.shape}')
|
||||||
|
|
||||||
|
if arc_angle == 0:
|
||||||
|
return numpy.tile(start, (num_points, 1))
|
||||||
|
|
||||||
|
angles = numpy.linspace(0, arc_angle, num_points + 1)[1:]
|
||||||
|
right_normal = numpy.array([numpy.sin(start_angle), -numpy.cos(start_angle)])
|
||||||
|
center = start + radius * right_normal
|
||||||
|
radial = start - center
|
||||||
|
|
||||||
|
cos_t = numpy.cos(-angles)
|
||||||
|
sin_t = numpy.sin(-angles)
|
||||||
|
xx = center[0] + cos_t * radial[0] - sin_t * radial[1]
|
||||||
|
yy = center[1] + sin_t * radial[0] + cos_t * radial[1]
|
||||||
|
return numpy.stack((xx, yy), axis=1)
|
||||||
|
|
||||||
|
|
||||||
def euler_bend(
|
def euler_bend(
|
||||||
switchover_angle: float,
|
switchover_angle: float,
|
||||||
num_points: int = 200,
|
num_points: int = 200,
|
||||||
|
*,
|
||||||
|
total_angle: float = pi / 2,
|
||||||
) -> NDArray[numpy.float64]:
|
) -> NDArray[numpy.float64]:
|
||||||
"""
|
"""
|
||||||
Generate a 90 degree Euler bend (AKA Clothoid bend or Cornu spiral).
|
Generate an Euler bend (AKA Clothoid bend or Cornu spiral).
|
||||||
|
|
||||||
|
Positive angles bend clockwise. By default, this generates the historical
|
||||||
|
90 degree bend.
|
||||||
|
|
||||||
Args:
|
Args:
|
||||||
switchover_angle: After this angle, the bend will transition into a circular arc
|
switchover_angle: After this angle, the bend will transition into a circular arc
|
||||||
(and transition back to an Euler spiral on the far side). If this is set to
|
(and transition back to an Euler spiral on the far side). If this is set to
|
||||||
`>= pi / 4`, no circular arc will be added.
|
`total_angle / 2`, no circular arc will be added.
|
||||||
num_points: Number of points in the curve
|
num_points: Number of points in the curve.
|
||||||
|
total_angle: Total tangent angle change across the bend, in radians.
|
||||||
|
|
||||||
Returns:
|
Returns:
|
||||||
`[[x0, y0], ...]` for the curve
|
`[[x0, y0], ...]` for the curve
|
||||||
"""
|
"""
|
||||||
|
if switchover_angle <= 0:
|
||||||
|
raise PatternError(f'switchover_angle must be positive, got {switchover_angle}')
|
||||||
|
if total_angle <= 0:
|
||||||
|
raise PatternError(f'total_angle must be positive, got {total_angle}')
|
||||||
|
if switchover_angle > total_angle / 2:
|
||||||
|
raise PatternError(
|
||||||
|
f'switchover_angle must be <= total_angle / 2; '
|
||||||
|
f'got {switchover_angle} for total_angle {total_angle}'
|
||||||
|
)
|
||||||
|
if num_points < 2:
|
||||||
|
raise PatternError(f'num_points must be at least 2, got {num_points}')
|
||||||
|
|
||||||
|
arc_angle = total_angle - 2 * switchover_angle
|
||||||
ll_max = numpy.sqrt(2 * switchover_angle) # total length of (one) spiral portion
|
ll_max = numpy.sqrt(2 * switchover_angle) # total length of (one) spiral portion
|
||||||
ll_tot = 2 * ll_max + (pi / 2 - 2 * switchover_angle)
|
ll_tot = 2 * ll_max + arc_angle
|
||||||
num_points_spiral = numpy.floor(ll_max / ll_tot * num_points).astype(int)
|
num_points_spiral = max(2, numpy.floor(ll_max / ll_tot * num_points).astype(int))
|
||||||
num_points_arc = num_points - 2 * num_points_spiral
|
num_points_arc = max(0, num_points - 2 * num_points_spiral)
|
||||||
|
if arc_angle > 0:
|
||||||
|
num_points_arc = max(1, num_points_arc)
|
||||||
|
|
||||||
def gen_spiral(ll_max: float) -> NDArray[numpy.float64]:
|
xy_spiral = euler_spiral(switchover_angle, num_points_spiral)
|
||||||
if ll_max == 0:
|
|
||||||
return numpy.zeros((num_points_spiral, 2))
|
|
||||||
|
|
||||||
resolution = 100000
|
|
||||||
qq = numpy.linspace(0, ll_max, resolution)
|
|
||||||
dx = numpy.cos(qq * qq / 2)
|
|
||||||
dy = -numpy.sin(qq * qq / 2)
|
|
||||||
|
|
||||||
dq = ll_max / (resolution - 1)
|
|
||||||
ix = numpy.zeros(resolution)
|
|
||||||
iy = numpy.zeros(resolution)
|
|
||||||
ix[1:] = numpy.cumsum((dx[:-1] + dx[1:]) / 2) * dq
|
|
||||||
iy[1:] = numpy.cumsum((dy[:-1] + dy[1:]) / 2) * dq
|
|
||||||
|
|
||||||
ll_target = numpy.linspace(0, ll_max, num_points_spiral)
|
|
||||||
x_target = numpy.interp(ll_target, qq, ix)
|
|
||||||
y_target = numpy.interp(ll_target, qq, iy)
|
|
||||||
|
|
||||||
return numpy.stack((x_target, y_target), axis=1)
|
|
||||||
|
|
||||||
xy_spiral = gen_spiral(ll_max)
|
|
||||||
xy_parts = [xy_spiral]
|
xy_parts = [xy_spiral]
|
||||||
|
|
||||||
if switchover_angle < pi / 4:
|
if arc_angle > 0:
|
||||||
# Build a circular segment to join the two euler portions
|
# Build a circular segment to join the two euler portions
|
||||||
rmin = 1.0 / ll_max
|
rmin = 1.0 / ll_max
|
||||||
half_angle = pi / 4 - switchover_angle
|
xy_arc = circular_arc(
|
||||||
qq = numpy.linspace(half_angle * 2, 0, num_points_arc + 1) + switchover_angle
|
rmin,
|
||||||
xc = rmin * numpy.cos(qq)
|
arc_angle,
|
||||||
yc = rmin * numpy.sin(qq) + xy_spiral[-1, 1]
|
num_points_arc,
|
||||||
xc += xy_spiral[-1, 0] - xc[0]
|
start_angle=-switchover_angle,
|
||||||
yc += xy_spiral[-1, 1] - yc[0]
|
start=xy_spiral[-1],
|
||||||
xy_parts.append(numpy.stack((xc[1:], yc[1:]), axis=1))
|
)
|
||||||
|
xy_parts.append(xy_arc)
|
||||||
|
|
||||||
endpoint_xy = xy_parts[-1][-1, :]
|
endpoint_xy = xy_parts[-1][-1, :]
|
||||||
second_spiral = xy_spiral[::-1, ::-1] + endpoint_xy - xy_spiral[-1, ::-1]
|
second_spiral = euler_spiral(
|
||||||
|
switchover_angle,
|
||||||
|
num_points_spiral,
|
||||||
|
start_angle=-(total_angle - switchover_angle),
|
||||||
|
start=endpoint_xy,
|
||||||
|
reverse=True,
|
||||||
|
)
|
||||||
|
|
||||||
xy_parts.append(second_spiral)
|
xy_parts.append(second_spiral[1:])
|
||||||
xy = numpy.concatenate(xy_parts)
|
xy = numpy.concatenate(xy_parts)
|
||||||
|
|
||||||
# Remove any 2x-duplicate points
|
# Remove any 2x-duplicate points
|
||||||
xy = xy[(numpy.roll(xy, 1, axis=0) - xy > 1e-12).any(axis=1)]
|
xy = remove_duplicate_vertices(xy, closed_path=False, tolerance=1e-12)
|
||||||
|
|
||||||
return xy
|
return xy
|
||||||
|
|
|
||||||
Loading…
Add table
Add a link
Reference in a new issue