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