Agent 3D

extends Node

const SPEED: float = 9.0
const GROUND: float = 30.0
const AGENT_RADIUS: float = 0.75
const DOT_COUNT: int = 80
const DOT_SPACING: float = 1.0

var walls: Array[AABB] = [
	AABB(Vector3(-8, 0, -13), Vector3(2, 3, 20)),
	AABB(Vector3(-1, 0, -7), Vector3(2, 3, 20)),
	AABB(Vector3(6, 0, -13), Vector3(2, 3, 20)),
]
var targets: Array[Vector3] = [Vector3(12, 0, 12), Vector3(12, 0, -12), Vector3(-12, 0, 12), Vector3(-12, 0, -12)]
var target_index: int = 0

var player: CharacterBody3D
var agent: NavigationAgent3D
var camera: Camera3D
var marker: MeshInstance3D
var dots: Array[MeshInstance3D] = []


func _ready() -> void:
	_add_environment()
	_add_light()
	_add_ground()

	for wall in walls:
		_add_wall(wall)

	var region: NavigationRegion3D = NavigationRegion3D.new()
	region.navigation_mesh = _bake()
	add_child(region)

	_add_navmesh_visual(region.navigation_mesh)
	_add_dots()

	marker = MeshInstance3D.new()
	var disc: CylinderMesh = CylinderMesh.new()
	disc.top_radius = 0.9
	disc.bottom_radius = 0.9
	disc.height = 0.1
	marker.mesh = disc
	marker.material_override = _flat(Color('#f3d48d'))
	add_child(marker)

	player = CharacterBody3D.new()
	player.position = Vector3(-12, 0, -12)
	add_child(player)
	_dress_player()

	agent = NavigationAgent3D.new()
	agent.radius = AGENT_RADIUS
	# the baked mesh floats one cell above the floor, and a desired distance under that gap stalls the agent on its first path point
	agent.path_desired_distance = 1.0
	agent.target_desired_distance = 1.2
	agent.path_height_offset = 0.5
	player.add_child(agent)

	camera = Camera3D.new()
	camera.position = Vector3(0, 20, 21)
	add_child(camera)
	camera.look_at(Vector3.ZERO, Vector3.UP)
	camera.current = true

	print('the walker tours the four corners, click the floor to send it there instead')


func _physics_process(_delta: float) -> void:
	var next: Vector3 = agent.get_next_path_position()
	var path: PackedVector3Array = agent.get_current_navigation_path()

	# the navigation map only syncs at the end of a physics frame, so the first queries come back empty
	if path.is_empty():
		_send_to(targets[target_index])
		return

	_update_dots(path)

	if agent.is_navigation_finished():
		target_index = (target_index + 1) % targets.size()
		_send_to(targets[target_index])
		return

	var direction: Vector3 = next - player.global_position
	direction.y = 0.0

	if direction.length() > 0.05:
		player.velocity = direction.normalized() * SPEED
		player.look_at(player.global_position + direction, Vector3.UP)

	player.move_and_slide()


func _unhandled_input(event: InputEvent) -> void:
	if event is InputEventMouseButton and event.pressed and event.button_index == MOUSE_BUTTON_LEFT:
		var hit: Variant = Plane(Vector3.UP, 0.0).intersects_ray(camera.project_ray_origin(event.position), camera.project_ray_normal(event.position))

		if hit != null:
			_send_to(hit)


func _send_to(where: Vector3) -> void:
	agent.target_position = where
	marker.position = where + Vector3(0, 0.1, 0)


func _bake() -> NavigationMesh:
	var nav: NavigationMesh = NavigationMesh.new()
	nav.cell_size = 0.25
	nav.cell_height = 0.25
	nav.agent_radius = AGENT_RADIUS
	nav.agent_height = 1.5
	nav.agent_max_climb = 0.25

	var source: NavigationMeshSourceGeometryData3D = NavigationMeshSourceGeometryData3D.new()

	var floor_mesh: PlaneMesh = PlaneMesh.new()
	floor_mesh.size = Vector2(GROUND, GROUND)
	# add_mesh would pull the geometry back from the GPU, add_mesh_array keeps it on the cpu
	source.add_mesh_array(floor_mesh.get_mesh_arrays(), Transform3D.IDENTITY)

	for wall in walls:
		var box: BoxMesh = BoxMesh.new()
		box.size = wall.size
		source.add_mesh_array(box.get_mesh_arrays(), Transform3D(Basis.IDENTITY, wall.position + wall.size * 0.5))

	NavigationServer3D.bake_from_source_geometry_data(nav, source)

	return nav


func _add_navmesh_visual(nav: NavigationMesh) -> void:
	var vertices: PackedVector3Array = nav.get_vertices()
	var faces: PackedVector3Array = PackedVector3Array()

	for i in nav.get_polygon_count():
		var indices: PackedInt32Array = nav.get_polygon(i)

		for corner in range(1, indices.size() - 1):
			faces.append(vertices[indices[0]])
			faces.append(vertices[indices[corner]])
			faces.append(vertices[indices[corner + 1]])

	var arrays: Array = []
	arrays.resize(Mesh.ARRAY_MAX)
	arrays[Mesh.ARRAY_VERTEX] = faces

	var mesh: ArrayMesh = ArrayMesh.new()
	mesh.add_surface_from_arrays(Mesh.PRIMITIVE_TRIANGLES, arrays)

	var material: StandardMaterial3D = _flat(Color(0.55, 0.65, 0.95, 0.4))
	material.transparency = BaseMaterial3D.TRANSPARENCY_ALPHA
	material.cull_mode = BaseMaterial3D.CULL_DISABLED

	var visual: MeshInstance3D = MeshInstance3D.new()
	visual.mesh = mesh
	visual.material_override = material
	add_child(visual)


