3D Navigation

click-to-move pathfinding on a navmesh with carved obstacles.

▶ Run in browser

Tags: 3d

A NavigationMesh3D is the walkable surface. add_polygon(vertices, cell_size=...) adds a floor and splits it into triangles about cell_size across, and add_obstacle(vertices) carves a hole: every triangle the obstacle polygon overlaps is removed. The hole is therefore the obstacle rounded outwards to the nearest triangle edge, and a finer cell_size makes it follow the shape more closely. Here each crate and sphere is carved with a small margin so the agent keeps clear of it, the spheres as octagons.

NavigationRegion3D(navigation_mesh=nav_mesh) registers the mesh with the navigation server while it is in the tree, and a NavigationAgent3D paths across every registered region. Setting agent.target_position plans a route; each fixed step the agent then writes a steering velocity into agent.velocity and emits navigation_finished on arrival. The agent moves nothing itself: it is a child of the Runner node it steers, so it shares that node’s position, and the runner moves by agent.velocity in its on_fixed_update. agent.stop() cancels a route without reporting an arrival.

A NavigationObstacle3D does not carve the mesh. The agent steers away from any obstacle within its avoidance_radius plus the obstacle’s radius, which here keeps it off the spheres as it rounds them.

A click becomes a destination in two steps: camera.ray_at(position) casts a ray through the pointer and intersect_plane finds where it meets the ground, then nav_mesh.get_closest_point(point, SNAP_RANGE) snaps that to walkable ground. A click on a crate walks to the crate’s edge, and one more than SNAP_RANGE from walkable ground returns None and is ignored.

Click or tap the ground to send the green runner there; R or Reset puts it back in the middle.

Source

  1# /// simvx
  2# web = { width = 1280, height = 720 }
  3# ///
  4"""3D Navigation: click-to-move pathfinding on a navmesh with carved obstacles.
  5
  6A `NavigationMesh3D` is the walkable surface. `add_polygon(vertices, cell_size=...)` adds a
  7floor and splits it into triangles about `cell_size` across, and `add_obstacle(vertices)`
  8carves a hole: every triangle the obstacle polygon overlaps is removed. The hole is therefore
  9the obstacle rounded outwards to the nearest triangle edge, and a finer `cell_size` makes it
 10follow the shape more closely. Here each crate and sphere is carved with a small margin so the
 11agent keeps clear of it, the spheres as octagons.
 12
 13`NavigationRegion3D(navigation_mesh=nav_mesh)` registers the mesh with the navigation server
 14while it is in the tree, and a `NavigationAgent3D` paths across every registered region.
 15Setting `agent.target_position` plans a route; each fixed step the agent then writes a steering
 16velocity into `agent.velocity` and emits `navigation_finished` on arrival. The agent moves
 17nothing itself: it is a child of the `Runner` node it steers, so it shares that node's
 18position, and the runner moves by `agent.velocity` in its `on_fixed_update`. `agent.stop()`
 19cancels a route without reporting an arrival.
 20
 21A `NavigationObstacle3D` does not carve the mesh. The agent steers away from any obstacle
 22within its `avoidance_radius` plus the obstacle's `radius`, which here keeps it off the spheres
 23as it rounds them.
 24
 25A click becomes a destination in two steps: `camera.ray_at(position)` casts a ray through the
 26pointer and `intersect_plane` finds where it meets the ground, then
 27`nav_mesh.get_closest_point(point, SNAP_RANGE)` snaps that to walkable ground. A click on a
 28crate walks to the crate's edge, and one more than `SNAP_RANGE` from walkable ground returns
 29`None` and is ignored.
 30
 31Click or tap the ground to send the green runner there; R or Reset puts it back in the middle.
 32"""
 33
 34import math
 35
 36from simvx.core import (
 37    Anchor,
 38    Button,
 39    Camera3D,
 40    CanvasLayer,
 41    DirectionalLight3D,
 42    Key,
 43    Label,
 44    Material,
 45    Mesh,
 46    MeshInstance3D,
 47    NavigationAgent3D,
 48    NavigationMesh3D,
 49    NavigationObstacle3D,
 50    NavigationRegion3D,
 51    Node3D,
 52    Pointer,
 53    Vec3,
 54    on_input,
 55)
 56from simvx.graphics import App
 57
 58FLOOR = 15  # the walkable floor runs from -FLOOR to FLOOR on x and z
 59CRATES = [((-5, -3), (3, 2)), ((4, 5), (2.5, 3)), ((-2, 8), (4, 1.5)), ((8, -6), (2, 4))]  # (x, z), (width, depth)
 60SPHERES = [(6, 0), (-8, -5)]  # (x, z)
 61SPHERE_RADIUS = 1.2
 62MARGIN = 0.3  # clearance carved around every obstacle
 63SNAP_RANGE = 4.0  # how far a click may be from walkable ground and still count
 64
 65
 66def rectangle(x: float, z: float, half_width: float, half_depth: float) -> list[Vec3]:
 67    return [Vec3(x + sx * half_width, 0, z + sz * half_depth) for sx, sz in [(-1, -1), (1, -1), (1, 1), (-1, 1)]]
 68
 69
 70def octagon(x: float, z: float, radius: float) -> list[Vec3]:
 71    return [Vec3(x + radius * math.cos(a), 0, z + radius * math.sin(a)) for a in (i * math.pi / 4 for i in range(8))]
 72
 73
 74class NavigationScene(Node3D):
 75    input_context = {"reset": [Key.R]}
 76
 77    def on_ready(self):
 78        self.camera = self.add_child(Camera3D(position=(0, 25, 18), fov=50))
 79        self.camera.look_at((0, 0, 0))
 80        self.add_child(DirectionalLight3D(colour=(1.0, 0.95, 0.9), intensity=1.2, direction=(-10, -15, -8)))
 81        ground = Material(colour=(0.3, 0.45, 0.3), roughness=0.9)
 82        self.add_child(MeshInstance3D(mesh=Mesh.plane(2 * FLOOR), material=ground))
 83
 84        self.nav_mesh = NavigationMesh3D()
 85        self.nav_mesh.add_polygon(rectangle(0, 0, FLOOR, FLOOR), cell_size=1.0)
 86
 87        crate = Material(colour=(0.55, 0.35, 0.2), roughness=0.7)
 88        for (x, z), (w, d) in CRATES:
 89            self.add_child(MeshInstance3D(mesh=Mesh.box((w, 1.5, d)), material=crate, position=(x, 0.75, z)))
 90            self.nav_mesh.add_obstacle(rectangle(x, z, w / 2 + MARGIN, d / 2 + MARGIN))
 91
 92        sphere = Material(colour=(0.7, 0.2, 0.2), roughness=0.5)
 93        for x, z in SPHERES:
 94            self.add_child(MeshInstance3D(mesh=Mesh.sphere(SPHERE_RADIUS), material=sphere, position=(x, 0.6, z)))
 95            self.nav_mesh.add_obstacle(octagon(x, z, SPHERE_RADIUS + MARGIN))
 96            self.add_child(NavigationObstacle3D(position=(x, 0, z), radius=SPHERE_RADIUS + MARGIN))
 97
 98        self.add_child(NavigationRegion3D(navigation_mesh=self.nav_mesh))
 99
