VALLONGOL/S1005403_RisCC
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix spline

Browse Source
This commit is contained in:
VALLONGOL 2025-10-20 08:25:03 +02:00
parent 261df8bad0
commit 9c89bf92a8
3 changed files with 104 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

91
target_simulator/core/models.py
View File

@ -138,12 +138,13 @@ class Target:
def generate_path_from_waypoints(
waypoints: List[Waypoint], use_spline: bool
) -> Tuple[List[Tuple[float, ...]], float]:
path, total_duration_s = [], 0.0
if not waypoints or waypoints[0].maneuver_type != ManeuverType.FLY_TO_POINT:
return path, total_duration_s
return [], 0.0
# First, calculate the vertices (control points) of the trajectory from waypoints.
vertices = []
total_duration_s = 0.0
first_wp = waypoints[0]
keyframes = []
pos_ft = [
(first_wp.target_range_nm or 0.0)
* NM_TO_FT
@ -153,21 +154,20 @@ class Target:
* math.cos(math.radians(first_wp.target_azimuth_deg or 0.0)),
first_wp.target_altitude_ft or 0.0,
]
speed_fps = first_wp.target_velocity_fps or 0.0
heading_rad = math.radians(first_wp.target_heading_deg or 0.0)
pitch_rad = 0.0
keyframes.append((total_duration_s, pos_ft[0], pos_ft[1], pos_ft[2]))
vertices.append((total_duration_s, pos_ft[0], pos_ft[1], pos_ft[2]))
for i, wp in enumerate(waypoints):
duration = wp.duration_s or 0.0
if i == 0:
continue
duration = wp.duration_s or 0.0
if wp.maneuver_type == ManeuverType.FLY_TO_POINT:
start_pos, start_time = list(pos_ft), total_duration_s
target_pos = [
pos_ft = [
(wp.target_range_nm or 0.0)
* NM_TO_FT
* math.sin(math.radians(wp.target_azimuth_deg or 0.0)),
@ -176,15 +176,6 @@ class Target:
* math.cos(math.radians(wp.target_azimuth_deg or 0.0)),
wp.target_altitude_ft or pos_ft[2],
]
time_step, num_steps = 0.1, max(1, int(duration / 0.1))
for step in range(1, num_steps + 1):
progress = step / num_steps
current_time = start_time + progress * duration
pos_ft = [
start_pos[j] + (target_pos[j] - start_pos[j]) * progress
for j in range(3)
]
keyframes.append((current_time, pos_ft[0], pos_ft[1], pos_ft[2]))
total_duration_s += duration
if (
wp.target_velocity_fps is not None
@ -211,23 +202,17 @@ class Target:
if wp.target_altitude_ft is not None:
pos_ft[2] = wp.target_altitude_ft
total_duration_s += duration
keyframes.append((total_duration_s, pos_ft[0], pos_ft[1], pos_ft[2]))
elif wp.maneuver_type == ManeuverType.DYNAMIC_MANEUVER:
if wp.maneuver_speed_fps is not None:
speed_fps = wp.maneuver_speed_fps
time_step, num_steps = 0.1, int(duration / 0.1)
accel_lon_fps2 = (wp.longitudinal_acceleration_g or 0.0) * G_IN_FPS2
accel_lat_fps2 = (wp.lateral_acceleration_g or 0.0) * G_IN_FPS2
accel_ver_fps2 = (wp.vertical_acceleration_g or 0.0) * G_IN_FPS2
dir_multiplier = (
1.0 if wp.turn_direction == TurnDirection.RIGHT else -1.0
) # Right turn = positive change in heading angle
dir_multiplier = 1.0 if wp.turn_direction == TurnDirection.RIGHT else -1.0
# --- NUOVA LOGICA DI INTEGRAZIONE SEMPLIFICATA ---
for _ in range(num_steps):
# Update kinematics for this step
speed_fps += accel_lon_fps2 * time_step
pitch_change_rad = (
(accel_ver_fps2 / (speed_fps + 1e-6)) * time_step
@ -241,39 +226,67 @@ class Target:
else 0
)
heading_rad += turn_rate_rad_s * time_step * dir_multiplier
# Calculate distances moved in this step
dist_step = speed_fps * time_step
dist_step_xy = dist_step * math.cos(pitch_rad)
dist_step_z = dist_step * math.sin(pitch_rad)
# Update position
pos_ft[0] += dist_step_xy * math.sin(heading_rad)
pos_ft[1] += dist_step_xy * math.cos(heading_rad)
pos_ft[2] += dist_step_z
total_duration_s += duration
total_duration_s += time_step
keyframes.append(
(total_duration_s, pos_ft[0], pos_ft[1], pos_ft[2])
)
vertices.append((total_duration_s, pos_ft[0], pos_ft[1], pos_ft[2]))
if use_spline and len(keyframes) >= 4:
# Now that we have the vertices, either spline them or generate a dense segmented path.
if use_spline and len(vertices) >= 4:
from target_simulator.utils.spline import catmull_rom_spline
points_to_spline = [p[1:] for p in keyframes]
final_path = []
points_to_spline = [p[1:] for p in vertices]
num_spline_points = max(len(vertices) * 20, 200)
splined_points = catmull_rom_spline(
points_to_spline, num_points=max(len(keyframes), 100)
points_to_spline, num_points=num_spline_points
)
final_path = []
final_duration = vertices[-1][0]
for i, point in enumerate(splined_points):
time = (
(i / (len(splined_points) - 1)) * total_duration_s
(i / (len(splined_points) - 1)) * final_duration
if len(splined_points) > 1
else 0.0
)
final_path.append((time, point[0], point[1], point[2]))
return final_path, total_duration_s
return keyframes, total_duration_s
return final_path, final_duration
# If not using spline, generate the dense, segmented path for simulation.
# This re-uses the original logic but iterates through the calculated vertices.
keyframes = []
if not vertices:
return [], 0.0
keyframes.append(vertices[0])
for i in range(len(vertices) - 1):
start_v = vertices[i]
end_v = vertices[i+1]
start_time, start_pos = start_v[0], list(start_v[1:])
end_time, end_pos = end_v[0], list(end_v[1:])
duration = end_time - start_time
if duration <= 0:
continue
time_step, num_steps = 0.1, max(1, int(duration / 0.1))
for step in range(1, num_steps + 1):
progress = step / num_steps
current_time = start_time + progress * duration
current_pos = [
start_pos[j] + (end_pos[j] - start_pos[j]) * progress
for j in range(3)
]
keyframes.append((current_time, current_pos[0], current_pos[1], current_pos[2]))
final_duration = vertices[-1][0]
return keyframes, final_duration
def update_state(self, delta_time_s: float):
if not self.active or not self._path:

60
target_simulator/utils/spline.py
View File

@ -14,26 +14,50 @@ def catmull_rom_spline(points, num_points=100):
Returns:
Lista di punti campionati sulla spline
"""
points = np.array(points)
points = np.asarray(points, dtype=float)
n = len(points)
if n < 4:
# Troppo pochi punti per spline: restituisci la polilinea
# Not enough points for a spline, return a polyline
return points.tolist()
# Estendi i punti per garantire la continuità agli estremi
extended = np.vstack([points[0], points, points[-1]])
# 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 = []
# 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())
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

10
todo.md
View File

@ -1,4 +1,12 @@
code da fare
# cose da fare
## bachi
quando faccio la traiettoria ad alti 9 sbaglia a disegnare la curva, rivedere il caso specifico ed anche con o senza spline
## sviluppi
leggere le informazioni dell'antenna del messaggio di stato
leggere la flag per capire se il target è attivo