func _add_dots() -> void:
	var sphere: SphereMesh = SphereMesh.new()
	sphere.radius = 0.16
	sphere.height = 0.32
	sphere.radial_segments = 6
	sphere.rings = 3

	var material: StandardMaterial3D = _flat(Color('#8eef97'))

	for i in DOT_COUNT:
		var dot: MeshInstance3D = MeshInstance3D.new()
		dot.mesh = sphere
		dot.material_override = material
		dot.visible = false
		add_child(dot)
		dots.append(dot)


func _update_dots(path: PackedVector3Array) -> void:
	var used: int = 0

	for i in range(path.size() - 1):
		var steps: int = int(path[i].distance_to(path[i + 1]) / DOT_SPACING)

		for step in steps:
			if used >= dots.size():
				break

			dots[used].position = path[i].lerp(path[i + 1], float(step) / float(steps)) + Vector3(0, 0.2, 0)
			dots[used].visible = true
			used += 1

	for i in range(used, dots.size()):
		dots[i].visible = false


func _dress_player() -> void:
	var capsule: CapsuleMesh = CapsuleMesh.new()
	capsule.radius = 0.5
	capsule.height = 2.0

	var visual: MeshInstance3D = MeshInstance3D.new()
	visual.mesh = capsule
	visual.position = Vector3(0, 1, 0)
	visual.material_override = _lit(Color('#8da5f3'))
	player.add_child(visual)

	var nose: MeshInstance3D = MeshInstance3D.new()
	var box: BoxMesh = BoxMesh.new()
	box.size = Vector3(0.3, 0.3, 0.7)
	nose.mesh = box
	nose.position = Vector3(0, 1.2, -0.6)
	nose.material_override = _lit(Color('#f3d48d'))
	player.add_child(nose)

	var shape: CapsuleShape3D = CapsuleShape3D.new()
	shape.radius = 0.5
	shape.height = 2.0

	var collision: CollisionShape3D = CollisionShape3D.new()
	collision.shape = shape
	collision.position = Vector3(0, 1, 0)
	player.add_child(collision)


func _add_wall(wall: AABB) -> void:
	var mesh: BoxMesh = BoxMesh.new()
	mesh.size = wall.size

	var visual: MeshInstance3D = MeshInstance3D.new()
	visual.mesh = mesh
	visual.material_override = _lit(Color('#fc7f7f'))

	var shape: BoxShape3D = BoxShape3D.new()
	shape.size = wall.size

	var collision: CollisionShape3D = CollisionShape3D.new()
	collision.shape = shape

	var body: StaticBody3D = StaticBody3D.new()
	body.position = wall.position + wall.size * 0.5
	body.add_child(visual)
	body.add_child(collision)
	add_child(body)


func _add_ground() -> void:
	var mesh: BoxMesh = BoxMesh.new()
	mesh.size = Vector3(GROUND, 1, GROUND)

	var visual: MeshInstance3D = MeshInstance3D.new()
	visual.mesh = mesh
	visual.material_override = _lit(Color('#2a2f3d'))

	var shape: BoxShape3D = BoxShape3D.new()
	shape.size = mesh.size

	var collision: CollisionShape3D = CollisionShape3D.new()
	collision.shape = shape

	var body: StaticBody3D = StaticBody3D.new()
	body.position = Vector3(0, -0.5, 0)
	body.add_child(visual)
	body.add_child(collision)
	add_child(body)


func _lit(color: Color) -> StandardMaterial3D:
	var material: StandardMaterial3D = StandardMaterial3D.new()
	material.albedo_color = color
	material.roughness = 0.6

	return material


func _flat(color: Color) -> StandardMaterial3D:
	var material: StandardMaterial3D = StandardMaterial3D.new()
	material.albedo_color = color
	material.shading_mode = BaseMaterial3D.SHADING_MODE_UNSHADED

	return material


func _add_environment() -> void:
	var sky_material: ProceduralSkyMaterial = ProceduralSkyMaterial.new()
	sky_material.sky_top_color = Color(0.05, 0.06, 0.12)
	sky_material.sky_horizon_color = Color(0.14, 0.16, 0.24)
	sky_material.ground_bottom_color = Color(0.06, 0.07, 0.11)
	sky_material.ground_horizon_color = Color(0.1, 0.11, 0.16)

	var sky: Sky = Sky.new()
	sky.sky_material = sky_material

	var env: Environment = Environment.new()
	env.background_mode = Environment.BG_SKY
	env.sky = sky
	env.ambient_light_source = Environment.AMBIENT_SOURCE_SKY
	env.ambient_light_energy = 0.7

	var world: WorldEnvironment = WorldEnvironment.new()
	world.environment = env
	add_child(world)


func _add_light() -> void:
	var light: DirectionalLight3D = DirectionalLight3D.new()
	light.rotation_degrees = Vector3(-55, -35, 0)
	light.light_energy = 1.2
	light.shadow_enabled = true
	add_child(light)