fix spline

target_simulator/core/models.py

@@ -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
+                pos_ft = [
-                target_pos = [
                     (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 = (
+                dir_multiplier = 1.0 if wp.turn_direction == TurnDirection.RIGHT else -1.0
-                    1.0 if wp.turn_direction == TurnDirection.RIGHT else -1.0
-                )  # Right turn = positive change in heading angle
 
-                # --- 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
+            vertices.append((total_duration_s, pos_ft[0], pos_ft[1], pos_ft[2]))
-                    keyframes.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]
+            points_to_spline = [p[1:] for p in vertices]
-            final_path = []
+            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 final_path, final_duration
-        return keyframes, total_duration_s
+
+        # 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:

target_simulator/utils/spline.py

@@ -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
+    total_segments = n - 1
-    for i in range(1, n):
+    if total_segments <= 0:
-        p0, p1, p2, p3 = extended[i - 1], extended[i], extended[i + 1], extended[i + 2]
+        return points.tolist()
-        for t in np.linspace(0, 1, num_points // (n - 1)):
+
-            t2 = t * t
+    for k in range(num_points):
-            t3 = t2 * t
+        s = (k / (num_points - 1)) * total_segments
-            # Catmull-Rom formula
+        seg = int(np.floor(s))
-            pt = 0.5 * (
+        if seg >= total_segments:
-                (2 * p1)
+            seg = total_segments - 1
-                + (-p0 + p2) * t
+        t = s - seg
-                + (2 * p0 - 5 * p1 + 4 * p2 - p3) * t2
+
-                + (-p0 + 3 * p1 - 3 * p2 + p3) * t3
+        # 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

todo.md

@@ -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