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. Any triangle the obstacle overlaps is removed, so the hole is the obstacle rounded outwards to the nearest triangle edge: the finer the cell_size, the closer the hole is to the shape you asked for.

  • get_closest_point() snapping a click to walkable ground. It skips the carved triangles and returns None when nothing walkable is within the distance you allow, which is how a click on a crate is rejected.

  • NavigationRegion3D registering the navmesh with the server

  • NavigationAgent3D attached as a child of the node it steers, which moves by the agent’s velocity each frame

  • NavigationObstacle3D for runtime avoidance around the spheres

  • 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. Any
 10    triangle the obstacle overlaps is removed, so the hole is the obstacle
 11    rounded outwards to the nearest triangle edge: the finer the cell_size, the
 12    closer the hole is to the shape you asked for.
 13  - get_closest_point() snapping a click to walkable ground. It skips the
 14    carved triangles and returns None when nothing walkable is within the
 15    distance you allow, which is how a click on a crate is rejected.
 16  - NavigationRegion3D registering the navmesh with the server
 17  - NavigationAgent3D attached as a child of the node it steers, which moves
 18    by the agent's velocity each frame
 19  - NavigationObstacle3D for runtime avoidance around the spheres
 20  - HUD showing agent state (idle / navigating / arrived)
 21
 22Controls: Left-click set destination, R reset agent
 23Run: uv run python examples/features/3d/navigation.py
 24"""
 25
 26import math
 27
 28from simvx.core import (
 29    Camera3D,
 30    DirectionalLight3D,
 31    Input,
 32    InputMap,
 33    Key,
 34    Material,
 35    Mesh,
 36    MeshInstance3D,
 37    MouseButton,
 38    NavigationAgent3D,
 39    NavigationMesh3D,
 40    NavigationObstacle3D,
 41    NavigationRegion3D,
 42    Node3D,
 43    Text2D,
 44    Vec3,
 45    screen_to_ray,
 46)
 47from simvx.graphics import App
 48
 49# Box obstacles: (centre, scale).  Used for both rendering and navmesh carving.
 50BOX_OBSTACLES = [
 51    ((-5, 0.75, -3), (3, 1.5, 2)),
 52    ((4, 0.75, 5), (2.5, 1.5, 3)),
 53    ((-2, 0.75, 8), (4, 1.5, 1.5)),
 54    ((8, 0.75, -6), (2, 1.5, 4)),
 55]
 56MARGIN = 0.3  # extra margin around obstacles for agent clearance
 57NAV_HALF = 14.5  # navmesh boundary (slightly inside the 15-unit polygon)
 58SNAP_RANGE = 4.0  # how far a click may be from walkable ground and still count
 59
 60
 61def _box_to_obstacle_poly(centre: tuple, scale: tuple) -> list[Vec3]:
 62    """Convert box centre + scale to an XZ obstacle polygon with margin."""
 63    cx, _, cz = centre
 64    hx, hz = scale[0] / 2 + MARGIN, scale[2] / 2 + MARGIN
 65    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)]
 66
 67
 68class NavigationScene(Node3D):
 69    def on_ready(self):
 70        InputMap.add_action("click", [MouseButton.LEFT])
 71        InputMap.add_action("reset", [Key.R])
 72        InputMap.add_action("quit", [Key.ESCAPE])
 73
 74        cam = Camera3D(position=(0, 25, 18), fov=50)
 75        cam.look_at((0, 0, 0), up=(0, 1, 0))
 76        self.add_child(cam)
 77
 78        sun = DirectionalLight3D(position=(10, 15, 8))
 79        sun.colour, sun.intensity = (1.0, 0.95, 0.9), 1.2
 80        sun.look_at((0, 0, 0))
 81        self.add_child(sun)
 82
 83        ground = MeshInstance3D(mesh=Mesh.cube(), material=Material(colour=(0.3, 0.45, 0.3, 1), roughness=0.9))
 84        ground.position, ground.scale = (0, -0.15, 0), (30, 0.3, 30)
 85        self.add_child(ground)
 86
 87        # Navigation mesh -- subdivided rectangle with obstacle holes carved out
 88        nav_mesh = NavigationMesh3D()
 89        nav_mesh.add_polygon(
 90            [Vec3(-15, 0, -15), Vec3(15, 0, -15), Vec3(15, 0, 15), Vec3(-15, 0, 15)],
 91            cell_size=1.0,
 92        )
 93        for pos, scl in BOX_OBSTACLES:
 94            nav_mesh.add_obstacle(_box_to_obstacle_poly(pos, scl))
 95
 96        # Box obstacle visuals
 97        box_mesh, box_mat = Mesh.cube(), Material(colour=(0.55, 0.35, 0.2, 1), roughness=0.7)
 98        for pos, scl in BOX_OBSTACLES:
 99            b = MeshInstance3D(mesh=box_mesh, material=box_mat, position=pos)
100            b.scale = scl
101            self.add_child(b)
102
103        # Sphere obstacles -- carved into the navmesh as circular polygons
104        sphere_obstacles = [(6, 0), (-8, -5)]
105        sphere_radius = 1.5 + MARGIN
106        for ox, oz in sphere_obstacles:
107            # Approximate circle as 8-sided polygon for navmesh carving
108            poly = [
109                Vec3(ox + sphere_radius * math.cos(a), 0, oz + sphere_radius * math.sin(a))
110                for a in (i * math.pi / 4 for i in range(8))
111            ]
112            nav_mesh.add_obstacle(poly)
113
114        self._nav_mesh = nav_mesh
115        self.add_child(NavigationRegion3D(navigation_mesh=nav_mesh))
116
117        # Sphere obstacle visuals. Each one also gets a NavigationObstacle3D so
118        # the agent steers around it at runtime, on top of the navmesh carve.
119        sph_mesh, sph_mat = Mesh.sphere(), Material(colour=(0.7, 0.2, 0.2, 1), roughness=0.5)
120        for ox, oz in sphere_obstacles:
121            vis = MeshInstance3D(mesh=sph_mesh, material=sph_mat, position=(ox, 0.6, oz))
122            vis.scale = (1.2, 1.2, 1.2)
123            self.add_child(vis)
124            self.add_child(NavigationObstacle3D(position=(ox, 0, oz), radius=sphere_radius))
125
126        # The agent steers the node it is attached to: a plain Node3D carrying
127        # both the agent and the green sphere that visualises it.
128        self._runner = self.add_child(Node3D(name="Runner"))
129        self._agent = self._runner.add_child(
130            NavigationAgent3D(max_speed=10.0, target_desired_distance=0.8, avoidance_radius=0.6)
131        )
132        self._agent.navigation_finished.connect(self._on_nav_finished)
133
134        agent_vis = MeshInstance3D(
135            mesh=Mesh.sphere(),
136            material=Material(colour=(0.2, 0.8, 0.3, 1), roughness=0.3, metallic=0.4),
137            position=(0, 0.5, 0),
138        )
139        agent_vis.scale = (0.8, 0.8, 0.8)
140        self._runner.add_child(agent_vis)
141
142        # Target marker (blue, hidden below ground)
143        self._marker = MeshInstance3D(mesh=Mesh.sphere(), material=Material(colour=(0.2, 0.4, 1.0, 0.5)))
144        self._marker.position, self._marker.scale = (0, -10, 0), (0.4, 0.4, 0.4)
145        self.add_child(self._marker)
146
147        self._hud = self.add_child(
148            Text2D(text="Click to set destination | [R] Reset", position=(10, 10), font_scale=1.5)
149        )
150        self._state_hud = self.add_child(Text2D(text="State: Idle", position=(10, 40), font_scale=1.5))
151        self._navigating = False
152        self._arrived = False
153
154    def _on_nav_finished(self):
155        self._navigating = False
156        self._arrived = True
157
158    def on_update(self, dt: float):
159        if Input.is_action_just_pressed("quit"):
160            self.app.quit()
161            return
162        # Click to set target -- project mouse onto ground plane (y=0)
163        if Input.is_action_just_pressed("click"):
164            cam = self.find(Camera3D)
165            if cam and self.app:
166                mouse = Input.mouse_position
167                w, h = self.app.width, self.app.height
168                origin, d = screen_to_ray(mouse, (w, h), cam.view_matrix, cam.projection_matrix(w / h))
169                if d[1] != 0:
170                    t = -origin[1] / d[1]
171                    if t > 0:
172                        hit = origin + d * t
173                        # Clamp target to navmesh bounds
174                        tx = max(-NAV_HALF, min(NAV_HALF, float(hit[0])))
175                        tz = max(-NAV_HALF, min(NAV_HALF, float(hit[2])))
176                        # A click can land on a crate or a sphere, where the
177                        # navmesh has been carved away. get_closest_point skips
178                        # the carved triangles, so this walks the target back
179                        # out to the edge of the obstacle, and returns None
180                        # when the click is nowhere near walkable ground.
181                        goal = self._nav_mesh.get_closest_point(Vec3(tx, 0, tz), SNAP_RANGE)
182                        if goal is not None:
183                            self._agent.target_position = goal
184                            self._marker.position = Vec3(goal.x, 0.2, goal.z)
185                            self._navigating, self._arrived = True, False
186
187        if Input.is_action_just_pressed("reset"):
188            self._runner.position, self._marker.position = Vec3(0, 0, 0), Vec3(0, -10, 0)
189            # Cancel any in-flight navigation. stop() drops the path and zeroes the
190            # steering velocity without reporting an arrival.
191            self._agent.stop()
192            self._navigating, self._arrived = False, False
193
194        # The agent computes a steering velocity (waypoint following plus
195        # obstacle avoidance) in its own fixed update; move its parent by it.
196        self._runner.position = self._runner.position + self._agent.velocity * dt
197
198        # Update HUD
199        if self._navigating and not self._agent.is_navigation_finished():
200            p = self._runner.position
201            waypoints = self._agent.remaining_path_points
202            self._state_hud.text = f"State: Navigating  pos=({p[0]:.1f}, {p[2]:.1f})  waypoints={waypoints}"
203        else:
204            self._state_hud.text = "State: Arrived" if self._arrived else "State: Idle"
205
206
207if __name__ == "__main__":
208    App(title="3D Navigation Demo", width=1280, height=720).run(NavigationScene())