159 lines
6.1 KiB
Python
159 lines
6.1 KiB
Python
"""Interpolating cubic flight paths and geometric (not mask-based) clipping."""
|
|
|
|
import math
|
|
from dataclasses import dataclass
|
|
|
|
Point2D = tuple[float, float]
|
|
Rectangle = tuple[float, float, float, float]
|
|
|
|
|
|
def mix(a: Point2D, b: Point2D, fraction: float) -> Point2D:
|
|
return a[0] + (b[0] - a[0]) * fraction, a[1] + (b[1] - a[1]) * fraction
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class Cubic:
|
|
start: Point2D
|
|
control1: Point2D
|
|
control2: Point2D
|
|
end: Point2D
|
|
|
|
@property
|
|
def points(self) -> tuple[Point2D, Point2D, Point2D, Point2D]:
|
|
return self.start, self.control1, self.control2, self.end
|
|
|
|
def point(self, fraction: float) -> Point2D:
|
|
a, b, c = (
|
|
mix(p, q, fraction)
|
|
for p, q in (
|
|
(self.start, self.control1),
|
|
(self.control1, self.control2),
|
|
(self.control2, self.end),
|
|
)
|
|
)
|
|
return mix(mix(a, b, fraction), mix(b, c, fraction), fraction)
|
|
|
|
def split(self, fraction: float) -> tuple["Cubic", "Cubic"]:
|
|
a, b, c = (
|
|
mix(p, q, fraction)
|
|
for p, q in (
|
|
(self.start, self.control1),
|
|
(self.control1, self.control2),
|
|
(self.control2, self.end),
|
|
)
|
|
)
|
|
d, e = mix(a, b, fraction), mix(b, c, fraction)
|
|
middle = mix(d, e, fraction)
|
|
return Cubic(self.start, a, d, middle), Cubic(middle, e, c, self.end)
|
|
|
|
def portion(self, first: float, last: float) -> "Cubic":
|
|
if not 0 <= first < last <= 1:
|
|
raise ValueError("Curve portion must be an increasing interval in [0, 1].")
|
|
left = self if last == 1 else self.split(last)[0]
|
|
return left if first == 0 else left.split(first / last)[1]
|
|
|
|
def transform(self, matrix: list[float]) -> "Cubic":
|
|
a, b, d, e, xoff, yoff = matrix
|
|
points = [(a * x + b * y + xoff, d * x + e * y + yoff) for x, y in self.points]
|
|
return Cubic(*points)
|
|
|
|
def boundary_roots(self, axis: int, boundary: float) -> list[float]:
|
|
p0, p1, p2, p3 = (point[axis] for point in self.points)
|
|
a = -p0 + 3 * p1 - 3 * p2 + p3
|
|
b = 3 * p0 - 6 * p1 + 3 * p2
|
|
c = -3 * p0 + 3 * p1
|
|
critical = [0.0, 1.0]
|
|
if abs(a) < 1e-12:
|
|
if abs(b) >= 1e-12:
|
|
critical.append(-c / (2 * b))
|
|
else:
|
|
discriminant = 4 * b * b - 12 * a * c
|
|
if discriminant >= 0:
|
|
root = math.sqrt(discriminant)
|
|
critical.extend(((-2 * b - root) / (6 * a), (-2 * b + root) / (6 * a)))
|
|
critical = sorted(t for t in critical if 0 <= t <= 1)
|
|
roots = []
|
|
for first, last in zip(critical, critical[1:], strict=False):
|
|
low, high = first, last
|
|
f_low = self.point(low)[axis] - boundary
|
|
f_high = self.point(high)[axis] - boundary
|
|
if abs(f_low) < 1e-9:
|
|
roots.append(low)
|
|
if abs(f_high) < 1e-9:
|
|
roots.append(high)
|
|
if f_low * f_high >= 0:
|
|
continue
|
|
for _ in range(55):
|
|
middle = (low + high) / 2
|
|
f_middle = self.point(middle)[axis] - boundary
|
|
if f_low * f_middle <= 0:
|
|
high = middle
|
|
else:
|
|
low, f_low = middle, f_middle
|
|
roots.append((low + high) / 2)
|
|
return roots
|
|
|
|
def inside_intervals(self, rectangle: Rectangle) -> list[tuple[float, float]]:
|
|
west, south, east, north = rectangle
|
|
xs, ys = [p[0] for p in self.points], [p[1] for p in self.points]
|
|
if min(xs) >= west and max(xs) <= east and min(ys) >= south and max(ys) <= north:
|
|
return [(0.0, 1.0)]
|
|
if max(xs) < west or min(xs) > east or max(ys) < south or min(ys) > north:
|
|
return []
|
|
parameters = [0.0, 1.0]
|
|
for axis, boundary in ((0, west), (0, east), (1, south), (1, north)):
|
|
parameters.extend(self.boundary_roots(axis, boundary))
|
|
parameters.sort()
|
|
unique = [parameters[0]]
|
|
for value in parameters[1:]:
|
|
if value - unique[-1] > 1e-12:
|
|
unique.append(value)
|
|
result: list[tuple[float, float]] = []
|
|
for first, last in zip(unique, unique[1:], strict=False):
|
|
x, y = self.point((first + last) / 2)
|
|
if west - 1e-9 <= x <= east + 1e-9 and south - 1e-9 <= y <= north + 1e-9:
|
|
if result and abs(result[-1][1] - first) < 1e-12:
|
|
result[-1] = result[-1][0], last
|
|
else:
|
|
result.append((first, last))
|
|
return result
|
|
|
|
def clipped(self, rectangle: Rectangle) -> list["Cubic"]:
|
|
return [self.portion(first, last) for first, last in self.inside_intervals(rectangle)]
|
|
|
|
|
|
def interpolate(points: list[Point2D]) -> list[Cubic]:
|
|
"""Centripetal Catmull-Rom converted to cubic Beziers through every sample."""
|
|
result = []
|
|
for index in range(len(points) - 1):
|
|
p1, p2 = points[index], points[index + 1]
|
|
if p1 == p2:
|
|
result.append(Cubic(p1, p1, p2, p2))
|
|
continue
|
|
p0 = points[index - 1] if index > 0 else p1
|
|
p3 = points[index + 2] if index + 2 < len(points) else p2
|
|
if p0 == p1:
|
|
p0 = (2 * p1[0] - p2[0], 2 * p1[1] - p2[1])
|
|
if p3 == p2:
|
|
p3 = (2 * p2[0] - p1[0], 2 * p2[1] - p1[1])
|
|
d01 = math.sqrt(math.dist(p0, p1))
|
|
d12 = math.sqrt(math.dist(p1, p2))
|
|
d23 = math.sqrt(math.dist(p2, p3))
|
|
control1 = []
|
|
control2 = []
|
|
for axis in (0, 1):
|
|
tangent1 = d12 * (
|
|
(p1[axis] - p0[axis]) / d01
|
|
- (p2[axis] - p0[axis]) / (d01 + d12)
|
|
+ (p2[axis] - p1[axis]) / d12
|
|
)
|
|
tangent2 = d12 * (
|
|
(p2[axis] - p1[axis]) / d12
|
|
- (p3[axis] - p1[axis]) / (d12 + d23)
|
|
+ (p3[axis] - p2[axis]) / d23
|
|
)
|
|
control1.append(p1[axis] + tangent1 / 3)
|
|
control2.append(p2[axis] - tangent2 / 3)
|
|
result.append(Cubic(p1, (control1[0], control1[1]), (control2[0], control2[1]), p2))
|
|
return result
|