3D Navigation

NavigationMesh3D pathfinding with obstacle carving.

▶ Run in browser

Tags: 3d

Demonstrates:

  • NavigationMesh3D with subdivided walkable polygon (cell_size)

  • Obstacle carving via add_obstacle() to cut holes in the navmesh

  • NavigationRegion3D registering the navmesh with the server

  • NavigationAgent3D following a path that routes around obstacles

  • HUD showing agent state (idle / navigating / arrived)

Controls: Left-click set destination, R reset agent Run: uv run python examples/features/3d/navigation.py

Source

  1"""3D Navigation: NavigationMesh3D pathfinding with obstacle carving.
  2
  3# /// simvx
  4# web = { width = 1280, height = 720 }
  5# ///
  6
  7Demonstrates:
  8  - NavigationMesh3D with subdivided walkable polygon (cell_size)
  9  - Obstacle carving via add_obstacle() to cut holes in the navmesh
 10  - NavigationRegion3D registering the navmesh with the server
 11  - NavigationAgent3D following a path that routes around obstacles
 12  - HUD showing agent state (idle / navigating / arrived)
 13
 14Controls: Left-click set destination, R reset agent
 15Run: uv run python examples/features/3d/navigation.py
 16"""
 17
 18
 19import numpy as np
 20
 21from simvx.core import (
 22    Camera3D,
 23    DirectionalLight3D,
 24    Input,
 25    InputMap,
 26    Key,
 27    Material,
 28    Mesh,
 29    MeshInstance3D,
 30    MouseButton,
 31    NavigationAgent3D,
 32    NavigationMesh3D,
 33    NavigationRegion3D,
 34    Node3D,
 35    Text2D,
 36    Vec3,
 37    screen_to_ray,
 38)
 39from simvx.graphics import App
 40
 41# Box obstacles: (centre, scale).  Used for both rendering and navmesh carving.
 42BOX_OBSTACLES = [
 43    ((-5, 0.75, -3), (3, 1.5, 2)), ((4, 0.75, 5), (2.5, 1.5, 3)),
 44    ((-2, 0.75, 8), (4, 1.5, 1.5)), ((8, 0.75, -6), (2, 1.5, 4)),
 45]
 46MARGIN = 0.3  # extra margin around obstacles for agent clearance
 47NAV_HALF = 14.5  # navmesh boundary (slightly inside the 15-unit polygon)
 48
 49
 50def _box_to_obstacle_poly(centre: tuple, scale: tuple) -> list[Vec3]:
 51    """Convert box centre + scale to an XZ obstacle polygon with margin."""
 52    cx, _, cz = centre
 53    hx, hz = scale[0] / 2 + MARGIN, scale[2] / 2 + MARGIN
 54    return [Vec3(cx - hx, 0, cz - hz), Vec3(cx + hx, 0, cz - hz), Vec3(cx + hx, 0, cz + hz), Vec3(cx - hx, 0, cz + hz)]
 55
 56
 57class NavigationScene(Node3D):
 58    def on_ready(self):
 59        InputMap.add_action("click", [MouseButton.LEFT])
 60        InputMap.add_action("reset", [Key.R])
 61        InputMap.add_action("quit", [Key.ESCAPE])
 62
 63        cam = Camera3D(position=(0, 25, 18), fov=50)
 64        cam.look_at((0, 0, 0), up=(0, 1, 0))
 65        self.add_child(cam)
 66
 67        sun = DirectionalLight3D(position=(10, 15, 8))
 68        sun.colour, sun.intensity = (1.0, 0.95, 0.9), 1.2
 69        sun.look_at((0, 0, 0))
 70        self.add_child(sun)
 71
 72        ground = MeshInstance3D(mesh=Mesh.cube(), material=Material(colour=(0.3, 0.45, 0.3, 1), roughness=0.9))
 73        ground.position, ground.scale = (0, -0.15, 0), (30, 0.3, 30)
 74        self.add_child(ground)
 75
 76        # Navigation mesh -- subdivided rectangle with obstacle holes carved out
 77        nav_mesh = NavigationMesh3D()
 78        nav_mesh.add_polygon(
 79            [Vec3(-15, 0, -15), Vec3(15, 0, -15), Vec3(15, 0, 15), Vec3(-15, 0, 15)],
 80            cell_size=1.0,
 81        )
 82        for pos, scl in BOX_OBSTACLES:
 83            nav_mesh.add_obstacle(_box_to_obstacle_poly(pos, scl))
 84
 85        # Box obstacle visuals
 86        box_mesh, box_mat = Mesh.cube(), Material(colour=(0.55, 0.35, 0.2, 1), roughness=0.7)
 87        for pos, scl in BOX_OBSTACLES:
 88            b = MeshInstance3D(mesh=box_mesh, material=box_mat, position=pos)
 89            b.scale = scl
 90            self.add_child(b)
 91
 92        # Sphere obstacles -- carved into the navmesh as circular polygons
 93        sphere_obstacles = [(6, 0), (-8, -5)]
 94        sphere_radius = 1.5 + MARGIN
 95        import math
 96        for ox, oz in sphere_obstacles:
 97            # Approximate circle as 8-sided polygon for navmesh carving
 98            poly = [Vec3(ox + sphere_radius * math.cos(a), 0, oz + sphere_radius * math.sin(a))
 99                    for a in (i * math.pi / 4 for i in range(8))]
