Add fillet flight path geometry and Alt/Speed wheels

- RouteGeometry (lib/domain/mission/route_geometry.dart): physically grounded flight path - straight segments + tangential arcs at each course change, sized from the drone's minimum turn radius (doc 3.7/4.6), replacing a naive spline that would suggest unrealistic turn radii. Marks a vertex "bad" (turn angle >= 160°, tangent length exceeding 90% of either adjacent leg, or exceeding the waypoint's catch radius) and a leg "bad" when the required climb/descent rate exceeds the drone's max climb/descent rate. Pure Dart, no Flutter dependency, so it stays usable if the mission domain is ever split into its own package (4.22). Wind-based turn-radius correction from the prototype isn't ported yet - the wind system (doc 3.8) doesn't exist in the Flutter app. - MissionMap now draws each RouteSegment as its own Polyline, colored red when bad instead of a single plain white line; waypoint markers turn red too when their vertex is bad. - ValueWheel: vertical drag-to-adjust tape control (HTML prototype's #altWheel/#spdWheel), custom-painted tick marks, blue center indicator, gradient fade at top/bottom. Wired into PlanScreen on both screen edges with Alt/Speed readouts. - curAlt/curSpeed now live in PlanScreen state instead of fixed constants: dropping a new waypoint uses whatever the wheels are currently set to; entering editing mode on an existing waypoint loads its values into the wheels; adjusting a wheel while editing writes live into that waypoint (provider gained setAltitude/setSpeed to match the existing moveWaypoint/toggleAction pattern). Added dedicated unit tests for the geometry (empty list, straight line, feasible 90° turn producing a line-arc-line segment sequence, an infeasible near-180° turn, and an infeasible descent rate) rather than relying on eyeballing it on the emulator - this is exactly the kind of ported-math correctness that's hard to verify visually but easy to get subtly wrong. All 11 tests (previous 6 + these 5) and flutter analyze pass. Also manually verified the wheels on the Pixel_10a emulator: drag changes the value and the on-screen readout in real time. Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
2026-07-27 23:13:24 +02:00
parent 6dcb765b80
commit b93bbab23b
6 changed files with 617 additions and 20 deletions
@@ -0,0 +1,221 @@
+import 'dart:math' as math;
+
+import 'package:latlong2/latlong.dart';
+
+import '../waypoint/flat_waypoint_list.dart';
+
+/// Ein Streckenabschnitt der Flugpfad-Darstellung: entweder ein
+/// Geradenstueck zwischen zwei Tangentenpunkten oder ein abgetasteter
+/// Kreisbogen (Fillet) an einer Kursaenderung (Architektur-Doku 3.7/4.6).
+class RouteSegment {
+  const RouteSegment({required this.points, required this.bad});
+
+  final List<LatLng> points;
+  final bool bad;
+}
+
+class RouteGeometry {
+  const RouteGeometry({required this.segments, required this.vertexBad});
+
+  final List<RouteSegment> segments;
+
+  /// Pro Wegpunkt: true, wenn die Kursaenderung an diesem Punkt mit dem
+  /// minimalen Kurvenradius der Drohne nicht fliegbar ist (Tangentenlaenge
+  /// ueberschreitet Fangradius oder angrenzendes Geradenstueck, oder der
+  /// Kurswinkel ist zu scharf).
+  final List<bool> vertexBad;
+
+  static const empty = RouteGeometry(segments: [], vertexBad: []);
+}
+
+class _V {
+  const _V(this.x, this.y);
+  final double x;
+  final double y;
+
+  _V operator -(_V other) => _V(x - other.x, y - other.y);
+  _V operator +(_V other) => _V(x + other.x, y + other.y);
+  _V scaled(double f) => _V(x * f, y * f);
+  double get length => math.sqrt(x * x + y * y);
+  double dot(_V other) => x * other.x + y * other.y;
+  double cross(_V other) => x * other.y - y * other.x;
+}
+
+class _MetricPoint {
+  const _MetricPoint(this.pos, this.alt, this.speed);
+  final _V pos;
+  final double alt;
+  final double speed;
+}
+
+class _VertexArc {
+  const _VertexArc({
+    required this.t1,
+    required this.t2,
+    required this.center,
+    required this.radius,
+    required this.angleStart,
+    required this.angleEnd,
+  });
+
+  final _V t1;
+  final _V t2;
+  final _V center;
+  final double radius;
+  final double angleStart;
+  final double angleEnd;
+}
+
+const _maxTurnAngle = 160 * math.pi / 180;
+
+/// Baut den physikalisch fundierten Flugpfad: Geradenstuecke + tangentiale
+/// Kreisboegen an jeder Kursaenderung (Doku 3.7), abgeleitet aus dem
+/// minimalen Kurvenradius der Drohne. Ersetzt einen naiven Spline durch
+/// alle Punkte (Doku 4.6) - ein Spline wuerde unrealistische Kurvenradien
+/// suggerieren und ein falsches Sicherheitsgefuehl bei engen Kurven geben.
+///
+/// Windkorrektur des effektiven Kurvenradius (HTML-Demonstrator:
+/// vertexGroundSpeed-Anpassung) ist noch nicht portiert, da das Wind-System
+/// (Doku 3.8) in der Flutter-App noch nicht existiert.
+RouteGeometry buildRouteGeometry(
+  List<Waypoint> waypoints, {
+  required double minTurnRadius,
+  required double maxClimbRate,
+  required double maxDescentRate,
+}) {
+  final n = waypoints.length;
+  if (n == 0) return RouteGeometry.empty;
+
+  final ref = LatLng(waypoints.first.lat, waypoints.first.lon);
+  final pts = [for (final w in waypoints) _toMeters(w, ref)];
+
+  final vertexBad = List<bool>.filled(n, false);
+  final arcs = List<_VertexArc?>.filled(n, null);
+
+  for (var i = 1; i < n - 1; i++) {
+    final pPrev = pts[i - 1];
+    final pCur = pts[i];
+    final pNext = pts[i + 1];
+
+    final inVec = pCur.pos - pPrev.pos;
+    final outVec = pNext.pos - pCur.pos;
+    final inLen = inVec.length;
+    final outLen = outVec.length;
+    final vIn = inLen > 1e-6 ? inVec.scaled(1 / inLen) : const _V(1, 0);
+    final vOut = outLen > 1e-6 ? outVec.scaled(1 / outLen) : const _V(1, 0);
+
+    final dot = vIn.dot(vOut).clamp(-1.0, 1.0);
+    final turnAngle = math.acos(dot);
+    if (turnAngle <= 0.02) continue;
+
+    final cappedAngle = math.min(turnAngle, _maxTurnAngle);
+    final t = minTurnRadius * math.tan(cappedAngle / 2);
+
+    var bad = false;
+    if (turnAngle >= _maxTurnAngle) bad = true;
+    if (t > inLen * 0.9 || t > outLen * 0.9) bad = true;
+    if (t > waypoints[i].catchRadiusM) bad = true;
+
+    final turnLeft = vIn.cross(vOut) > 0;
+    final normalIn =
+        turnLeft ? _V(-vIn.y, vIn.x) : _V(vIn.y, -vIn.x);
+
+    final t1 = pCur.pos - vIn.scaled(t);
+    final t2 = pCur.pos + vOut.scaled(t);
+    final center = t1 + normalIn.scaled(minTurnRadius);
+    final angleStart = math.atan2(t1.y - center.y, t1.x - center.x);
+    final angleEnd = angleStart + (turnLeft ? cappedAngle : -cappedAngle);
+
+    vertexBad[i] = bad;
+    arcs[i] = _VertexArc(
+      t1: t1,
+      t2: t2,
+      center: center,
+      radius: minTurnRadius,
+      angleStart: angleStart,
+      angleEnd: angleEnd,
+    );
+  }
+
+  final legClimbBad = List<bool>.filled(math.max(n - 1, 0), false);
+  for (var i = 0; i < n - 1; i++) {
+    final a = pts[i];
+    final b = pts[i + 1];
+    final horiz = (b.pos - a.pos).length;
+    legClimbBad[i] = _isClimbBad(
+      a.alt,
+      b.alt,
+      horiz,
+      b.speed,
+      maxClimbRate: maxClimbRate,
+      maxDescentRate: maxDescentRate,
+    );
+  }
+
+  final segments = <RouteSegment>[];
+  for (var leg = 0; leg < n - 1; leg++) {
+    final startArc = arcs[leg];
+    final endArc = arcs[leg + 1];
+    final startPt = (leg == 0) ? pts[0].pos : (startArc?.t2 ?? pts[leg].pos);
+    final endPt =
+        (leg == n - 2) ? pts[n - 1].pos : (endArc?.t1 ?? pts[leg + 1].pos);
+
+    segments.add(RouteSegment(
+      points: [_fromMeters(startPt, ref), _fromMeters(endPt, ref)],
+      bad: legClimbBad[leg],
+    ));
+
+    if (leg + 1 <= n - 2 && endArc != null) {
+      const steps = 14;
+      final arcPoints = [
+        for (var k = 0; k <= steps; k++)
+          _fromMeters(
+            _pointOnCircle(
+              endArc.center,
+              endArc.radius,
+              endArc.angleStart +
+                  (endArc.angleEnd - endArc.angleStart) * k / steps,
+            ),
+            ref,
+          ),
+      ];
+      segments.add(RouteSegment(points: arcPoints, bad: vertexBad[leg + 1]));
+    }
+  }
+
+  return RouteGeometry(segments: segments, vertexBad: vertexBad);
+}
+
+_V _pointOnCircle(_V center, double radius, double angle) =>
+    _V(center.x + radius * math.cos(angle), center.y + radius * math.sin(angle));
+
+bool _isClimbBad(
+  double altA,
+  double altB,
+  double horizDist,
+  double speed, {
+  required double maxClimbRate,
+  required double maxDescentRate,
+}) {
+  final vert = altB - altA;
+  if (horizDist < 0.001) return vert.abs() > 0.001;
+  final rate = vert / (horizDist / speed);
+  if (rate > 0) return rate > maxClimbRate;
+  return rate.abs() > maxDescentRate;
+}
+
+// Equirektangulare Naeherung relativ zum ersten Wegpunkt - ausreichend fuer
+// missionstypische Distanzen (HTML-Demonstrator: toMeters()/fromMeters()).
+_MetricPoint _toMeters(Waypoint w, LatLng ref) {
+  final dLat = (w.lat - ref.latitude) * 110540;
+  final dLon =
+      (w.lon - ref.longitude) * 111320 * math.cos(ref.latitude * math.pi / 180);
+  return _MetricPoint(_V(dLon, dLat), w.altitudeM, w.speedMs);
+}
+
+LatLng _fromMeters(_V m, LatLng ref) {
+  return LatLng(
+    ref.latitude + m.y / 110540,
+    ref.longitude + m.x / (111320 * math.cos(ref.latitude * math.pi / 180)),
+  );
+}