100        self.runner = self.add_child(Node3D(name="Runner"))
101        self.agent = self.runner.add_child(
102            NavigationAgent3D(max_speed=10.0, target_desired_distance=0.8, avoidance_radius=0.6)
103        )
104        self.agent.navigation_finished.connect(self.on_arrived)
105        body = Material(colour=(0.2, 0.8, 0.3), roughness=0.3, metallic=0.4)
106        self.runner.add_child(MeshInstance3D(mesh=Mesh.sphere(0.8), material=body, position=(0, 0.5, 0)))
107
108        self.marker = self.add_child(
109            MeshInstance3D(mesh=Mesh.sphere(0.4), material=Material(colour=(0.2, 0.4, 1.0)), visible=False)
110        )
111
112        # Example help overlay: not part of the technique shown.
113        hud = self.add_child(CanvasLayer())
114        self.status = hud.add_child(Label("Idle", anchor=Anchor.TOP_LEFT, inset=12, font_size=20))
115        hud.add_child(Label("Click or tap the ground to walk there   R: reset", anchor=Anchor.BOTTOM_LEFT, inset=12))
116        hud.add_child(Button("Reset", action="reset", anchor=Anchor.BOTTOM_RIGHT, inset=12))
117
118    @on_input(Pointer.PRIMARY)
119    def walk_to(self, event):
120        point = self.camera.ray_at(event.position).intersect_plane((0, 0, 0), (0, 1, 0))
121        goal = None if point is None else self.nav_mesh.get_closest_point(point, SNAP_RANGE)
122        if goal is not None:
123            self.agent.target_position = goal
124            self.marker.position = (goal.x, 0.2, goal.z)
125            self.marker.visible = True
126
127    @on_input("reset")
128    def reset(self, _event):
129        self.agent.stop()
130        self.runner.position = (0, 0, 0)
131        self.marker.visible = False
132        self.status.text = "Idle"
133
134    def on_arrived(self):
135        self.status.text = "Arrived"
136
137    def on_fixed_update(self, dt):
138        self.runner.position = self.runner.position + self.agent.velocity * dt
139        if not self.agent.is_navigation_finished():
140            x, _, z = self.runner.position
141            self.status.text = f"Navigating ({x:.1f}, {z:.1f}), {self.agent.remaining_path_points} waypoints left"
142
143
144if __name__ == "__main__":
145    App(title="3D Navigation", width=1280, height=720).run(NavigationScene())