43 lines
1.6 KiB
Python
43 lines
1.6 KiB
Python
import numpy as np
|
|
from scipy import interpolate
|
|
|
|
|
|
class Curve:
|
|
def __init__(self, target_points):
|
|
# list of points to [(x1, y1, z1), (...), ...]
|
|
self.target_points = target_points
|
|
self.computed_points = []
|
|
|
|
def compute_curve(self, resolution=40):
|
|
"""
|
|
Fill self.computed_points with a list of points that approximate a smooth curve following self.target_points.
|
|
|
|
https://stackoverflow.com/questions/18962175/spline-interpolation-coefficients-of-a-line-curve-in-3d-space
|
|
|
|
Args:
|
|
points (np.array): Points where the curve should pass in order.
|
|
resolution (int, optional): Total number of points to compute. Defaults to 40.
|
|
"""
|
|
# Remove duplicates. Curve can't intersect itself
|
|
points = tuple(map(tuple, np.array(self.target_points)))
|
|
points = sorted(set(points), key=points.index)
|
|
|
|
# Change coordinates structure to (x1, x2, x3, ...), (y1, y2, y3, ...) (z1, z2, z3, ...)
|
|
coords = np.array(points, dtype=np.float32)
|
|
x = coords[:, 0]
|
|
y = coords[:, 1]
|
|
z = coords[:, 2]
|
|
|
|
# Compute
|
|
tck, u = interpolate.splprep([x, y, z], s=2, k=2)
|
|
x_knots, y_knots, z_knots = interpolate.splev(tck[0], tck)
|
|
u_fine = np.linspace(0, 1, resolution)
|
|
x_fine, y_fine, z_fine = interpolate.splev(u_fine, tck)
|
|
|
|
x_rounded = np.round(x_fine).astype(int)
|
|
y_rounded = np.round(y_fine).astype(int)
|
|
z_rounded = np.round(z_fine).astype(int)
|
|
|
|
self.computed_points = [(x, y, z) for x, y, z in zip(
|
|
x_rounded, y_rounded, z_rounded)]
|