navmesh_chhunks_demo_3d.gd 6.3 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158
  1. extends Node3D
  2. static var map_cell_size: float = 0.25
  3. static var chunk_size: int = 16
  4. static var cell_size: float = 0.25
  5. static var agent_radius: float = 0.5
  6. static var chunk_id_to_region: Dictionary = {}
  7. var path_start_position: Vector3
  8. func _ready() -> void:
  9. NavigationServer3D.set_debug_enabled(true)
  10. path_start_position = %DebugPaths.global_position
  11. var map: RID = get_world_3d().navigation_map
  12. NavigationServer3D.map_set_cell_size(map, map_cell_size)
  13. # Disable performance costly edge connection margin feature.
  14. # This feature is not needed to merge navigation mesh edges.
  15. # If edges are well aligned they will merge just fine by edge key.
  16. NavigationServer3D.map_set_use_edge_connections(map, false)
  17. # Parse the collision shapes below our parse root node.
  18. var source_geometry: NavigationMeshSourceGeometryData3D = NavigationMeshSourceGeometryData3D.new()
  19. var parse_settings: NavigationMesh = NavigationMesh.new()
  20. parse_settings.geometry_parsed_geometry_type = NavigationMesh.PARSED_GEOMETRY_STATIC_COLLIDERS
  21. NavigationServer3D.parse_source_geometry_data(parse_settings, source_geometry, %ParseRootNode)
  22. create_region_chunks(%ChunksContainer, source_geometry, chunk_size * cell_size, agent_radius)
  23. static func create_region_chunks(chunks_root_node: Node, p_source_geometry: NavigationMeshSourceGeometryData3D, p_chunk_size: float, p_agent_radius: float) -> void:
  24. # We need to know how many chunks are required for the input geometry.
  25. # So first get an axis aligned bounding box that covers all vertices.
  26. var input_geometry_bounds: AABB = calculate_source_geometry_bounds(p_source_geometry)
  27. # Rasterize bounding box into chunk grid to know range of required chunks.
  28. var start_chunk: Vector3 = floor(
  29. input_geometry_bounds.position / p_chunk_size
  30. )
  31. var end_chunk: Vector3 = floor(
  32. (input_geometry_bounds.position + input_geometry_bounds.size)
  33. / p_chunk_size
  34. )
  35. # NavigationMesh.border_size is limited to the xz-axis.
  36. # So we can only bake one chunk for the y-axis and also
  37. # need to span the bake bounds over the entire y-axis.
  38. # If we dont do this we would create duplicated polygons
  39. # and stack them on top of each other causing merge errors.
  40. var bounds_min_height: float = start_chunk.y
  41. var bounds_max_height: float = end_chunk.y + p_chunk_size
  42. var chunk_y: int = 0
  43. for chunk_z in range(start_chunk.z, end_chunk.z + 1):
  44. for chunk_x in range(start_chunk.x, end_chunk.x + 1):
  45. var chunk_id: Vector3i = Vector3i(chunk_x, chunk_y, chunk_z)
  46. var chunk_bounding_box: AABB = AABB(
  47. Vector3(chunk_x, bounds_min_height, chunk_z) * p_chunk_size,
  48. Vector3(p_chunk_size, bounds_max_height, p_chunk_size),
  49. )
  50. # We grow the chunk bounding box to include geometry
  51. # from all the neighbor chunks so edges can align.
  52. # The border size is the same value as our grow amount so
  53. # the final navigation mesh ends up with the intended chunk size.
  54. var baking_bounds: AABB = chunk_bounding_box.grow(p_chunk_size)
  55. var chunk_navmesh: NavigationMesh = NavigationMesh.new()
  56. chunk_navmesh.geometry_parsed_geometry_type = NavigationMesh.PARSED_GEOMETRY_STATIC_COLLIDERS
  57. chunk_navmesh.cell_size = cell_size
  58. chunk_navmesh.cell_height = cell_size
  59. chunk_navmesh.filter_baking_aabb = baking_bounds
  60. chunk_navmesh.border_size = p_chunk_size
  61. chunk_navmesh.agent_radius = p_agent_radius
  62. NavigationServer3D.bake_from_source_geometry_data(chunk_navmesh, p_source_geometry)
  63. # The only reason we reset the baking bounds here is to not render its debug.
  64. chunk_navmesh.filter_baking_aabb = AABB()
  65. # Snap vertex positions to avoid most rasterization issues with float precision.
  66. var navmesh_vertices: PackedVector3Array = chunk_navmesh.vertices
  67. for i in navmesh_vertices.size():
  68. var vertex: Vector3 = navmesh_vertices[i]
  69. navmesh_vertices[i] = vertex.snappedf(map_cell_size * 0.1)
  70. chunk_navmesh.vertices = navmesh_vertices
  71. var chunk_region: NavigationRegion3D = NavigationRegion3D.new()
  72. chunk_region.navigation_mesh = chunk_navmesh
  73. chunks_root_node.add_child(chunk_region)
  74. chunk_id_to_region[chunk_id] = chunk_region
  75. static func calculate_source_geometry_bounds(p_source_geometry: NavigationMeshSourceGeometryData3D) -> AABB:
  76. if p_source_geometry.has_method("get_bounds"):
  77. # Godot 4.3 Patch added get_bounds() function that does the same but faster.
  78. return p_source_geometry.call("get_bounds")
  79. var bounds: AABB = AABB()
  80. var first_vertex: bool = true
  81. var vertices: PackedFloat32Array = p_source_geometry.get_vertices()
  82. var vertices_count: int = vertices.size() / 3
  83. for i in vertices_count:
  84. var vertex: Vector3 = Vector3(vertices[i * 3], vertices[i * 3 + 1], vertices[i * 3 + 2])
  85. if first_vertex:
  86. first_vertex = false
  87. bounds.position = vertex
  88. else:
  89. bounds = bounds.expand(vertex)
  90. for projected_obstruction: Dictionary in p_source_geometry.get_projected_obstructions():
  91. var projected_obstruction_vertices: PackedFloat32Array = projected_obstruction["vertices"]
  92. for i in projected_obstruction_vertices.size() / 3:
  93. var vertex: Vector3 = Vector3(projected_obstruction.vertices[i * 3], projected_obstruction.vertices[i * 3 + 1], projected_obstruction.vertices[i * 3 + 2]);
  94. if first_vertex:
  95. first_vertex = false
  96. bounds.position = vertex
  97. else:
  98. bounds = bounds.expand(vertex)
  99. return bounds
  100. func _process(_delta: float) -> void:
  101. var mouse_cursor_position: Vector2 = get_viewport().get_mouse_position()
  102. var map: RID = get_world_3d().navigation_map
  103. # Do not query when the map has never synchronized and is empty.
  104. if NavigationServer3D.map_get_iteration_id(map) == 0:
  105. return
  106. var camera: Camera3D = get_viewport().get_camera_3d()
  107. var camera_ray_length: float = 1000.0
  108. var camera_ray_start: Vector3 = camera.project_ray_origin(mouse_cursor_position)
  109. var camera_ray_end: Vector3 = camera_ray_start + camera.project_ray_normal(mouse_cursor_position) * camera_ray_length
  110. var closest_point_on_navmesh: Vector3 = NavigationServer3D.map_get_closest_point_to_segment(
  111. map,
  112. camera_ray_start,
  113. camera_ray_end
  114. )
  115. if Input.is_mouse_button_pressed(MOUSE_BUTTON_LEFT):
  116. path_start_position = closest_point_on_navmesh
  117. %DebugPaths.global_position = path_start_position
  118. %PathDebugCorridorFunnel.target_position = closest_point_on_navmesh
  119. %PathDebugEdgeCentered.target_position = closest_point_on_navmesh
  120. %PathDebugCorridorFunnel.get_next_path_position()
  121. %PathDebugEdgeCentered.get_next_path_position()