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

  • NavigationObstacle3D for dynamic (runtime) avoidance

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