Use navmeshes to improve TARCAP flight plans.

Started with TARCAP because they're simple, but will follow and extend
this to the other flight plans next.

This works by building navigation meshes (navmeshes) of the theater
based on the threat regions. A navmesh is created for each faction to
allow the unique pathing around each side's threats. Navmeshes are built
such that there are nav edges around threat zones to allow the planner
to pick waypoints that (slightly) route around threats before
approaching the target.

Using the navmesh, routes are found using A*. Performance appears
adequate, and could probably be improved with a cache if needed since
the small number of origin points means many flights will share portions
of their flight paths.

This adds a few visual debugging tools to the map. They're disabled by
default, but changing the local `debug` variable in `DisplayOptions` to
`True` will make them appear in the display options menu. These are:

* Display navmeshes (red and blue). Displaying either navmesh will draw
  each navmesh polygon on the map view and highlight the mesh that
  contains the cursor. Neighbors are indicated by a small yellow line
  pointing from the center of the polygon's edge/vertext that is shared
  with its neighbor toward the centroid of the zone.
* Shortest path from control point to mouse location. The first control
  point for the selected faction is arbitrarily selected, and the
  shortest path from that control point to the mouse cursor will be
  drawn on the map.
* TARCAP plan near mouse location. A TARCAP will be planned from the
  faction's first control point to the target nearest the mouse cursor.

https://github.com/Khopa/dcs_liberation/issues/292

changelog.md

@@ -5,6 +5,7 @@ Saves from 2.3 are not compatible with 2.4.
 ## Features/Improvements
 
 * **[Flight Planner]** Air-to-air and SEAD escorts will no longer be automatically planned for packages that are not in range of threats.
+* **[Flight Planner]** TARCAP flights will now navigate around threat areas en route to the target area when practical. More types coming soon.
 
 # 2.3.3
 

game/game.py

@@ -28,6 +28,7 @@ from .event.event import Event, UnitsDeliveryEvent
 from .event.frontlineattack import FrontlineAttackEvent
 from .factions.faction import Faction
 from .infos.information import Information
+from .navmesh import NavMesh
 from .procurement import ProcurementAi
 from .settings import Settings
 from .theater import ConflictTheater, ControlPoint
@@ -114,9 +115,6 @@ class Game:
 
         self.sanitize_sides()
 
-        self.blue_threat_zone: ThreatZones
-        self.red_threat_zone: ThreatZones
-
         self.on_load()
 
         # Turn 0 procurement. We don't actually have any missions to plan, but
@@ -136,6 +134,8 @@ class Game:
         # recomputed on load for the sake of save compatibility.
         del state["blue_threat_zone"]
         del state["red_threat_zone"]
+        del state["blue_navmesh"]
+        del state["red_navmesh"]
         return state
 
     def __setstate__(self, state: Dict[str, Any]) -> None:
@@ -380,12 +380,21 @@ class Game:
     def compute_threat_zones(self) -> None:
         self.blue_threat_zone = ThreatZones.for_faction(self, player=True)
         self.red_threat_zone = ThreatZones.for_faction(self, player=False)
+        self.blue_navmesh = NavMesh.from_threat_zones(self.red_threat_zone,
+                                                      self.theater)
+        self.red_navmesh = NavMesh.from_threat_zones(self.blue_threat_zone,
+                                                     self.theater)
 
     def threat_zone_for(self, player: bool) -> ThreatZones:
         if player:
             return self.blue_threat_zone
         return self.red_threat_zone
 
