diff --git a/target_simulator/core/models.py b/target_simulator/core/models.py index d552582..e447bb5 100644 --- a/target_simulator/core/models.py +++ b/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 - 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: diff --git a/target_simulator/utils/spline.py b/target_simulator/utils/spline.py index 35227f9..23f13af 100644 --- a/target_simulator/utils/spline.py +++ b/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 - 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 + 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 \ No newline at end of file diff --git a/todo.md b/todo.md index c9f51ea..01ec219 100644 --- a/todo.md +++ b/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