63 lines
1.7 KiB
Python
63 lines
1.7 KiB
Python
"""
|
|
Spline utilities for Catmull-Rom interpolation and trajectory sampling.
|
|
"""
|
|
|
|
import numpy as np
|
|
|
|
|
|
def catmull_rom_spline(points, num_points=100):
|
|
"""
|
|
Calcola una Catmull-Rom spline che passa per tutti i punti dati.
|
|
Args:
|
|
points: lista di tuple (x, y) o (x, y, z)
|
|
num_points: numero di punti da campionare lungo la curva
|
|
Returns:
|
|
Lista di punti campionati sulla spline
|
|
"""
|
|
points = np.asarray(points, dtype=float)
|
|
n = len(points)
|
|
if n < 4:
|
|
# Not enough points for a spline, return a polyline
|
|
return points.tolist()
|
|
|
|
# Pad the points to ensure continuity at the ends
|
|
p_start = points[0]
|
|
p_end = points[-1]
|
|
extended_points = np.vstack([p_start, points, p_end])
|
|
|
|
# Define the Catmull-Rom matrix
|
|
C = 0.5 * np.array([
|
|
[0, 2, 0, 0],
|
|
[-1, 0, 1, 0],
|
|
[2, -5, 4, -1],
|
|
[-1, 3, -3, 1]
|
|
])
|
|
|
|
result = []
|
|
total_segments = n - 1
|
|
if total_segments <= 0:
|
|
return points.tolist()
|
|
|
|
for k in range(num_points):
|
|
s = (k / (num_points - 1)) * total_segments
|
|
seg = int(np.floor(s))
|
|
if seg >= total_segments:
|
|
seg = total_segments - 1
|
|
t = s - seg
|
|
|
|
# Control points for the segment
|
|
# The segment is between P1 and P2 of the control points
|
|
# extended_points is indexed s.t. extended_points[i+1] = points[i]
|
|
P = extended_points[seg : seg + 4]
|
|
|
|
# Powers of t
|
|
T = np.array([1, t, t**2, t**3])
|
|
|
|
# Calculate the point
|
|
pt = T @ C @ P
|
|
result.append(pt.tolist())
|
|
|
|
# Ensure exact endpoints match control points
|
|
result[0] = points[0].tolist()
|
|
result[-1] = points[-1].tolist()
|
|
return result |