38 lines
1.2 KiB
Python
38 lines
1.2 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.array(points)
|
|
n = len(points)
|
|
if n < 4:
|
|
# Troppo pochi punti per spline: restituisci la polilinea
|
|
return points.tolist()
|
|
# Estendi i punti per garantire la continuità agli estremi
|
|
extended = np.vstack([points[0], points, points[-1]])
|
|
result = []
|
|
# Usa n-1 segmenti validi
|
|
for i in range(1, n):
|
|
p0, p1, p2, p3 = extended[i-1], extended[i], extended[i+1], extended[i+2]
|
|
for t in np.linspace(0, 1, num_points//(n-1)):
|
|
t2 = t*t
|
|
t3 = t2*t
|
|
# Catmull-Rom formula
|
|
pt = (
|
|
0.5 * ((2*p1) +
|
|
(-p0 + p2)*t +
|
|
(2*p0 - 5*p1 + 4*p2 - p3)*t2 +
|
|
(-p0 + 3*p1 - 3*p2 + p3)*t3)
|
|
)
|
|
result.append(pt.tolist())
|
|
return result
|