+    def navmesh_for(self, player: bool) -> NavMesh:
+        if player:
+            return self.blue_navmesh
+        return self.red_navmesh
+
     def compute_conflicts_position(self):
         """
         Compute the current conflict center position(s), mainly used for culling calculation

game/navmesh.py

@@ -0,0 +1,270 @@
+from __future__ import annotations
+
+import heapq
+import math
+from collections import defaultdict
+from dataclasses import dataclass, field
+from typing import Dict, List, Optional, Set, Tuple, Union
+
+from dcs.mapping import Point
+from shapely.geometry import (
+    LineString,
+    MultiPolygon,
+    Point as ShapelyPoint,
+    Polygon,
+    box,
+)
+from shapely.ops import nearest_points, triangulate
+
+from game.theater import ConflictTheater
+from game.threatzones import ThreatZones
+from game.utils import nautical_miles
+
+
+class NavMeshPoly:
+    def __init__(self, ident: int, poly: Polygon, threatened: bool) -> None:
+        self.ident = ident
+        self.poly = poly
+        self.threatened = threatened
+        self.neighbors: Dict[NavMeshPoly, Union[LineString, ShapelyPoint]] = {}
+
+    def __eq__(self, other: object) -> bool:
+        if not isinstance(other, NavMeshPoly):
+            return False
+        return self.ident == other.ident
+
+    def __hash__(self) -> int:
+        return self.ident
+
+
+@dataclass(frozen=True)
+class NavPoint:
+    point: ShapelyPoint
+    poly: NavMeshPoly
+
+    @property
+    def world_point(self) -> Point:
+        return Point(self.point.x, self.point.y)
+
+    def __hash__(self) -> int:
+        return hash((self.poly.ident, int(self.point.x), int(self.point.y)))
+
+    def __eq__(self, other: object) -> bool:
+        if not isinstance(other, NavPoint):
+            return False
+
+        # The int comparisons here aren't really correct, but the units here are
+        # meters and even approximate floating point comparisons can cause
+        # issues when these are used in sets or maps. For our purposes, two
+        # waypoints within a meter of each other might as well be identical for
+        # the purposes of path finding, so not an issue.
+        if int(self.point.x) != int(other.point.x):
+            return False
+        if int(self.point.y) != int(other.point.y):
+            return False
+        return self.poly == other.poly
+
+    def __str__(self) -> str:
+        return f"{self.point} in {self.poly.ident}"
+
+
+@dataclass(frozen=True, order=True)
+class FrontierNode:
+    cost: float
+    point: NavPoint = field(compare=False)
+
+
+class NavFrontier:
+    def __init__(self) -> None:
+        self.nodes: List[FrontierNode] = []
+
+    def push(self, poly: NavPoint, cost: float) -> None:
+        heapq.heappush(self.nodes, FrontierNode(cost, poly))
+
+    def pop(self) -> Optional[NavPoint]:
+        try:
+            return heapq.heappop(self.nodes).point
+        except IndexError:
+            return None
+
+
+class NavMesh:
+    def __init__(self, polys: List[NavMeshPoly]) -> None:
+        self.polys = polys
+
+    def localize(self, point: Point) -> Optional[NavMeshPoly]:
+        # This is a naive implementation but it's O(n). Runs at about 10k
+        # lookups a second on a 5950X. Flights usually have 5-10 waypoints, so
+        # that's 1k-2k flights before we lose a full second to localization as a
+        # part of flight plan creation.
+        #
+        # Can improve the algorithm later if needed, but that seems unnecessary
+        # currently.
+        p = ShapelyPoint(point.x, point.y)
+        for navpoly in self.polys:
+            if navpoly.poly.contains(p):
+                return navpoly
+        return None
+
+    @staticmethod
+    def travel_cost(a: NavPoint, b: NavPoint) -> float:
+        modifier = 1.0
+        if a.poly.threatened:
+            modifier = 3.0
+        return a.point.distance(b.point) * modifier
+
+    def travel_heuristic(self, a: NavPoint, b: NavPoint) -> float:
+        return self.travel_cost(a, b)
+
+    @staticmethod
+    def reconstruct_path(came_from: Dict[NavPoint, Optional[NavPoint]],
+                         origin: NavPoint,
+                         destination: NavPoint) -> List[Point]:
+        current = destination
+        path: List[Point] = []
+        while current != origin:
+            path.append(current.world_point)
+            previous = came_from[current]
+            if previous is None:
+                raise RuntimeError(
+                    f"Could not reconstruct path to {destination} from {origin}"
+                )
+            current = previous
+        path.append(origin.world_point)
+        path.reverse()
+        return path
+
+    @staticmethod
+    def dcs_to_shapely_point(point: Point) -> ShapelyPoint:
+        return ShapelyPoint(point.x, point.y)
+
+    def shortest_path(self, origin: Point, destination: Point) -> List[Point]:
+        origin_poly = self.localize(origin)
+        if origin_poly is None:
+            raise ValueError(f"Origin point {origin} is outside the navmesh")
+        destination_poly = self.localize(destination)
+        if destination_poly is None:
+            raise ValueError(
+                f"Origin point {destination} is outside the navmesh")
+
+        return self._shortest_path(
+            NavPoint(self.dcs_to_shapely_point(origin), origin_poly),
+            NavPoint(self.dcs_to_shapely_point(destination), destination_poly)
+        )
+
+    def _shortest_path(self, origin: NavPoint,
+                       destination: NavPoint) -> List[Point]:
+        # Adapted from
+        # https://www.redblobgames.com/pathfinding/a-star/implementation.py.
+        frontier = NavFrontier()
+        frontier.push(origin, 0.0)
+        came_from: Dict[NavPoint, Optional[NavPoint]] = {origin: None}
+
+        best_known: Dict[NavPoint, float] = defaultdict(lambda: math.inf)
+        best_known[origin] = 0.0
+
+        while (current := frontier.pop()) is not None:
+            if current == destination:
+                break
+
+            if current.poly == destination.poly:
+                # Made it to the correct nav poly. Add the leg from the border
+                # to the target.
+                cost = best_known[current] + self.travel_cost(
+                    current, destination
+                )
+                if cost < best_known[destination]:
+                    best_known[destination] = cost
+                    estimated = cost
+                    frontier.push(destination, estimated)
+                    came_from[destination] = current
+
+            for neighbor, boundary in current.poly.neighbors.items():
+                previous = came_from[current]
+                if previous is not None and previous.poly == neighbor:
+                    # Don't backtrack.
+                    continue
+                if previous is None and current != origin:
+                    raise RuntimeError
+                _, neighbor_point = nearest_points(current.point, boundary)
+                neighbor_nav = NavPoint(neighbor_point, neighbor)
+                cost = best_known[current] + self.travel_cost(
+                    current, neighbor_nav
+                )
+                if cost < best_known[neighbor_nav]:
+                    best_known[neighbor_nav] = cost
+                    estimated = cost + self.travel_heuristic(
+                        neighbor_nav, destination
+                    )
+                    frontier.push(neighbor_nav, estimated)
+                    came_from[neighbor_nav] = current
+
+        return self.reconstruct_path(came_from, origin, destination)
+
+    @staticmethod
+    def map_bounds(theater: ConflictTheater) -> Polygon:
+        points = []
+        for cp in theater.controlpoints:
+            points.append(ShapelyPoint(cp.position.x, cp.position.y))
+            for tgo in cp.ground_objects:
+                points.append(ShapelyPoint(tgo.position.x, tgo.position.y))
+        return box(*LineString(points).bounds).buffer(nautical_miles(60).meters,
+                                                      resolution=1)
+
+    @staticmethod
+    def create_navpolys(polys: List[Polygon],
+                        threat_zones: ThreatZones) -> List[NavMeshPoly]:
+        return [NavMeshPoly(i, p, threat_zones.threatened(p))
+                for i, p in enumerate(polys)]
+
+    @staticmethod
+    def associate_neighbors(polys: List[NavMeshPoly]) -> None:
+        # Maps (rounded) points to polygons that have a vertex at that point.
+        # The points are rounded to the nearest int so we can use them as dict
+        # keys. This allows us to perform approximate neighbor lookups more
+        # efficiently than comparing each poly to every other poly by finding
+        # approximate neighbors before checking if the polys actually touch.
+        points_map: Dict[Tuple[int, int], Set[NavMeshPoly]] = defaultdict(set)
+
+        for navpoly in polys:
+            # The coordinates of the polygon's boundary are a sequence of
+            # coordinates that define the polygon. The first point is repeated
+            # at the end, so skip the last vertex.
+            for x, y in navpoly.poly.boundary.coords[:-1]:
+                point = (int(x), int(y))
+                neighbors = {}
+                for potential_neighbor in points_map[point]:
+                    intersection = navpoly.poly.intersection(
+                        potential_neighbor.poly)
+                    if not intersection.is_empty:
+                        potential_neighbor.neighbors[navpoly] = intersection
+                        neighbors[potential_neighbor] = intersection
+                navpoly.neighbors.update(neighbors)
+                points_map[point].add(navpoly)
+
+    @classmethod
+    def from_threat_zones(cls, threat_zones: ThreatZones,
+                          theater: ConflictTheater) -> NavMesh:
+        # Simplify the threat poly to reduce the number of nav zones. Increase
+        # the size of the zone and then simplify it with the buffer size as the
+        # error margin. This will create a simpler poly around the threat zone.
+        buffer = nautical_miles(10).meters
+        threat_poly = threat_zones.all.buffer(buffer).simplify(buffer)
+
+        # Threat zones can be disconnected. Create a list of threat zones.
+        if isinstance(threat_poly, MultiPolygon):
+            polys = list(threat_poly.geoms)
+        else:
+            polys = [threat_poly]
+
+        # Subtract the threat zones from the whole-map poly to build a navmesh
+        # for the *safe* areas. Navigation within threatened regions is always
+        # a straight line to the target or out of the threatened region.
+        bounds = cls.map_bounds(theater)
+        for poly in polys:
+            bounds = bounds.difference(poly)
+
+        # Triangulate the safe-region to build the navmesh.
+        navpolys = cls.create_navpolys(triangulate(bounds), threat_zones)
+        cls.associate_neighbors(navpolys)
+        return cls(navpolys)

game/theater/conflicttheater.py

@@ -524,6 +524,21 @@ class ConflictTheater:
                 closest_distance = distance
         return closest
 
+    def closest_target(self, point: Point) -> MissionTarget:
+        closest: MissionTarget = self.controlpoints[0]
+        closest_distance = point.distance_to_point(closest.position)
+        for control_point in self.controlpoints[1:]:
+            distance = point.distance_to_point(control_point.position)
+            if distance < closest_distance:
+                closest = control_point
+                closest_distance = distance
+            for tgo in control_point.ground_objects:
+                distance = point.distance_to_point(tgo.position)
+                if distance < closest_distance:
+                    closest = tgo
+                    closest_distance = distance
+        return closest
+    
     def closest_opposing_control_points(self) -> Tuple[ControlPoint, ControlPoint]:
         """
         Returns a tuple of the two nearest opposing ControlPoints in theater.

