# -*- coding: utf-8 -*- """ 水中航线（WaterRoute）— 完整版 """ from __future__ import annotations import math from dataclasses import dataclass, field from typing import List, Optional from PyQt5.QtCore import QObject from uas.route.base_params import WaterParams from uas.route.base_route import BaseRoute from uas.route.base_segment import BaseSegment, BaseTransition, SegmentKind from uas.route.base_waypoint import EquipmentDomain from uas.utils.geo_utils import GeoUtils # ───────────────────────────────────────────────────────────────── # 水中直线段 # ───────────────────────────────────────────────────────────────── @dataclass class WaterSegment(BaseSegment): """水中直线航行段（水面/水下通用）""" domain: EquipmentDomain = field(default=EquipmentDomain.WATER, repr=False) kind: SegmentKind = field(default=SegmentKind.STRAIGHT, repr=False) depth_start: float = 0.0 depth_end: float = 0.0 is_subsurface: bool = False def query(self, t: float) -> WaterParams: return _water_seg_interpolate(self, t) def interpolate(self, ratio: float) -> WaterParams: t = self.t_start + max(0.0, min(1.0, ratio)) * self.duration return _water_seg_interpolate(self, t) def validate(self) -> list: errors = [] if self.distance < 0: errors.append("距离不能为负") if self.duration < 0: errors.append("时长不能为负") if self.depth_start < 0 or self.depth_end < 0: errors.append("深度不能为负") return errors @property def depth_change(self) -> float: return self.depth_end - self.depth_start @property def is_diving(self) -> bool: return self.depth_change > 1.0 @property def is_surfacing(self) -> bool: return self.depth_change < -1.0 def _water_seg_interpolate(seg: WaterSegment, t: float) -> WaterParams: """水中直线段内匀加速插值""" dt = t - seg.t_start ratio = dt / seg.duration if seg.duration > 1e-6 else 1.0 ratio = max(0.0, min(1.0, ratio)) dist = seg.v_start * dt + 0.5 * seg.acceleration * dt * dt dist = max(0.0, min(dist, seg.distance)) new_lat, new_lon = GeoUtils.offset_position(seg.start_lat, seg.start_lon, seg.bearing, dist) # new_lat, new_lon = _offset_position( # seg.start_lat, seg.start_lon, seg.bearing, dist # ) speed = max(seg.v_start + seg.acceleration * dt, 0.0) alt = seg.start_alt + (seg.end_alt - seg.start_alt) * ratio depth = seg.depth_start + (seg.depth_end - seg.depth_start) * ratio return WaterParams( timestamp = t, latitude = new_lat, longitude = new_lon, altitude = alt, heading = seg.bearing, speed = speed, acceleration = seg.acceleration, depth = depth, is_subsurface = seg.is_subsurface, domain = EquipmentDomain.WATER, ) # ───────────────────────────────────────────────────────────────── # 水中回转机动段 # ───────────────────────────────────────────────────────────────── @dataclass class WaterTurnSegment(BaseTransition): """水中回转机动段（舰艇大半径圆弧转向）""" turn_radius: float = 500.0 depth: float = 0.0 domain: EquipmentDomain = field(default=EquipmentDomain.WATER, repr=False) kind: SegmentKind = field(default=SegmentKind.MANEUVER, repr=False) _center_lat: float = field(default=0.0, init=False, repr=False) _center_lon: float = field(default=0.0, init=False, repr=False) _exit_lat: float = field(default=0.0, init=False, repr=False) _exit_lon: float = field(default=0.0, init=False, repr=False) def __post_init__(self): self._compute_geometry() def _compute_geometry(self): angle = self.turn_angle r = self.turn_radius if self.turn_direction >= 0: center_bearing = (self.entry_heading + 90) % 360 else: center_bearing = (self.entry_heading - 90) % 360 self._center_lat, self._center_lon = GeoUtils.offset_position( self.entry_lat, self.entry_lon, center_bearing, r ) exit_from_center = (center_bearing + 180 + angle) % 360 self._exit_lat, self._exit_lon = GeoUtils.offset_position( self._center_lat, self._center_lon, (exit_from_center + 180) % 360, r, ) arc_len = abs(math.radians(angle)) * r self.duration = arc_len / self.speed if self.speed > 1e-6 else 0.0 @property def exit_lat(self) -> float: return self._exit_lat @property def exit_lon(self) -> float: return self._exit_lon def query(self, t: float) -> WaterParams: dt = t - self.t_start ratio = dt / self.duration if self.duration > 1e-6 else 1.0 ratio = max(0.0, min(1.0, ratio)) swept_angle = self.turn_angle * ratio if self.turn_direction >= 0: center_bearing = (self.entry_heading + 90) % 360 else: center_bearing = (self.entry_heading - 90) % 360 entry_from_center = (center_bearing + 180) % 360 current_from_center = (entry_from_center + swept_angle) % 360 cur_lat, cur_lon = GeoUtils.offset_position( self._center_lat, self._center_lon, current_from_center, self.turn_radius, ) cur_heading = (self.entry_heading + swept_angle) % 360 turn_rate = (self.turn_angle / self.duration if self.duration > 1e-6 else 0.0) rudder_angle = min(35.0, abs(turn_rate) * 2) * self.turn_direction return WaterParams( timestamp = t, latitude = cur_lat, longitude = cur_lon, altitude = self.altitude, heading = cur_heading, speed = self.speed, depth = self.depth, rudder_angle = rudder_angle, domain = EquipmentDomain.WATER, ) def validate(self) -> list: errors = [] if self.turn_radius <= 0: errors.append(f"回转半径必须为正: {self.turn_radius}") if self.speed <= 0: errors.append(f"速度必须为正: {self.speed}") return errors # ───────────────────────────────────────────────────────────────── # 水中航线 # ───────────────────────────────────────────────────────────────── class WaterRoute(BaseRoute): """水中装备航线（水面舰艇 / 潜艇 / UUV）""" def __init__( self, start_time: float = 0.0, default_turn_radius: float = 500.0, min_turn_radius: float = 100.0, is_subsurface: bool = False, parent: QObject = None, ): super().__init__(start_time=start_time, parent=parent) self.default_turn_radius = default_turn_radius self.min_turn_radius = min_turn_radius self.is_subsurface = is_subsurface @property def domain(self) -> EquipmentDomain: return EquipmentDomain.WATER def _rebuild(self) -> None: self._segments.clear() self._transitions.clear() n = len(self._waypoints) if n < 2: self.total_duration = 0.0 return bearings: List[float] = [ # _bearing( # self._waypoints[i].latitude, self._waypoints[i].longitude, # self._waypoints[i+1].latitude, self._waypoints[i+1].longitude, # ) GeoUtils.bearing( self._waypoints[i].latitude, self._waypoints[i].longitude, self._waypoints[i+1].latitude, self._waypoints[i+1].longitude,) for i in range(n - 1) ] t_cursor = self.start_time prev_exit_lat = self._waypoints[0].latitude prev_exit_lon = self._waypoints[0].longitude for i in range(n - 1): wp_a = self._waypoints[i] wp_b = self._waypoints[i + 1] brg_in = bearings[i] brg_out = bearings[i + 1] if (i + 1) < len(bearings) else brg_in r = max( getattr(wp_b, "turn_radius", 0.0) or self.default_turn_radius, self.min_turn_radius, ) turn_angle = GeoUtils.heading_diff(brg_in, brg_out) need_turn = i < n - 2 and abs(turn_angle) > 0.5 if need_turn: tip_dist_m = r * abs(math.tan(math.radians(turn_angle / 2))) tip_lat, tip_lon = GeoUtils.offset_position( wp_b.latitude, wp_b.longitude, (brg_in + 180) % 360, tip_dist_m, ) seg_total = GeoUtils.haversine( prev_exit_lat, prev_exit_lon, wp_b.latitude, wp_b.longitude,) # seg_total = _haversine( # prev_exit_lat, prev_exit_lon, # wp_b.latitude, wp_b.longitude, # ) tip_check = GeoUtils.haversine( wp_b.latitude, wp_b.longitude, tip_lat, tip_lon, ) # tip_check = _haversine( # wp_b.latitude, wp_b.longitude, tip_lat, tip_lon, # ) if tip_check > seg_total * 0.95: need_turn = False tip_lat, tip_lon = wp_b.latitude, wp_b.longitude else: tip_lat, tip_lon = wp_b.latitude, wp_b.longitude seg_dist = GeoUtils.haversine( prev_exit_lat, prev_exit_lon, tip_lat, tip_lon ) # seg_dist = _haversine( # prev_exit_lat, prev_exit_lon, tip_lat, tip_lon # ) v0 = wp_a.speed v1 = wp_b.speed v_avg = (v0 + v1) / 2.0 or 1.0 duration = seg_dist / v_avg if v_avg > 1e-6 else 0.0 acc = (v1 - v0) / duration if duration > 1e-6 else 0.0 depth_a = getattr(wp_a, "depth", 0.0) depth_b = getattr(wp_b, "depth", 0.0) seg = self._make_segment( wp_from = wp_a, wp_to = wp_b, start_lat = prev_exit_lat, start_lon = prev_exit_lon, start_alt = wp_a.altitude, end_lat = tip_lat, end_lon = tip_lon, end_alt = wp_b.altitude, bearing = brg_in, t_start = t_cursor, v_start = v0, v_end = v1, acc = acc, dist = seg_dist, duration = duration, depth_start = depth_a, depth_end = depth_b, ) self._segments.append(seg) t_cursor += duration if need_turn: trans = self._make_transition( entry_lat = tip_lat, entry_lon = tip_lon, entry_alt = wp_b.altitude, entry_heading = brg_in, exit_heading = brg_out, speed = wp_b.speed, altitude = wp_b.altitude, t_start = t_cursor, turn_radius = r, depth = depth_b, ) self._transitions.append(trans) if trans is not None: t_cursor += trans.duration prev_exit_lat = trans.exit_lat prev_exit_lon = trans.exit_lon else: prev_exit_lat = tip_lat prev_exit_lon = tip_lon else: self._transitions.append(None) prev_exit_lat = tip_lat prev_exit_lon = tip_lon self._update_total_duration() def _make_segment( self, wp_from, wp_to, start_lat, start_lon, start_alt, end_lat, end_lon, end_alt, bearing, t_start, v_start=0.0, v_end=0.0, acc=0.0, dist=0.0, duration=0.0, depth_start=0.0, depth_end=0.0, **kwargs, ) -> WaterSegment: return WaterSegment( wp_from = wp_from, wp_to = wp_to, start_lat = start_lat, start_lon = start_lon, start_alt = start_alt, end_lat = end_lat, end_lon = end_lon, end_alt = end_alt, distance = dist, bearing = bearing, t_start = t_start, t_end = t_start + duration, duration = duration, v_start = v_start, v_end = v_end, acceleration = acc, depth_start = depth_start, depth_end = depth_end, is_subsurface = self.is_subsurface, ) def _make_transition( self, entry_lat, entry_lon, entry_alt, entry_heading, exit_heading, speed, altitude, t_start, turn_radius=500.0, depth=0.0, **kwargs, ) -> Optional[WaterTurnSegment]: return WaterTurnSegment( entry_lat = entry_lat, entry_lon = entry_lon, entry_alt = entry_alt, entry_heading = entry_heading, exit_heading = exit_heading, speed = speed, altitude = altitude, t_start = t_start, turn_radius = turn_radius, depth = depth, ) def _interpolate_segment(self, seg: BaseSegment, t: float) -> WaterParams: return _water_seg_interpolate(seg, t) # type: ignore[arg-type] def _state_at_start(self) -> WaterParams: seg = self._segments[0] return WaterParams( timestamp = self.t_start, latitude = seg.start_lat, longitude = seg.start_lon, altitude = seg.start_alt, heading = seg.bearing, speed = seg.v_start, depth = seg.depth_start, # type: ignore[attr-defined] is_subsurface = self.is_subsurface, domain = EquipmentDomain.WATER, ) def _state_at_end(self) -> WaterParams: last_trans = self._transitions[-1] if self._transitions else None if last_trans is not None: s = last_trans.query(last_trans.t_end) s.timestamp = self.t_end return s last_seg = self._segments[-1] return WaterParams( timestamp = self.t_end, latitude = last_seg.end_lat, longitude = last_seg.end_lon, altitude = last_seg.end_alt, heading = last_seg.bearing, speed = last_seg.v_end, depth = last_seg.depth_end, # type: ignore[attr-defined] is_subsurface = self.is_subsurface, domain = EquipmentDomain.WATER, )