Source code for simvx.core.navigation3d.server

"""NavigationServer3D — singleton coordinator across all registered regions."""

from __future__ import annotations

import logging
from typing import TYPE_CHECKING

from ..math.types import Vec3
from ._helpers import _path_length

if TYPE_CHECKING:
    from .nodes import NavigationObstacle3D, NavigationRegion3D

log = logging.getLogger(__name__)




class NavigationServer3D:
    """Global navigation server that manages all registered navigation regions.

    Provides unified pathfinding and spatial queries across all active regions.
    """

    _instance: NavigationServer3D | None = None

    def __init__(self):
        self._regions: list[NavigationRegion3D] = []
        self._obstacles: list[NavigationObstacle3D] = []



    @classmethod
    def get_singleton(cls) -> NavigationServer3D:
        """Return the global NavigationServer3D instance (created on first access)."""
        if cls._instance is None:
            cls._instance = cls()
        return cls._instance




    @classmethod
    def reset(cls) -> None:
        """Reset the singleton (useful for testing)."""
        cls._instance = None




    def register_region(self, region: NavigationRegion3D) -> None:
        """Register a navigation region."""
        if region not in self._regions:
            self._regions.append(region)




    def unregister_region(self, region: NavigationRegion3D) -> None:
        """Unregister a navigation region."""
        if region in self._regions:
            self._regions.remove(region)




    def register_obstacle(self, obstacle: NavigationObstacle3D) -> None:
        """Register a dynamic obstacle."""
        if obstacle not in self._obstacles:
            self._obstacles.append(obstacle)




    def unregister_obstacle(self, obstacle: NavigationObstacle3D) -> None:
        """Unregister a dynamic obstacle."""
        if obstacle in self._obstacles:
            self._obstacles.remove(obstacle)




    def find_path(self, start: Vec3, end: Vec3) -> list[Vec3]:
        """Find a path from start to end across all active navigation regions.

        Queries each enabled region and returns the shortest valid path found.

        Args:
            start: Start position in world space.
            end: End position in world space.

        Returns:
            List of Vec3 waypoints, or empty list if unreachable.
        """
        best_path: list[Vec3] = []
        best_cost = float("inf")
        for region in self._regions:
            if not region.enabled or region.navigation_mesh is None:
                continue
            path = region.navigation_mesh.find_path(start, end)
            if path:
                cost = _path_length(path)
                if cost < best_cost:
                    best_cost = cost
                    best_path = path
        return best_path




    def get_closest_point(self, point: Vec3) -> Vec3:
        """Find the closest point on any active navmesh surface.

        Args:
            point: Query point in world space.

        Returns:
            Closest point on any navmesh, or the input point if no regions exist.
        """
        best_point = Vec3(point)
        best_dist_sq = float("inf")
        for region in self._regions:
            if not region.enabled or region.navigation_mesh is None:
                continue
            cp = region.navigation_mesh.get_closest_point(point)
            d = (cp - point).length_squared()
            if d < best_dist_sq:
                best_dist_sq = d
                best_point = cp
        return best_point


    @property
    def obstacles(self) -> list[NavigationObstacle3D]:
        """All registered dynamic obstacles."""
        return list(self._obstacles)