game/threatzones.py

@@ -29,6 +29,13 @@ class ThreatZones:
         self.air_defenses = air_defenses
         self.all = unary_union([airbases, air_defenses])
 
+    def threatened(self, position: BaseGeometry) -> bool:
+        return self.all.intersects(position)
+
+    def path_threatened(self, a: DcsPoint, b: DcsPoint) -> bool:
+        return self.threatened(LineString(
+            [self.dcs_to_shapely_point(a), self.dcs_to_shapely_point(b)]))
+
     @singledispatchmethod
     def threatened_by_aircraft(self, target) -> bool:
         raise NotImplementedError

gen/flights/flightplan.py

@@ -450,17 +450,21 @@ class CasFlightPlan(PatrollingFlightPlan):
 @dataclass(frozen=True)
 class TarCapFlightPlan(PatrollingFlightPlan):
     takeoff: FlightWaypoint
+    nav_to: List[FlightWaypoint]
+    nav_from: List[FlightWaypoint]
     land: FlightWaypoint
     divert: Optional[FlightWaypoint]
     lead_time: timedelta
 
     def iter_waypoints(self) -> Iterator[FlightWaypoint]:
+        yield self.takeoff
+        yield from self.nav_to
         yield from [
-            self.takeoff,
             self.patrol_start,
             self.patrol_end,
-            self.land,
         ]