100            nav_mesh.add_obstacle(poly)
101
102        self.add_child(NavigationRegion3D(navigation_mesh=nav_mesh))
103
104        # Sphere obstacle visuals
105        sph_mesh, sph_mat = Mesh.sphere(), Material(colour=(0.7, 0.2, 0.2, 1), roughness=0.5)
106        for ox, oz in sphere_obstacles:
107            vis = MeshInstance3D(mesh=sph_mesh, material=sph_mat, position=(ox, 0.6, oz))
108            vis.scale = (1.2, 1.2, 1.2)
109            self.add_child(vis)
110
111        # Navigation agent
112        self._agent = self.add_child(
113            NavigationAgent3D(max_speed=10.0, target_desired_distance=0.8, avoidance_radius=0.6)
114        )
115        self._agent.navigation_finished.connect(self._on_nav_finished)
116
117        # Agent visual (green sphere)
118        self._agent_vis = MeshInstance3D(
119            mesh=Mesh.sphere(), material=Material(colour=(0.2, 0.8, 0.3, 1), roughness=0.3, metallic=0.4),
120            position=(0, 0.5, 0),
121        )
122        self._agent_vis.scale = (0.8, 0.8, 0.8)
123        self.add_child(self._agent_vis)
124
125        # Target marker (blue, hidden below ground)
126        self._marker = MeshInstance3D(mesh=Mesh.sphere(), material=Material(colour=(0.2, 0.4, 1.0, 0.5)))
127        self._marker.position, self._marker.scale = (0, -10, 0), (0.4, 0.4, 0.4)
128        self.add_child(self._marker)
129
130        self._hud = self.add_child(
131            Text2D(text="Click to set destination | [R] Reset", position=(10, 10), font_scale=1.5))
132        self._state_hud = self.add_child(Text2D(text="State: Idle", position=(10, 40), font_scale=1.5))
133        self._navigating = False
134
135    def _on_nav_finished(self):
136        self._navigating = False
137
138    def on_update(self, dt: float):
139        if Input.is_action_just_pressed("quit"):
140            self.app.quit()
141            return
142        # Click to set target -- project mouse onto ground plane (y=0)
143        if Input.is_action_just_pressed("click"):
144            cam = self.find(Camera3D)
145            if cam and self.app:
146                mouse = Input.mouse_position
147                w, h = self.app.width, self.app.height
148                origin, d = screen_to_ray(mouse, (w, h), cam.view_matrix, cam.projection_matrix(w / h))
149                if d[1] != 0:
150                    t = -origin[1] / d[1]
151                    if t > 0:
152                        hit = origin + d * t
153                        # Clamp target to navmesh bounds
154                        tx = max(-NAV_HALF, min(NAV_HALF, float(hit[0])))
155                        tz = max(-NAV_HALF, min(NAV_HALF, float(hit[2])))
156                        self._agent.target_position = Vec3(tx, 0, tz)
157                        self._marker.position = Vec3(tx, 0.2, tz)
158                        self._navigating = True
159
160        if Input.is_action_just_pressed("reset"):
161            self._agent_vis.position, self._marker.position = Vec3(0, 0.5, 0), Vec3(0, -10, 0)
162            # Cancel any in-flight navigation: re-target the agent to its new
163            # position so the path and finished flag are recomputed.
164            self._agent.position = Vec3(0, 0, 0)
165            self._agent.target_position = Vec3(0, 0, 0)
166            self._navigating = False
167
168        # Sync agent position from visual so path queries work from current position
169        self._agent.position = Vec3(self._agent_vis.position[0], 0, self._agent_vis.position[2])
170
171        # Steer agent toward next path position
172        if not self._agent.is_navigation_finished():
173            next_pos = self._agent.get_next_path_position()
174            direction = (next_pos - self._agent_vis.position)
175            direction = Vec3(direction[0], 0, direction[2])  # Keep on ground plane
176            if np.linalg.norm(direction) > 0.01:
177                direction = direction / np.linalg.norm(direction)
178                self._agent_vis.position += direction * self._agent.max_speed * dt
179                self._agent_vis.position = Vec3(self._agent_vis.position[0], 0.5, self._agent_vis.position[2])
180
181        # Update HUD
182        if self._navigating and not self._agent.is_navigation_finished():
183            p = self._agent_vis.position
184            waypoints = self._agent.remaining_path_points
185            self._state_hud.text = f"State: Navigating  pos=({p[0]:.1f}, {p[2]:.1f})  waypoints={waypoints}"
186        else:
187            self._state_hud.text = "State: Arrived" if self._navigating else "State: Idle"
188
189
190if __name__ == "__main__":
191    App(title="3D Navigation Demo", width=1280, height=720).run(NavigationScene())