40 lines
1.2 KiB
Python
40 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
|