+        yield from self.nav_from
+        yield self.land
         if self.divert is not None:
             yield self.divert
 
@@ -876,7 +880,7 @@ class FlightPlanBuilder:
             int(self.doctrine.max_patrol_altitude.meters)
         ))
 
-        builder = WaypointBuilder(self.game.conditions, flight, self.doctrine)
+        builder = WaypointBuilder(flight, self.game, self.is_player)
         start, end = builder.race_track(start, end, patrol_alt)
 
         return BarCapFlightPlan(
@@ -903,7 +907,7 @@ class FlightPlanBuilder:
         start = target.point_from_heading(heading,
                                           -self.doctrine.sweep_distance.meters)
 
-        builder = WaypointBuilder(self.game.conditions, flight, self.doctrine)
+        builder = WaypointBuilder(flight, self.game, self.is_player)
         start, end = builder.sweep(start, target,
                                    self.doctrine.ingress_altitude)
 
@@ -1000,7 +1004,7 @@ class FlightPlanBuilder:
                            int(self.doctrine.max_patrol_altitude.meters)))
 
         # Create points
-        builder = WaypointBuilder(self.game.conditions, flight, self.doctrine)
+        builder = WaypointBuilder(flight, self.game, self.is_player)
 
         if isinstance(location, FrontLine):
             orbit0p, orbit1p = self.racetrack_for_frontline(location)
@@ -1019,6 +1023,10 @@ class FlightPlanBuilder:
             patrol_duration=self.doctrine.cap_duration,
             engagement_distance=self.doctrine.cap_engagement_range,
             takeoff=builder.takeoff(flight.departure),
+            nav_to=builder.nav_path(flight.departure.position, orbit0p,
+                                    patrol_alt),
+            nav_from=builder.nav_path(orbit1p, flight.arrival.position,
+                                      patrol_alt),
             patrol_start=start,
             patrol_end=end,
             land=builder.land(flight.arrival),
@@ -1113,7 +1121,7 @@ class FlightPlanBuilder:
     def generate_escort(self, flight: Flight) -> StrikeFlightPlan:
         assert self.package.waypoints is not None
 
-        builder = WaypointBuilder(self.game.conditions, flight, self.doctrine)
+        builder = WaypointBuilder(flight, self.game, self.is_player)
         ingress, target, egress = builder.escort(
             self.package.waypoints.ingress, self.package.target,
             self.package.waypoints.egress)
@@ -1151,7 +1159,7 @@ class FlightPlanBuilder:
         center = ingress.point_from_heading(heading, distance / 2)
         egress = ingress.point_from_heading(heading, distance)
 
