Dynamic obstacles

extends Node

const SPEED: float = 330.0
const BODY_RADIUS: float = 16.0
const MARGIN: float = 60.0
const GAP: float = 40.0
const SWAP_EVERY: float = 1.2
const COLUMN_CLEAR: float = 120.0

var bounds: Rect2 = Rect2()
var column_x: float = 0.0
var poly: NavigationPolygon
var region: NavigationRegion2D
var region_visual: Node2D
var obstacle_visuals: Array[Polygon2D] = []
var obstacles: Array[NavigationObstacle2D] = []
var open_index: int = 1
var swap_timer: float = 0.0
var bakes: int = 0
var info: Label

var goals: PackedVector2Array = PackedVector2Array()
var goal_index: int = 0
var body: Node2D
var agent: NavigationAgent2D
var path_line: Line2D


func _ready() -> void:
	_add_background()

	var view: Vector2 = get_viewport().get_visible_rect().size
	bounds = Rect2(MARGIN, MARGIN, view.x - MARGIN * 2.0, view.y - MARGIN * 2.0)
	column_x = view.x * 0.5
	goals = PackedVector2Array([Vector2(view.x - 130, view.y * 0.5), Vector2(130, view.y * 0.5)])

	poly = NavigationPolygon.new()
	poly.agent_radius = BODY_RADIUS + 8.0
	# with the default PARSED_GEOMETRY_BOTH every Polygon2D on screen is parsed as an obstruction
	poly.parsed_geometry_type = NavigationPolygon.PARSED_GEOMETRY_STATIC_COLLIDERS

	region = NavigationRegion2D.new()
	add_child(region)

	var slots: Array[Rect2] = []
	var span: float = (bounds.size.y - GAP * 2.0) / 3.0

	for i in 3:
		slots.append(Rect2(column_x - 45.0, MARGIN + i * (span + GAP), 90, span))

	for slot in slots:
		var obstacle: NavigationObstacle2D = NavigationObstacle2D.new()
		obstacle.position = slot.position + slot.size * 0.5
		obstacle.vertices = _rect_points(Rect2(-slot.size * 0.5, slot.size))
		add_child(obstacle)
		obstacles.append(obstacle)

	region_visual = Node2D.new()
	add_child(region_visual)

	for slot in slots:
		var block: Polygon2D = _make_rect(slot.size, Color('#fc7f7f'))
		block.position = slot.position + slot.size * 0.5
		add_child(block)
		obstacle_visuals.append(block)

	path_line = Line2D.new()
	path_line.width = 5.0
	path_line.default_color = Color('#8eef97')
	add_child(path_line)

	body = Node2D.new()
	body.position = goals[1]
	body.add_child(_make_circle(BODY_RADIUS, Color('#8da5f3')))
	add_child(body)

	agent = NavigationAgent2D.new()
	agent.radius = BODY_RADIUS
	agent.path_desired_distance = 12.0
	agent.target_desired_distance = 12.0
	body.add_child(agent)

	info = Label.new()
	info.position = Vector2(MARGIN, 18)
	info.add_theme_font_size_override('font_size', 20)
	info.add_theme_color_override('font_color', Color('#f3d48d'))
	add_child(info)

	_apply_obstacles()

	print('one of the three obstacles steps out every %.1fs and the agent re-routes through the gap' % SWAP_EVERY)


func _physics_process(delta: float) -> void:
	swap_timer += delta

	if swap_timer >= SWAP_EVERY and absf(body.position.x - column_x) > COLUMN_CLEAR:
		swap_timer = 0.0
		open_index = (open_index + 1) % obstacles.size()
		_apply_obstacles()

	var next: Vector2 = agent.get_next_path_position()
	var path: PackedVector2Array = agent.get_current_navigation_path()

	if path.is_empty():
		agent.target_position = goals[goal_index]
		return

	path_line.points = path

	if agent.is_navigation_finished():
		goal_index = (goal_index + 1) % goals.size()
		agent.target_position = goals[goal_index]
		return

	body.position = body.position.move_toward(next, SPEED * delta)


func _apply_obstacles() -> void:
	for i in obstacles.size():
		obstacles[i].affect_navigation_mesh = i != open_index
		obstacle_visuals[i].color = Color(0.99, 0.5, 0.5, 0.16) if i == open_index else Color('#fc7f7f')

	_rebake()
	agent.target_position = goals[goal_index]


func _rebake() -> void:
	var source: NavigationMeshSourceGeometryData2D = NavigationMeshSourceGeometryData2D.new()
	# parse_source_geometry_data wipes the data it receives, so the traversable outline comes after it
	NavigationServer2D.parse_source_geometry_data(poly, source, self)
	source.add_traversable_outline(_rect_points(bounds))
	NavigationServer2D.bake_from_source_geometry_data(poly, source)

	region.navigation_polygon = poly
	_rebuild_region_visual()

	bakes += 1
	info.text = 'bake %d: %d obstacles in the mesh, %d navigation polygons' % [bakes, obstacles.size() - 1, poly.get_polygon_count()]


func _rebuild_region_visual() -> void:
	for face in region_visual.get_children():
		face.queue_free()

	var vertices: PackedVector2Array = poly.get_vertices()

	for i in poly.get_polygon_count():
		var points: PackedVector2Array = PackedVector2Array()

		for index in poly.get_polygon(i):
			points.append(vertices[index])

		var face: Polygon2D = Polygon2D.new()
		face.polygon = points
		face.color = Color(0.55, 0.65, 0.95, 0.12)
		region_visual.add_child(face)

		var edge: Line2D = Line2D.new()
		edge.points = points
		edge.closed = true
		edge.width = 2.0
		edge.default_color = Color(0.55, 0.65, 0.95, 0.45)
		region_visual.add_child(edge)


func _rect_points(rect: Rect2) -> PackedVector2Array:
	return PackedVector2Array([rect.position, Vector2(rect.end.x, rect.position.y), rect.end, Vector2(rect.position.x, rect.end.y)])


func _make_rect(size: Vector2, color: Color) -> Polygon2D:
	var rect: Polygon2D = Polygon2D.new()
	rect.polygon = PackedVector2Array([-size * 0.5, Vector2(size.x, -size.y) * 0.5, size * 0.5, Vector2(-size.x, size.y) * 0.5])
	rect.color = color

	return rect


func _make_circle(radius: float, color: Color) -> Polygon2D:
	var points: PackedVector2Array = PackedVector2Array()

	for i in 20:
		points.append(Vector2(cos(TAU * i / 20.0), sin(TAU * i / 20.0)) * radius)

	var circle: Polygon2D = Polygon2D.new()
	circle.polygon = points
	circle.color = color

	return circle


func _add_background() -> void:
	var background: ColorRect = ColorRect.new()
	background.color = Color('#12141a')
	add_child(background)
	background.set_anchors_and_offsets_preset(Control.PRESET_FULL_RECT)