Source code for simvx.core.navigation3d.nodes

"""Navigation3D node types: Region (holds a mesh), Agent (path-follower), Obstacle (dynamic)."""

import logging

from ..descriptors import Property, Signal
from ..math.types import Vec3
from ..nodes_3d.node3d import Node3D
from .mesh import NavigationMesh3D
from .server import NavigationServer3D

log = logging.getLogger(__name__)




class NavigationRegion3D(Node3D):
    """Node that holds a NavigationMesh3D and registers it with the server.

    Attach a NavigationMesh3D to this node and add it to the scene tree to make
    its walkable surface available for pathfinding.
    """

    enabled = Property(True, hint="Whether this region participates in pathfinding")

    def __init__(self, navigation_mesh: NavigationMesh3D | None = None, **kwargs):
        super().__init__(**kwargs)
        self.navigation_mesh: NavigationMesh3D | None = navigation_mesh



    def enter_tree(self) -> None:
        NavigationServer3D.get_singleton().register_region(self)




    def exit_tree(self) -> None:
        NavigationServer3D.get_singleton().unregister_region(self)





class NavigationAgent3D(Node3D):
    """3D pathfinding agent that follows paths on the navigation mesh.

    Computes a path to target_position and advances along it each physics frame.
    Emits navigation_finished when the target is reached.

    Attach as a child of a Node3D whose position you want to steer.
    """

    target_desired_distance = Property(1.0, range=(0.01, 100.0), hint="Distance to target to consider reached")
    path_desired_distance = Property(1.0, range=(0.01, 100.0), hint="Distance to path point before advancing")
    max_speed = Property(10.0, range=(0.0, 1000.0), hint="Maximum movement speed")
    avoidance_radius = Property(0.5, range=(0.0, 50.0), hint="Radius for obstacle avoidance")

    def __init__(self, **kwargs):
        super().__init__(**kwargs)
        self._target_position: Vec3 = Vec3()
        self._path: list[Vec3] = []
        self._path_index: int = 0
        self._navigation_finished_flag: bool = True
        self.velocity: Vec3 = Vec3()
        self.navigation_finished = Signal()

    @property
    def target_position(self) -> Vec3:
        return self._target_position



    @target_position.setter
    def target_position(self, value: Vec3) -> None:
        self._target_position = Vec3(value)
        self._recompute_path()


    def _recompute_path(self) -> None:
        """Recompute path from current position to target."""
        origin = self._get_origin()
        server = NavigationServer3D.get_singleton()
        self._path = server.find_path(origin, self._target_position)
        self._path_index = 1 if len(self._path) > 1 else 0
        self._navigation_finished_flag = len(self._path) == 0

    def _get_origin(self) -> Vec3:
        """Get the world position of the agent."""
        return Vec3(self.world_position)



    def is_navigation_finished(self) -> bool:
        """Check if the agent has reached its target or has no path."""
        return self._navigation_finished_flag




    @property
    def remaining_path_points(self) -> int:
        """Number of waypoints ahead of the agent on its current path."""
        if not self._path:
            return 0
        return max(0, len(self._path) - self._path_index)




    @property
    def remaining_path_length(self) -> float:
        """Total distance along the path from the agent's current position to the final waypoint.

        Sums the straight-line distance from the agent to the next waypoint plus each
        remaining segment. Returns 0.0 when the path is empty or fully consumed.
        """
        if not self._path or self._path_index >= len(self._path):
            return 0.0
        total = float((self._path[self._path_index] - self._get_origin()).length())
        for i in range(self._path_index, len(self._path) - 1):
            total += float((self._path[i + 1] - self._path[i]).length())
        return total




    def get_next_path_position(self) -> Vec3:
        """Get the next waypoint position the agent is heading toward.

        Returns:
            Next waypoint, or current position if path is empty.
        """
        if not self._path or self._path_index >= len(self._path):
            return self._get_origin()
        return self._path[self._path_index]




    def physics_process(self, dt: float) -> None:
        """Advance along the path, computing steering velocity."""
        if self._navigation_finished_flag or not self._path:
            self.velocity = Vec3()
            return

        origin = self._get_origin()

        # Advance past reached waypoints
        while self._path_index < len(self._path):
            wp = self._path[self._path_index]
            dist = (wp - origin).length()
            if self._path_index == len(self._path) - 1:
                # Final waypoint — use target distance
                if dist <= self.target_desired_distance:
                    self._navigation_finished_flag = True
                    self.velocity = Vec3()
                    self.navigation_finished()
                    return
                break
            else:
                if dist <= self.path_desired_distance:
                    self._path_index += 1
                else:
                    break

        if self._path_index >= len(self._path):
            self._navigation_finished_flag = True
            self.velocity = Vec3()
            self.navigation_finished()
            return

        # Compute steering velocity toward next waypoint
        target_wp = self._path[self._path_index]
        direction = target_wp - origin
        dist = direction.length()
        if dist > 1e-6:
            direction = direction.normalized()
            speed = min(self.max_speed, dist / dt) if dt > 0 else self.max_speed
            self.velocity = direction * speed
        else:
            self.velocity = Vec3()

        # Simple obstacle avoidance — steer away from nearby obstacles
        self._apply_avoidance(origin)


    def _apply_avoidance(self, origin: Vec3) -> None:
        """Apply simple obstacle avoidance to the current velocity."""
        if self.avoidance_radius <= 0:
            return
        server = NavigationServer3D.get_singleton()
        for obstacle in server.obstacles:
            if not obstacle.visible:
                continue
            obs_pos = obstacle.world_position
            to_agent = origin - obs_pos
            dist = to_agent.length()
            combined_radius = self.avoidance_radius + obstacle.radius
            if dist < combined_radius and dist > 1e-6:
                # Push velocity away from obstacle
                push_dir = to_agent.normalized()
                push_strength = (combined_radius - dist) / combined_radius
                self.velocity = self.velocity + push_dir * (self.max_speed * push_strength * 0.5)
                # Clamp to max speed
                vel_len = self.velocity.length()
                if vel_len > self.max_speed:
                    self.velocity = self.velocity.normalized() * self.max_speed




class NavigationObstacle3D(Node3D):
    """Dynamic obstacle that affects navigation agent avoidance.

    Place in the scene tree to create areas that agents will steer around.
    Does not carve the navmesh — instead, agents detect obstacles at runtime
    and adjust their velocity to avoid collisions.
    """

    radius = Property(1.0, range=(0.01, 100.0), hint="Avoidance radius")
    height = Property(2.0, range=(0.01, 100.0), hint="Obstacle height")



    def enter_tree(self) -> None:
        NavigationServer3D.get_singleton().register_obstacle(self)




    def exit_tree(self) -> None:
        NavigationServer3D.get_singleton().unregister_obstacle(self)