-        builder = WaypointBuilder(self.game.conditions, flight, self.doctrine)
+        builder = WaypointBuilder(flight, self.game, self.is_player)
 
         return CasFlightPlan(
             package=self.package,
@@ -1247,7 +1255,7 @@ class FlightPlanBuilder:
             flight: The flight to generate the landing waypoint for.
             arrival: Arrival airfield or carrier.
         """
-        builder = WaypointBuilder(self.game.conditions, flight, self.doctrine)
+        builder = WaypointBuilder(flight, self.game, self.is_player)
         return builder.land(arrival)
 
     def strike_flightplan(
@@ -1255,8 +1263,7 @@ class FlightPlanBuilder:
             ingress_type: FlightWaypointType,
             targets: Optional[List[StrikeTarget]] = None) -> StrikeFlightPlan:
         assert self.package.waypoints is not None
-        builder = WaypointBuilder(self.game.conditions, flight, self.doctrine,
-                                  targets)
+        builder = WaypointBuilder(flight, self.game, self.is_player, targets)
 
         target_waypoints: List[FlightWaypoint] = []
         if targets is not None:

gen/flights/waypointbuilder.py

@@ -1,21 +1,31 @@
 from __future__ import annotations
 
+import random
 from dataclasses import dataclass
-from typing import List, Optional, Tuple, Union
+from typing import (
+    Iterable,
+    Iterator,
+    List,
+    Optional,
+    TYPE_CHECKING,
+    Tuple,
+    Union,
+)
 
 from dcs.mapping import Point
 from dcs.unit import Unit
 from dcs.unitgroup import VehicleGroup
 
-from game.data.doctrine import Doctrine
+if TYPE_CHECKING:
+    from game import Game
+
 from game.theater import (
     ControlPoint,
     MissionTarget,
     OffMapSpawn,
     TheaterGroundObject,
 )
-from game.utils import Distance, meters
-from game.weather import Conditions
+from game.utils import Distance, meters, nautical_miles
 from .flight import Flight, FlightWaypoint, FlightWaypointType
 
 
@@ -26,12 +36,13 @@ class StrikeTarget:
 
 
 class WaypointBuilder:
-    def __init__(self, conditions: Conditions, flight: Flight,
-                 doctrine: Doctrine,
+    def __init__(self, flight: Flight, game: Game, player: bool,
                  targets: Optional[List[StrikeTarget]] = None) -> None:
-        self.conditions = conditions
         self.flight = flight
-        self.doctrine = doctrine
+        self.conditions = game.conditions
+        self.doctrine = game.faction_for(player).doctrine
+        self.threat_zones = game.threat_zone_for(not player)
+        self.navmesh = game.navmesh_for(player)
         self.targets = targets
 
     @property
@@ -429,3 +440,80 @@ class WaypointBuilder:
 
         egress = self.egress(egress, target)
         return ingress, waypoint, egress
+
+    @staticmethod
+    def nav(position: Point, altitude: Distance) -> FlightWaypoint:
+        """Creates a navigation point.
+
+        Args:
+            position: Position of the waypoint.
+            altitude: Altitude of the waypoint.
+        """
+        waypoint = FlightWaypoint(
+            FlightWaypointType.NAV,
+            position.x,
+            position.y,
+            altitude
+        )
+        waypoint.name = "NAV"
+        waypoint.description = ""
+        waypoint.pretty_name = ""
+        return waypoint
+
+    def nav_path(self, a: Point, b: Point,
+                 altitude: Distance) -> List[FlightWaypoint]:
+        path = self.clean_nav_points(self.navmesh.shortest_path(a, b))
+        return [self.nav(self.perturb(p), altitude) for p in path]
+
+    def clean_nav_points(self, points: Iterable[Point]) -> Iterator[Point]:
+        # Examine a sliding window of three waypoints. `current` is the waypoint
+        # being checked for prunability. `previous` is the last emitted waypoint
+        # before `current`. `nxt` is the waypoint after `current`.
+        previous: Optional[Point] = None
+        current: Optional[Point] = None
+        for nxt in points:
+            if current is None:
+                current = nxt
+                continue
+            if previous is None:
+                previous = current
+                current = nxt
+                continue
+
+            if self.nav_point_prunable(previous, current, nxt):
+                current = nxt
+                continue
+
+            yield current
+            previous = current
+            current = nxt
+
+    def nav_point_prunable(self, previous: Point, current: Point,
+                           nxt: Point) -> bool:
+        previous_threatened = self.threat_zones.path_threatened(previous,
+                                                                current)
+        next_threatened = self.threat_zones.path_threatened(current, nxt)
+        pruned_threatened = self.threat_zones.path_threatened(previous, nxt)
+        previous_distance = meters(previous.distance_to_point(current))
+        distance = meters(current.distance_to_point(nxt))
+        distance_without = previous_distance + distance
+        if distance > distance_without:
+            # Don't prune paths to make them longer.
+            return False
+
+        # We could shorten the path by removing the intermediate
+        # waypoint. Do so if the new path isn't higher threat.
+        if not pruned_threatened:
+            # The new path is not threatened, so safe to prune.
+            return True
+
+        # The new path is threatened. Only allow if both paths were
+        # threatened anyway.
+        return previous_threatened and next_threatened
+
+    @staticmethod
+    def perturb(point: Point) -> Point:
+        deviation = nautical_miles(1)
+        x_adj = random.randint(int(-deviation.meters), int(deviation.meters))
+        y_adj = random.randint(int(-deviation.meters), int(deviation.meters))
+        return Point(point.x + x_adj, point.y + y_adj)

qt_ui/displayoptions.py

@@ -1,5 +1,5 @@
 """Visibility options for the game map."""
-from dataclasses import dataclass
+from dataclasses import dataclass, field
 from typing import Iterator, Optional, Union
 
 
@@ -7,6 +7,7 @@ from typing import Iterator, Optional, Union
 class DisplayRule:
     name: str
     _value: bool
+    debug_only: bool = field(default=False)
 
     @property
     def menu_text(self) -> str:
@@ -29,8 +30,9 @@ class DisplayRule:
 
 
 class DisplayGroup:
-    def __init__(self, name: Optional[str]) -> None:
+    def __init__(self, name: Optional[str], debug_only: bool = False) -> None:
         self.name = name
+        self.debug_only = debug_only
 
     def __iter__(self) -> Iterator[DisplayRule]:
         # Python 3.6 enforces that __dict__ is order preserving by default.
@@ -60,6 +62,23 @@ class ThreatZoneOptions(DisplayGroup):
             f"Show {coalition_name.lower()} air defenses threat zones", False)
 
 
+class NavMeshOptions(DisplayGroup):
+    def __init__(self) -> None:
+        super().__init__("Navmeshes", debug_only=True)
+        self.hide = DisplayRule("DEBUG Hide Navmeshes", True)
+        self.blue_navmesh = DisplayRule("DEBUG Show blue navmesh", False)
+        self.red_navmesh = DisplayRule("DEBUG Show red navmesh", False)
+
+
+class PathDebugOptions(DisplayGroup):
+    def __init__(self) -> None:
+        super().__init__("Shortest paths", debug_only=True)
+        self.hide = DisplayRule("DEBUG Hide paths", True)
+        self.shortest_path = DisplayRule("DEBUG Show shortest path", False)
+        self.blue_tarcap = DisplayRule("DEBUG Show blue TARCAP plan", False)
+        self.red_tarcap = DisplayRule("DEBUG Show red TARCAP plan", False)
+
+
 class DisplayOptions:
     ground_objects = DisplayRule("Ground Objects", True)
     control_points = DisplayRule("Control Points", True)
@@ -71,15 +90,23 @@ class DisplayOptions:
     waypoint_info = DisplayRule("Waypoint Information", True)
     culling = DisplayRule("Display Culling Zones", False)
     flight_paths = FlightPathOptions()
-    actual_frontline_pos = DisplayRule("Display Actual Frontline Location", False)
+    actual_frontline_pos = DisplayRule("Display Actual Frontline Location",
+                                       False)
     blue_threat_zones = ThreatZoneOptions("Blue")
     red_threat_zones = ThreatZoneOptions("Red")
+    navmeshes = NavMeshOptions()
+    path_debug = PathDebugOptions()
 
     @classmethod
     def menu_items(cls) -> Iterator[Union[DisplayGroup, DisplayRule]]:
+        debug = False  # Set to True to enable debug options.
         # Python 3.6 enforces that __dict__ is order preserving by default.
         for value in cls.__dict__.values():
             if isinstance(value, DisplayRule):
+                if value.debug_only and not debug:
+                    continue
                 yield value
             elif isinstance(value, DisplayGroup):
+                if value.debug_only and not debug:
+                    continue
                 yield value

qt_ui/widgets/map/QLiberationMap.py

@@ -3,6 +3,7 @@ from __future__ import annotations
 import datetime
 import logging
 import math
+from functools import singledispatchmethod
 from typing import Iterable, Iterator, List, Optional, Tuple
 
 from PySide2 import QtCore, QtWidgets
@@ -25,14 +26,18 @@ from PySide2.QtWidgets import (
     QGraphicsView,
 )
 from dcs import Point
+from dcs.planes import F_16C_50
 from dcs.mapping import point_from_heading
 from shapely.geometry import (
+    LineString,
     MultiPolygon,
+    Point as ShapelyPoint,
     Polygon,
 )
 
 import qt_ui.uiconstants as CONST
 from game import Game, db
+from game.navmesh import NavMesh
 from game.theater import ControlPoint, Enum
 from game.theater.conflicttheater import FrontLine, ReferencePoint
 from game.theater.theatergroundobject import (
@@ -40,9 +45,14 @@ from game.theater.theatergroundobject import (
 )
 from game.utils import Distance, meters, nautical_miles
 from game.weather import TimeOfDay
-from gen import Conflict
-from gen.flights.flight import Flight, FlightWaypoint, FlightWaypointType
-from gen.flights.flightplan import FlightPlan
+from gen import Conflict, Package
+from gen.flights.flight import (
+    Flight,
+    FlightType,
+    FlightWaypoint,
+    FlightWaypointType,
+)
+from gen.flights.flightplan import FlightPlan, FlightPlanBuilder
 from qt_ui.displayoptions import DisplayOptions, ThreatZoneOptions
 from qt_ui.models import GameModel
 from qt_ui.widgets.map.QFrontLine import QFrontLine
@@ -163,6 +173,9 @@ class QLiberationMap(QGraphicsView):
 
         self.nm_to_pixel_ratio: int = 0
 
+        self.navmesh_highlight: Optional[QPolygonF] = None
+        self.shortest_path_segments: List[QLineF] = []
+
 
     def init_scene(self):
         scene = QLiberationScene(self)
@@ -280,14 +293,14 @@ class QLiberationMap(QGraphicsView):
             scene.addEllipse(transformed[0]-radius, transformed[1]-radius, 2*radius, 2*radius, CONST.COLORS["transparent"], CONST.COLORS["light_green_transparent"])
 
     def draw_shapely_poly(self, scene: QGraphicsScene, poly: Polygon, pen: QPen,
-                          brush: QBrush) -> None:
+                          brush: QBrush) -> Optional[QPolygonF]:
         if poly.is_empty:
-            return
+            return None
         points = []
         for x, y in poly.exterior.coords:
             x, y = self._transform_point(Point(x, y))
             points.append(QPointF(x, y))
-        scene.addPolygon(QPolygonF(points), pen, brush)
+        return scene.addPolygon(QPolygonF(points), pen, brush)
 
     def draw_threat_zone(self, scene: QGraphicsScene, poly: Polygon,
                          player: bool) -> None:
@@ -317,6 +330,133 @@ class QLiberationMap(QGraphicsView):
         for poly in polys:
             self.draw_threat_zone(scene, poly, player)
 
+    def draw_navmesh_neighbor_line(self, scene: QGraphicsScene, poly: Polygon,
+                                   begin: ShapelyPoint) -> None:
+        vertex = Point(begin.x, begin.y)
+        centroid = poly.centroid
+        direction = Point(centroid.x, centroid.y)
+        end = vertex.point_from_heading(vertex.heading_between_point(direction),
+                                        nautical_miles(2).meters)
+
+        scene.addLine(QLineF(QPointF(*self._transform_point(vertex)),
+                             QPointF(*self._transform_point(end))),
+                      CONST.COLORS["yellow"])
+
+    @singledispatchmethod
+    def draw_navmesh_border(self, intersection, scene: QGraphicsScene,
+                            poly: Polygon) -> None:
+        raise NotImplementedError("draw_navmesh_border not implemented for %s",
+                                  intersection.__class__.__name__)
+
+    @draw_navmesh_border.register
+    def draw_navmesh_point_border(self, intersection: ShapelyPoint,
+                                  scene: QGraphicsScene, poly: Polygon) -> None:
+        # Draw a line from the vertex toward the center of the polygon.
+        self.draw_navmesh_neighbor_line(scene, poly, intersection)
+
+    @draw_navmesh_border.register
+    def draw_navmesh_edge_border(self, intersection: LineString,
+                                 scene: QGraphicsScene, poly: Polygon) -> None:
+        # Draw a line from the center of the edge toward the center of the
+        # polygon.
+        edge_center = intersection.interpolate(0.5, normalized=True)
+        self.draw_navmesh_neighbor_line(scene, poly, edge_center)
+
+    def display_navmesh(self, scene: QGraphicsScene, player: bool) -> None:
+        for navpoly in self.game.navmesh_for(player).polys:
+            self.draw_shapely_poly(scene, navpoly.poly, CONST.COLORS["black"],
+                                   CONST.COLORS["transparent"])
+
+            position = self._transform_point(
+                Point(navpoly.poly.centroid.x, navpoly.poly.centroid.y))
+            text = scene.addSimpleText(f"Navmesh {navpoly.ident}",
+                                       self.waypoint_info_font)
+            text.setBrush(QColor(255, 255, 255))
+            text.setPen(QColor(255, 255, 255))
+            text.moveBy(position[0] + 8, position[1])
+            text.setZValue(2)
+
+            for border in navpoly.neighbors.values():
+                self.draw_navmesh_border(border, scene, navpoly.poly)
+
+    def highlight_mouse_navmesh(self, scene: QGraphicsScene, navmesh: NavMesh,
+                                mouse_position: Point) -> None:
+        if self.navmesh_highlight is not None:
+            try:
+                scene.removeItem(self.navmesh_highlight)
+            except RuntimeError:
+                pass
+        navpoly = navmesh.localize(mouse_position)
+        if navpoly is None:
+            return
+        self.navmesh_highlight = self.draw_shapely_poly(
+            scene, navpoly.poly, CONST.COLORS["transparent"],
+            CONST.COLORS["light_green_transparent"])
+
+    def draw_shortest_path(self, scene: QGraphicsScene, navmesh: NavMesh,
+                           destination: Point, player: bool) -> None:
+        for line in self.shortest_path_segments:
+            try:
+                scene.removeItem(line)
+            except RuntimeError:
+                pass
+
+        if player:
+            origin = self.game.theater.player_points()[0]
+        else:
+            origin = self.game.theater.enemy_points()[0]
+
+        prev_pos = self._transform_point(origin.position)
+        try:
+            path = navmesh.shortest_path(origin.position, destination)
+        except ValueError:
+            return
+        for waypoint in path[1:]:
+            new_pos = self._transform_point(waypoint)
+            flight_path_pen = self.flight_path_pen(player, selected=True)
+            # Draw the line to the *middle* of the waypoint.
+            offset = self.WAYPOINT_SIZE // 2
+            self.shortest_path_segments.append(scene.addLine(
+                prev_pos[0] + offset, prev_pos[1] + offset,
+                new_pos[0] + offset, new_pos[1] + offset,
+                flight_path_pen
+            ))
+
+            self.shortest_path_segments.append(scene.addEllipse(
+                new_pos[0], new_pos[1], self.WAYPOINT_SIZE,
+                self.WAYPOINT_SIZE, flight_path_pen, flight_path_pen
+            ))
+
+            prev_pos = new_pos
+
+    def draw_tarcap_plan(self, scene: QGraphicsScene, point_near_target: Point,
+                         player: bool) -> None:
+        for line in self.shortest_path_segments:
+            try:
+                scene.removeItem(line)
+            except RuntimeError:
+                pass
+
+        self.clear_flight_paths(scene)
+
+        target = self.game.theater.closest_target(point_near_target)
+
+        if player:
+            origin = self.game.theater.player_points()[0]
+        else:
+            origin = self.game.theater.enemy_points()[0]
+
+        package = Package(target)
+        flight = Flight(package, F_16C_50, 2, FlightType.TARCAP,
+                        start_type="Warm", departure=origin, arrival=origin,
+                        divert=None)
+        package.add_flight(flight)
+        planner = FlightPlanBuilder(self.game, package, is_player=player)
+        planner.populate_flight_plan(flight)
+
+        self.draw_flight_plan(scene, flight, selected=True)
+
+
     @staticmethod
     def should_display_ground_objects_at(cp: ControlPoint) -> bool:
         return ((DisplayOptions.sam_ranges and cp.captured) or
@@ -378,6 +518,11 @@ class QLiberationMap(QGraphicsView):
         self.display_threat_zones(scene, DisplayOptions.red_threat_zones,
                                   player=False)
 
+        if DisplayOptions.navmeshes.blue_navmesh:
+            self.display_navmesh(scene, player=True)
+        if DisplayOptions.navmeshes.red_navmesh:
+            self.display_navmesh(scene, player=False)
+
         for cp in self.game.theater.controlpoints:
 
             pos = self._transform_point(cp.position)
@@ -477,8 +622,9 @@ class QLiberationMap(QGraphicsView):
                                         flight.flight_plan)
             prev_pos = tuple(new_pos)
 
-    def draw_waypoint(self, scene: QGraphicsScene, position: Tuple[int, int],
-                      player: bool, selected: bool) -> None:
+    def draw_waypoint(self, scene: QGraphicsScene,
+                      position: Tuple[float, float], player: bool,
+                      selected: bool) -> None:
         waypoint_pen = self.waypoint_pen(player, selected)
         waypoint_brush = self.waypoint_brush(player, selected)
         self.flight_path_items.append(scene.addEllipse(
@@ -521,8 +667,8 @@ class QLiberationMap(QGraphicsView):
         item.setZValue(2)
         self.flight_path_items.append(item)
 
-    def draw_flight_path(self, scene: QGraphicsScene, pos0: Tuple[int, int],
-                         pos1: Tuple[int, int], player: bool,
+    def draw_flight_path(self, scene: QGraphicsScene, pos0: Tuple[float, float],
+                         pos1: Tuple[float, float], player: bool,
                          selected: bool) -> None:
         flight_path_pen = self.flight_path_pen(player, selected)
         # Draw the line to the *middle* of the waypoint.
@@ -874,17 +1020,41 @@ class QLiberationMap(QGraphicsView):
         return self.game.theater.is_in_sea(world_destination)
 
     def sceneMouseMovedEvent(self, event: QGraphicsSceneMouseEvent):
+        if self.game is None:
+            return
+
+        mouse_position = Point(event.scenePos().x(), event.scenePos().y())
         if self.state == QLiberationMapState.MOVING_UNIT:
             self.setCursor(Qt.PointingHandCursor)
             self.movement_line.setLine(
                 QLineF(self.movement_line.line().p1(), event.scenePos()))
 
-            pos = Point(event.scenePos().x(), event.scenePos().y())
-            if self.is_valid_ship_pos(pos):
+            if self.is_valid_ship_pos(mouse_position):
                 self.movement_line.setPen(CONST.COLORS["green"])
             else:
                 self.movement_line.setPen(CONST.COLORS["red"])
 
+        mouse_world_pos = self._scene_to_dcs_coords(mouse_position)
+        if DisplayOptions.navmeshes.blue_navmesh:
+            self.highlight_mouse_navmesh(
+                self.scene(), self.game.blue_navmesh,
+                self._scene_to_dcs_coords(mouse_position))
+            if DisplayOptions.path_debug.shortest_path:
+                self.draw_shortest_path(self.scene(), self.game.blue_navmesh,
+                                        mouse_world_pos, player=True)
+
+        if DisplayOptions.navmeshes.red_navmesh:
+            self.highlight_mouse_navmesh(
+                self.scene(), self.game.red_navmesh, mouse_world_pos)
+            if DisplayOptions.path_debug.shortest_path:
+                self.draw_shortest_path(self.scene(), self.game.red_navmesh,
+                                        mouse_world_pos, player=False)
+
+        if DisplayOptions.path_debug.blue_tarcap:
+            self.draw_tarcap_plan(self.scene(), mouse_world_pos, player=True)
+        if DisplayOptions.path_debug.red_tarcap:
+            self.draw_tarcap_plan(self.scene(), mouse_world_pos, player=False)
+
     def sceneMousePressEvent(self, event: QGraphicsSceneMouseEvent):
         if self.state == QLiberationMapState.MOVING_UNIT:
             if event.buttons() == Qt.RightButton: