[Navigation] Create a dedicated 2D navigation server

* Add a dedicated 2D server
* Create dedicated tests
* Split performance metrics between 2D and 3D
* Rename the 3D only server module

.github/CODEOWNERS

@@ -167,7 +167,8 @@
 /modules/gridmap/                             @godotengine/3d-nodes
 /modules/gridmap/doc_classes/                 @godotengine/3d-nodes @godotengine/documentation
 /modules/gridmap/icons/                       @godotengine/3d-nodes @godotengine/usability
-/modules/navigation/                          @godotengine/navigation
+/modules/navigation_2d/                       @godotengine/navigation
+/modules/navigation_3d/                       @godotengine/navigation
 /modules/noise/                               @godotengine/core
 /modules/noise/doc_classes/                   @godotengine/core @godotengine/documentation
 /modules/noise/icons/                         @godotengine/core @godotengine/usability

core/config/project_settings.cpp

@@ -1631,6 +1631,17 @@ ProjectSettings::ProjectSettings() {
 	GLOBAL_DEF_INTERNAL("internationalization/locale/translations_pot_files", PackedStringArray());
 	GLOBAL_DEF_INTERNAL("internationalization/locale/translation_add_builtin_strings_to_pot", false);
 
+	GLOBAL_DEF("navigation/world/map_use_async_iterations", true);
+
+	GLOBAL_DEF("navigation/avoidance/thread_model/avoidance_use_multiple_threads", true);
+	GLOBAL_DEF("navigation/avoidance/thread_model/avoidance_use_high_priority_threads", true);
+
+	GLOBAL_DEF("navigation/pathfinding/max_threads", 4);
+
+	GLOBAL_DEF("navigation/baking/use_crash_prevention_checks", true);
+	GLOBAL_DEF("navigation/baking/thread_model/baking_use_multiple_threads", true);
+	GLOBAL_DEF("navigation/baking/thread_model/baking_use_high_priority_threads", true);
+
 	ProjectSettings::get_singleton()->add_hidden_prefix("input/");
 }
 

doc/classes/NavigationServer2D.xml

@@ -303,6 +303,13 @@
 				Returns all created navigation map [RID]s on the NavigationServer. This returns both 2D and 3D created navigation maps as there is technically no distinction between them.
 			</description>
 		</method>
+		<method name="get_process_info" qualifiers="const">
+			<return type="int" />
+			<param index="0" name="process_info" type="int" enum="NavigationServer2D.ProcessInfo" />
+			<description>
+				Returns information about the current state of the NavigationServer. See [enum ProcessInfo] for a list of available states.
+			</description>
+		</method>
 		<method name="is_baking_navigation_polygon" qualifiers="const">
 			<return type="bool" />
 			<param index="0" name="navigation_polygon" type="NavigationPolygon" />
@@ -971,6 +978,13 @@
 				If [param enabled] is [code]true[/code], the navigation [param region] will use edge connections to connect with other navigation regions within proximity of the navigation map edge connection margin.
 			</description>
 		</method>
+		<method name="set_active">
+			<return type="void" />
+			<param index="0" name="active" type="bool" />
+			<description>
+				Control activation of this server.
+			</description>
+		</method>
 		<method name="set_debug_enabled">
 			<return type="void" />
 			<param index="0" name="enabled" type="bool" />
@@ -1006,6 +1020,11 @@
 		</method>
 	</methods>
 	<signals>
+		<signal name="avoidance_debug_changed">
+			<description>
+				Emitted when avoidance debug settings are changed. Only available in debug builds.
+			</description>
+		</signal>
 		<signal name="map_changed">
 			<param index="0" name="map" type="RID" />
 			<description>
@@ -1018,4 +1037,36 @@
 			</description>
 		</signal>
 	</signals>
+	<constants>
+		<constant name="INFO_ACTIVE_MAPS" value="0" enum="ProcessInfo">
+			Constant to get the number of active navigation maps.
+		</constant>
+		<constant name="INFO_REGION_COUNT" value="1" enum="ProcessInfo">
+			Constant to get the number of active navigation regions.
+		</constant>
+		<constant name="INFO_AGENT_COUNT" value="2" enum="ProcessInfo">
+			Constant to get the number of active navigation agents processing avoidance.
+		</constant>
+		<constant name="INFO_LINK_COUNT" value="3" enum="ProcessInfo">
+			Constant to get the number of active navigation links.
+		</constant>
+		<constant name="INFO_POLYGON_COUNT" value="4" enum="ProcessInfo">
+			Constant to get the number of navigation mesh polygons.
+		</constant>
+		<constant name="INFO_EDGE_COUNT" value="5" enum="ProcessInfo">
+			Constant to get the number of navigation mesh polygon edges.
+		</constant>
+		<constant name="INFO_EDGE_MERGE_COUNT" value="6" enum="ProcessInfo">
+			Constant to get the number of navigation mesh polygon edges that were merged due to edge key overlap.
+		</constant>
+		<constant name="INFO_EDGE_CONNECTION_COUNT" value="7" enum="ProcessInfo">
+			Constant to get the number of navigation mesh polygon edges that are considered connected by edge proximity.
+		</constant>
+		<constant name="INFO_EDGE_FREE_COUNT" value="8" enum="ProcessInfo">
+			Constant to get the number of navigation mesh polygon edges that could not be merged but may be still connected by edge proximity or with links.
+		</constant>
+		<constant name="INFO_OBSTACLE_COUNT" value="9" enum="ProcessInfo">
+			Constant to get the number of active navigation obstacles.
+		</constant>
+	</constants>
 </class>

doc/classes/Performance.xml

@@ -195,34 +195,34 @@
 			Output latency of the [AudioServer]. Equivalent to calling [method AudioServer.get_output_latency], it is not recommended to call this every frame.
 		</constant>
 		<constant name="NAVIGATION_ACTIVE_MAPS" value="24" enum="Monitor">
-			Number of active navigation maps in the [NavigationServer3D]. This also includes the two empty default navigation maps created by World2D and World3D.
+			Number of active navigation maps in [NavigationServer2D] and [NavigationServer3D]. This also includes the two empty default navigation maps created by World2D and World3D.
 		</constant>
 		<constant name="NAVIGATION_REGION_COUNT" value="25" enum="Monitor">
-			Number of active navigation regions in the [NavigationServer3D].
+			Number of active navigation regions in [NavigationServer2D] and [NavigationServer3D].
 		</constant>
 		<constant name="NAVIGATION_AGENT_COUNT" value="26" enum="Monitor">
-			Number of active navigation agents processing avoidance in the [NavigationServer3D].
+			Number of active navigation agents processing avoidance in [NavigationServer2D] and [NavigationServer3D].
 		</constant>
 		<constant name="NAVIGATION_LINK_COUNT" value="27" enum="Monitor">
-			Number of active navigation links in the [NavigationServer3D].
+			Number of active navigation links in [NavigationServer2D] and [NavigationServer3D].
 		</constant>
 		<constant name="NAVIGATION_POLYGON_COUNT" value="28" enum="Monitor">
-			Number of navigation mesh polygons in the [NavigationServer3D].
+			Number of navigation mesh polygons in [NavigationServer2D] and [NavigationServer3D].
 		</constant>
 		<constant name="NAVIGATION_EDGE_COUNT" value="29" enum="Monitor">
-			Number of navigation mesh polygon edges in the [NavigationServer3D].
+			Number of navigation mesh polygon edges in [NavigationServer2D] and [NavigationServer3D].
 		</constant>
 		<constant name="NAVIGATION_EDGE_MERGE_COUNT" value="30" enum="Monitor">
-			Number of navigation mesh polygon edges that were merged due to edge key overlap in the [NavigationServer3D].
+			Number of navigation mesh polygon edges that were merged due to edge key overlap in [NavigationServer2D] and [NavigationServer3D].
 		</constant>
 		<constant name="NAVIGATION_EDGE_CONNECTION_COUNT" value="31" enum="Monitor">
-			Number of polygon edges that are considered connected by edge proximity [NavigationServer3D].
+			Number of polygon edges that are considered connected by edge proximity [NavigationServer2D] and [NavigationServer3D].
 		</constant>
 		<constant name="NAVIGATION_EDGE_FREE_COUNT" value="32" enum="Monitor">
-			Number of navigation mesh polygon edges that could not be merged in the [NavigationServer3D]. The edges still may be connected by edge proximity or with links.
+			Number of navigation mesh polygon edges that could not be merged in [NavigationServer2D] and [NavigationServer3D]. The edges still may be connected by edge proximity or with links.
 		</constant>
 		<constant name="NAVIGATION_OBSTACLE_COUNT" value="33" enum="Monitor">
-			Number of active navigation obstacles in the [NavigationServer3D].
+			Number of active navigation obstacles in the [NavigationServer2D] and [NavigationServer3D].
 		</constant>
 		<constant name="PIPELINE_COMPILATIONS_CANVAS" value="34" enum="Monitor">
 			Number of pipeline compilations that were triggered by the 2D canvas renderer.
@@ -239,7 +239,67 @@
 		<constant name="PIPELINE_COMPILATIONS_SPECIALIZATION" value="38" enum="Monitor">
 			Number of pipeline compilations that were triggered to optimize the current scene. These compilations are done in the background and should not cause any stutters whatsoever.
 		</constant>
-		<constant name="MONITOR_MAX" value="39" enum="Monitor">
+		<constant name="NAVIGATION_2D_ACTIVE_MAPS" value="39" enum="Monitor">
+			Number of active navigation maps in the [NavigationServer2D]. This also includes the two empty default navigation maps created by World2D.
+		</constant>
+		<constant name="NAVIGATION_2D_REGION_COUNT" value="40" enum="Monitor">
+			Number of active navigation regions in the [NavigationServer2D].
+		</constant>
+		<constant name="NAVIGATION_2D_AGENT_COUNT" value="41" enum="Monitor">
+			Number of active navigation agents processing avoidance in the [NavigationServer2D].
+		</constant>
+		<constant name="NAVIGATION_2D_LINK_COUNT" value="42" enum="Monitor">
+			Number of active navigation links in the [NavigationServer2D].
+		</constant>
+		<constant name="NAVIGATION_2D_POLYGON_COUNT" value="43" enum="Monitor">
+			Number of navigation mesh polygons in the [NavigationServer2D].
+		</constant>
+		<constant name="NAVIGATION_2D_EDGE_COUNT" value="44" enum="Monitor">
+			Number of navigation mesh polygon edges in the [NavigationServer2D].
+		</constant>
+		<constant name="NAVIGATION_2D_EDGE_MERGE_COUNT" value="45" enum="Monitor">
+			Number of navigation mesh polygon edges that were merged due to edge key overlap in the [NavigationServer2D].
+		</constant>
+		<constant name="NAVIGATION_2D_EDGE_CONNECTION_COUNT" value="46" enum="Monitor">
+			Number of polygon edges that are considered connected by edge proximity [NavigationServer2D].
+		</constant>
+		<constant name="NAVIGATION_2D_EDGE_FREE_COUNT" value="47" enum="Monitor">
+			Number of navigation mesh polygon edges that could not be merged in the [NavigationServer2D]. The edges still may be connected by edge proximity or with links.
+		</constant>
+		<constant name="NAVIGATION_2D_OBSTACLE_COUNT" value="48" enum="Monitor">
+			Number of active navigation obstacles in the [NavigationServer2D].
+		</constant>
+		<constant name="NAVIGATION_3D_ACTIVE_MAPS" value="49" enum="Monitor">
+			Number of active navigation maps in the [NavigationServer3D]. This also includes the two empty default navigation maps created by World3D.
+		</constant>
+		<constant name="NAVIGATION_3D_REGION_COUNT" value="50" enum="Monitor">
+			Number of active navigation regions in the [NavigationServer3D].
+		</constant>
+		<constant name="NAVIGATION_3D_AGENT_COUNT" value="51" enum="Monitor">
+			Number of active navigation agents processing avoidance in the [NavigationServer3D].
+		</constant>
+		<constant name="NAVIGATION_3D_LINK_COUNT" value="52" enum="Monitor">
+			Number of active navigation links in the [NavigationServer3D].
+		</constant>
+		<constant name="NAVIGATION_3D_POLYGON_COUNT" value="53" enum="Monitor">
+			Number of navigation mesh polygons in the [NavigationServer3D].
+		</constant>
+		<constant name="NAVIGATION_3D_EDGE_COUNT" value="54" enum="Monitor">
+			Number of navigation mesh polygon edges in the [NavigationServer3D].
+		</constant>
+		<constant name="NAVIGATION_3D_EDGE_MERGE_COUNT" value="55" enum="Monitor">
+			Number of navigation mesh polygon edges that were merged due to edge key overlap in the [NavigationServer3D].
+		</constant>
+		<constant name="NAVIGATION_3D_EDGE_CONNECTION_COUNT" value="56" enum="Monitor">
+			Number of polygon edges that are considered connected by edge proximity [NavigationServer3D].
+		</constant>
+		<constant name="NAVIGATION_3D_EDGE_FREE_COUNT" value="57" enum="Monitor">
+			Number of navigation mesh polygon edges that could not be merged in the [NavigationServer3D]. The edges still may be connected by edge proximity or with links.
+		</constant>
+		<constant name="NAVIGATION_3D_OBSTACLE_COUNT" value="58" enum="Monitor">
+			Number of active navigation obstacles in the [NavigationServer3D].
+		</constant>
+		<constant name="MONITOR_MAX" value="59" enum="Monitor">
 			Represents the size of the [enum Monitor] enum.
 		</constant>
 	</constants>

doc/classes/ProjectSettings.xml

@@ -688,31 +688,58 @@
 		<member name="debug/shader_language/warnings/unused_varying" type="bool" setter="" getter="" default="true">
 			When set to [code]true[/code], produces a warning when a varying is never used.
 		</member>
-		<member name="debug/shapes/avoidance/agents_radius_color" type="Color" setter="" getter="" default="Color(1, 1, 0, 0.25)">
+		<member name="debug/shapes/avoidance/2d/agents_radius_color" type="Color" setter="" getter="" default="Color(1, 1, 0, 0.25)">
 			Color of the avoidance agents radius, visible when "Visible Avoidance" is enabled in the Debug menu.
 		</member>
-		<member name="debug/shapes/avoidance/enable_agents_radius" type="bool" setter="" getter="" default="true">
+		<member name="debug/shapes/avoidance/2d/enable_agents_radius" type="bool" setter="" getter="" default="true">
 			If enabled, displays avoidance agents radius when "Visible Avoidance" is enabled in the Debug menu.
 		</member>
-		<member name="debug/shapes/avoidance/enable_obstacles_radius" type="bool" setter="" getter="" default="true">
+		<member name="debug/shapes/avoidance/2d/enable_obstacles_radius" type="bool" setter="" getter="" default="true">
 			If enabled, displays avoidance obstacles radius when "Visible Avoidance" is enabled in the Debug menu.
 		</member>
-		<member name="debug/shapes/avoidance/enable_obstacles_static" type="bool" setter="" getter="" default="true">
+		<member name="debug/shapes/avoidance/2d/enable_obstacles_static" type="bool" setter="" getter="" default="true">
 			If enabled, displays static avoidance obstacles when "Visible Avoidance" is enabled in the Debug menu.
 		</member>
-		<member name="debug/shapes/avoidance/obstacles_radius_color" type="Color" setter="" getter="" default="Color(1, 0.5, 0, 0.25)">
+		<member name="debug/shapes/avoidance/2d/obstacles_radius_color" type="Color" setter="" getter="" default="Color(1, 0.5, 0, 0.25)">
 			Color of the avoidance obstacles radius, visible when "Visible Avoidance" is enabled in the Debug menu.
 		</member>
-		<member name="debug/shapes/avoidance/obstacles_static_edge_pushin_color" type="Color" setter="" getter="" default="Color(1, 0, 0, 1)">
+		<member name="debug/shapes/avoidance/2d/obstacles_static_edge_pushin_color" type="Color" setter="" getter="" default="Color(1, 0, 0, 1)">
 			Color of the static avoidance obstacles edges when their vertices are winded in order to push agents in, visible when "Visible Avoidance" is enabled in the Debug menu.
 		</member>
-		<member name="debug/shapes/avoidance/obstacles_static_edge_pushout_color" type="Color" setter="" getter="" default="Color(1, 1, 0, 1)">
+		<member name="debug/shapes/avoidance/2d/obstacles_static_edge_pushout_color" type="Color" setter="" getter="" default="Color(1, 1, 0, 1)">
 			Color of the static avoidance obstacles edges when their vertices are winded in order to push agents out, visible when "Visible Avoidance" is enabled in the Debug menu.
 		</member>
-		<member name="debug/shapes/avoidance/obstacles_static_face_pushin_color" type="Color" setter="" getter="" default="Color(1, 0, 0, 0)">
+		<member name="debug/shapes/avoidance/2d/obstacles_static_face_pushin_color" type="Color" setter="" getter="" default="Color(1, 0, 0, 0)">
 			Color of the static avoidance obstacles faces when their vertices are winded in order to push agents in, visible when "Visible Avoidance" is enabled in the Debug menu.
 		</member>
-		<member name="debug/shapes/avoidance/obstacles_static_face_pushout_color" type="Color" setter="" getter="" default="Color(1, 1, 0, 0.5)">
+		<member name="debug/shapes/avoidance/2d/obstacles_static_face_pushout_color" type="Color" setter="" getter="" default="Color(1, 1, 0, 0.5)">
+			Color of the static avoidance obstacles faces when their vertices are winded in order to push agents out, visible when "Visible Avoidance" is enabled in the Debug menu.
+		</member>
+		<member name="debug/shapes/avoidance/3d/agents_radius_color" type="Color" setter="" getter="" default="Color(1, 1, 0, 0.25)">
+			Color of the avoidance agents radius, visible when "Visible Avoidance" is enabled in the Debug menu.
+		</member>
+		<member name="debug/shapes/avoidance/3d/enable_agents_radius" type="bool" setter="" getter="" default="true">
+			If enabled, displays avoidance agents radius when "Visible Avoidance" is enabled in the Debug menu.
+		</member>
+		<member name="debug/shapes/avoidance/3d/enable_obstacles_radius" type="bool" setter="" getter="" default="true">
+			If enabled, displays avoidance obstacles radius when "Visible Avoidance" is enabled in the Debug menu.
+		</member>
+		<member name="debug/shapes/avoidance/3d/enable_obstacles_static" type="bool" setter="" getter="" default="true">
+			If enabled, displays static avoidance obstacles when "Visible Avoidance" is enabled in the Debug menu.
+		</member>
+		<member name="debug/shapes/avoidance/3d/obstacles_radius_color" type="Color" setter="" getter="" default="Color(1, 0.5, 0, 0.25)">
+			Color of the avoidance obstacles radius, visible when "Visible Avoidance" is enabled in the Debug menu.
+		</member>
+		<member name="debug/shapes/avoidance/3d/obstacles_static_edge_pushin_color" type="Color" setter="" getter="" default="Color(1, 0, 0, 1)">
+			Color of the static avoidance obstacles edges when their vertices are winded in order to push agents in, visible when "Visible Avoidance" is enabled in the Debug menu.
+		</member>
+		<member name="debug/shapes/avoidance/3d/obstacles_static_edge_pushout_color" type="Color" setter="" getter="" default="Color(1, 1, 0, 1)">
+			Color of the static avoidance obstacles edges when their vertices are winded in order to push agents out, visible when "Visible Avoidance" is enabled in the Debug menu.
+		</member>
+		<member name="debug/shapes/avoidance/3d/obstacles_static_face_pushin_color" type="Color" setter="" getter="" default="Color(1, 0, 0, 0)">
+			Color of the static avoidance obstacles faces when their vertices are winded in order to push agents in, visible when "Visible Avoidance" is enabled in the Debug menu.
+		</member>
+		<member name="debug/shapes/avoidance/3d/obstacles_static_face_pushout_color" type="Color" setter="" getter="" default="Color(1, 1, 0, 0.5)">
 			Color of the static avoidance obstacles faces when their vertices are winded in order to push agents out, visible when "Visible Avoidance" is enabled in the Debug menu.
 		</member>
 		<member name="debug/shapes/collision/contact_color" type="Color" setter="" getter="" default="Color(1, 0.2, 0.1, 0.8)">
@@ -727,58 +754,100 @@
 		<member name="debug/shapes/collision/shape_color" type="Color" setter="" getter="" default="Color(0, 0.6, 0.7, 0.42)">
 			Color of the collision shapes, visible when "Visible Collision Shapes" is enabled in the Debug menu.
 		</member>
-		<member name="debug/shapes/navigation/agent_path_color" type="Color" setter="" getter="" default="Color(1, 0, 0, 1)">
+		<member name="debug/shapes/navigation/2d/agent_path_color" type="Color" setter="" getter="" default="Color(1, 0, 0, 1)">
+			Color to display enabled navigation agent paths when an agent has debug enabled.
+		</member>
+		<member name="debug/shapes/navigation/2d/agent_path_point_size" type="float" setter="" getter="" default="4.0">
+			Rasterized size (pixel) used to render navigation agent path points when an agent has debug enabled.
+		</member>
+		<member name="debug/shapes/navigation/2d/edge_connection_color" type="Color" setter="" getter="" default="Color(1, 0, 1, 1)">
+			Color to display edge connections between navigation regions, visible when "Visible Navigation" is enabled in the Debug menu.
+		</member>
+		<member name="debug/shapes/navigation/2d/enable_agent_paths" type="bool" setter="" getter="" default="true">
+			If enabled, displays navigation agent paths when an agent has debug enabled.
+		</member>
+		<member name="debug/shapes/navigation/2d/enable_edge_connections" type="bool" setter="" getter="" default="true">
+			If enabled, displays edge connections between navigation regions when "Visible Navigation" is enabled in the Debug menu.
+		</member>
+		<member name="debug/shapes/navigation/2d/enable_edge_lines" type="bool" setter="" getter="" default="true">
+			If enabled, displays navigation mesh polygon edges when "Visible Navigation" is enabled in the Debug menu.
+		</member>
+		<member name="debug/shapes/navigation/2d/enable_geometry_face_random_color" type="bool" setter="" getter="" default="true">
+			If enabled, colorizes each navigation mesh polygon face with a random color when "Visible Navigation" is enabled in the Debug menu.
+		</member>
+		<member name="debug/shapes/navigation/2d/enable_link_connections" type="bool" setter="" getter="" default="true">
+			If enabled, displays navigation link connections when "Visible Navigation" is enabled in the Debug menu.
+		</member>
+		<member name="debug/shapes/navigation/2d/geometry_edge_color" type="Color" setter="" getter="" default="Color(0.5, 1, 1, 1)">
+			Color to display enabled navigation mesh polygon edges, visible when "Visible Navigation" is enabled in the Debug menu.
+		</member>
+		<member name="debug/shapes/navigation/2d/geometry_edge_disabled_color" type="Color" setter="" getter="" default="Color(0.5, 0.5, 0.5, 1)">
+			Color to display disabled navigation mesh polygon edges, visible when "Visible Navigation" is enabled in the Debug menu.
+		</member>
+		<member name="debug/shapes/navigation/2d/geometry_face_color" type="Color" setter="" getter="" default="Color(0.5, 1, 1, 0.4)">
+			Color to display enabled navigation mesh polygon faces, visible when "Visible Navigation" is enabled in the Debug menu.
+		</member>
+		<member name="debug/shapes/navigation/2d/geometry_face_disabled_color" type="Color" setter="" getter="" default="Color(0.5, 0.5, 0.5, 0.4)">
+			Color to display disabled navigation mesh polygon faces, visible when "Visible Navigation" is enabled in the Debug menu.
+		</member>
+		<member name="debug/shapes/navigation/2d/link_connection_color" type="Color" setter="" getter="" default="Color(1, 0.5, 1, 1)">
+			Color to use to display navigation link connections, visible when "Visible Navigation" is enabled in the Debug menu.
+		</member>
+		<member name="debug/shapes/navigation/2d/link_connection_disabled_color" type="Color" setter="" getter="" default="Color(0.5, 0.5, 0.5, 1)">
+			Color to use to display disabled navigation link connections, visible when "Visible Navigation" is enabled in the Debug menu.
+		</member>
+		<member name="debug/shapes/navigation/3d/agent_path_color" type="Color" setter="" getter="" default="Color(1, 0, 0, 1)">
 			Color to display enabled navigation agent paths when an agent has debug enabled.
 		</member>
-		<member name="debug/shapes/navigation/agent_path_point_size" type="float" setter="" getter="" default="4.0">
+		<member name="debug/shapes/navigation/3d/agent_path_point_size" type="float" setter="" getter="" default="4.0">
 			Rasterized size (pixel) used to render navigation agent path points when an agent has debug enabled.
 		</member>
-		<member name="debug/shapes/navigation/edge_connection_color" type="Color" setter="" getter="" default="Color(1, 0, 1, 1)">
+		<member name="debug/shapes/navigation/3d/edge_connection_color" type="Color" setter="" getter="" default="Color(1, 0, 1, 1)">
 			Color to display edge connections between navigation regions, visible when "Visible Navigation" is enabled in the Debug menu.
 		</member>
-		<member name="debug/shapes/navigation/enable_agent_paths" type="bool" setter="" getter="" default="true">
+		<member name="debug/shapes/navigation/3d/enable_agent_paths" type="bool" setter="" getter="" default="true">
 			If enabled, displays navigation agent paths when an agent has debug enabled.
 		</member>
-		<member name="debug/shapes/navigation/enable_agent_paths_xray" type="bool" setter="" getter="" default="true">
+		<member name="debug/shapes/navigation/3d/enable_agent_paths_xray" type="bool" setter="" getter="" default="true">
 			If enabled, displays navigation agent paths through geometry when an agent has debug enabled.
 		</member>
-		<member name="debug/shapes/navigation/enable_edge_connections" type="bool" setter="" getter="" default="true">
+		<member name="debug/shapes/navigation/3d/enable_edge_connections" type="bool" setter="" getter="" default="true">
 			If enabled, displays edge connections between navigation regions when "Visible Navigation" is enabled in the Debug menu.
 		</member>
-		<member name="debug/shapes/navigation/enable_edge_connections_xray" type="bool" setter="" getter="" default="true">
+		<member name="debug/shapes/navigation/3d/enable_edge_connections_xray" type="bool" setter="" getter="" default="true">
 			If enabled, displays edge connections between navigation regions through geometry when "Visible Navigation" is enabled in the Debug menu.
 		</member>
-		<member name="debug/shapes/navigation/enable_edge_lines" type="bool" setter="" getter="" default="true">
+		<member name="debug/shapes/navigation/3d/enable_edge_lines" type="bool" setter="" getter="" default="true">
 			If enabled, displays navigation mesh polygon edges when "Visible Navigation" is enabled in the Debug menu.
 		</member>
-		<member name="debug/shapes/navigation/enable_edge_lines_xray" type="bool" setter="" getter="" default="true">
+		<member name="debug/shapes/navigation/3d/enable_edge_lines_xray" type="bool" setter="" getter="" default="true">
 			If enabled, displays navigation mesh polygon edges through geometry when "Visible Navigation" is enabled in the Debug menu.
 		</member>
-		<member name="debug/shapes/navigation/enable_geometry_face_random_color" type="bool" setter="" getter="" default="true">
+		<member name="debug/shapes/navigation/3d/enable_geometry_face_random_color" type="bool" setter="" getter="" default="true">
 			If enabled, colorizes each navigation mesh polygon face with a random color when "Visible Navigation" is enabled in the Debug menu.
 		</member>
-		<member name="debug/shapes/navigation/enable_link_connections" type="bool" setter="" getter="" default="true">
+		<member name="debug/shapes/navigation/3d/enable_link_connections" type="bool" setter="" getter="" default="true">
 			If enabled, displays navigation link connections when "Visible Navigation" is enabled in the Debug menu.
 		</member>
-		<member name="debug/shapes/navigation/enable_link_connections_xray" type="bool" setter="" getter="" default="true">
+		<member name="debug/shapes/navigation/3d/enable_link_connections_xray" type="bool" setter="" getter="" default="true">
 			If enabled, displays navigation link connections through geometry when "Visible Navigation" is enabled in the Debug menu.
 		</member>
-		<member name="debug/shapes/navigation/geometry_edge_color" type="Color" setter="" getter="" default="Color(0.5, 1, 1, 1)">
+		<member name="debug/shapes/navigation/3d/geometry_edge_color" type="Color" setter="" getter="" default="Color(0.5, 1, 1, 1)">
 			Color to display enabled navigation mesh polygon edges, visible when "Visible Navigation" is enabled in the Debug menu.
 		</member>
-		<member name="debug/shapes/navigation/geometry_edge_disabled_color" type="Color" setter="" getter="" default="Color(0.5, 0.5, 0.5, 1)">
+		<member name="debug/shapes/navigation/3d/geometry_edge_disabled_color" type="Color" setter="" getter="" default="Color(0.5, 0.5, 0.5, 1)">
 			Color to display disabled navigation mesh polygon edges, visible when "Visible Navigation" is enabled in the Debug menu.
 		</member>
-		<member name="debug/shapes/navigation/geometry_face_color" type="Color" setter="" getter="" default="Color(0.5, 1, 1, 0.4)">
+		<member name="debug/shapes/navigation/3d/geometry_face_color" type="Color" setter="" getter="" default="Color(0.5, 1, 1, 0.4)">
 			Color to display enabled navigation mesh polygon faces, visible when "Visible Navigation" is enabled in the Debug menu.
 		</member>
-		<member name="debug/shapes/navigation/geometry_face_disabled_color" type="Color" setter="" getter="" default="Color(0.5, 0.5, 0.5, 0.4)">
+		<member name="debug/shapes/navigation/3d/geometry_face_disabled_color" type="Color" setter="" getter="" default="Color(0.5, 0.5, 0.5, 0.4)">
 			Color to display disabled navigation mesh polygon faces, visible when "Visible Navigation" is enabled in the Debug menu.
 		</member>
-		<member name="debug/shapes/navigation/link_connection_color" type="Color" setter="" getter="" default="Color(1, 0.5, 1, 1)">
+		<member name="debug/shapes/navigation/3d/link_connection_color" type="Color" setter="" getter="" default="Color(1, 0.5, 1, 1)">
 			Color to use to display navigation link connections, visible when "Visible Navigation" is enabled in the Debug menu.
 		</member>
-		<member name="debug/shapes/navigation/link_connection_disabled_color" type="Color" setter="" getter="" default="Color(0.5, 0.5, 0.5, 1)">
+		<member name="debug/shapes/navigation/3d/link_connection_disabled_color" type="Color" setter="" getter="" default="Color(0.5, 0.5, 0.5, 1)">
 			Color to use to display disabled navigation link connections, visible when "Visible Navigation" is enabled in the Debug menu.
 		</member>
 		<member name="debug/shapes/paths/geometry_color" type="Color" setter="" getter="" default="Color(0.1, 1, 0.7, 0.4)">

editor/editor_node.cpp

@@ -68,6 +68,7 @@
 #include "scene/resources/portable_compressed_texture.h"
 #include "scene/theme/theme_db.h"
 #include "servers/display_server.h"
+#include "servers/navigation_server_2d.h"
 #include "servers/navigation_server_3d.h"
 #include "servers/rendering_server.h"
 
@@ -497,16 +498,25 @@ void EditorNode::_update_from_settings() {
 	ResourceImporterTexture::get_singleton()->update_imports();
 
 #ifdef DEBUG_ENABLED
-	NavigationServer3D::get_singleton()->set_debug_navigation_edge_connection_color(GLOBAL_GET("debug/shapes/navigation/edge_connection_color"));
-	NavigationServer3D::get_singleton()->set_debug_navigation_geometry_edge_color(GLOBAL_GET("debug/shapes/navigation/geometry_edge_color"));
-	NavigationServer3D::get_singleton()->set_debug_navigation_geometry_face_color(GLOBAL_GET("debug/shapes/navigation/geometry_face_color"));
-	NavigationServer3D::get_singleton()->set_debug_navigation_geometry_edge_disabled_color(GLOBAL_GET("debug/shapes/navigation/geometry_edge_disabled_color"));
-	NavigationServer3D::get_singleton()->set_debug_navigation_geometry_face_disabled_color(GLOBAL_GET("debug/shapes/navigation/geometry_face_disabled_color"));
-	NavigationServer3D::get_singleton()->set_debug_navigation_enable_edge_connections(GLOBAL_GET("debug/shapes/navigation/enable_edge_connections"));
-	NavigationServer3D::get_singleton()->set_debug_navigation_enable_edge_connections_xray(GLOBAL_GET("debug/shapes/navigation/enable_edge_connections_xray"));
-	NavigationServer3D::get_singleton()->set_debug_navigation_enable_edge_lines(GLOBAL_GET("debug/shapes/navigation/enable_edge_lines"));
-	NavigationServer3D::get_singleton()->set_debug_navigation_enable_edge_lines_xray(GLOBAL_GET("debug/shapes/navigation/enable_edge_lines_xray"));
-	NavigationServer3D::get_singleton()->set_debug_navigation_enable_geometry_face_random_color(GLOBAL_GET("debug/shapes/navigation/enable_geometry_face_random_color"));
+	NavigationServer2D::get_singleton()->set_debug_navigation_edge_connection_color(GLOBAL_GET("debug/shapes/navigation/2d/edge_connection_color"));
+	NavigationServer2D::get_singleton()->set_debug_navigation_geometry_edge_color(GLOBAL_GET("debug/shapes/navigation/2d/geometry_edge_color"));
+	NavigationServer2D::get_singleton()->set_debug_navigation_geometry_face_color(GLOBAL_GET("debug/shapes/navigation/2d/geometry_face_color"));
+	NavigationServer2D::get_singleton()->set_debug_navigation_geometry_edge_disabled_color(GLOBAL_GET("debug/shapes/navigation/2d/geometry_edge_disabled_color"));
+	NavigationServer2D::get_singleton()->set_debug_navigation_geometry_face_disabled_color(GLOBAL_GET("debug/shapes/navigation/2d/geometry_face_disabled_color"));
+	NavigationServer2D::get_singleton()->set_debug_navigation_enable_edge_connections(GLOBAL_GET("debug/shapes/navigation/2d/enable_edge_connections"));
+	NavigationServer2D::get_singleton()->set_debug_navigation_enable_edge_lines(GLOBAL_GET("debug/shapes/navigation/2d/enable_edge_lines"));
+	NavigationServer2D::get_singleton()->set_debug_navigation_enable_geometry_face_random_color(GLOBAL_GET("debug/shapes/navigation/2d/enable_geometry_face_random_color"));
+
+	NavigationServer3D::get_singleton()->set_debug_navigation_edge_connection_color(GLOBAL_GET("debug/shapes/navigation/3d/edge_connection_color"));
+	NavigationServer3D::get_singleton()->set_debug_navigation_geometry_edge_color(GLOBAL_GET("debug/shapes/navigation/3d/geometry_edge_color"));
+	NavigationServer3D::get_singleton()->set_debug_navigation_geometry_face_color(GLOBAL_GET("debug/shapes/navigation/3d/geometry_face_color"));
+	NavigationServer3D::get_singleton()->set_debug_navigation_geometry_edge_disabled_color(GLOBAL_GET("debug/shapes/navigation/3d/geometry_edge_disabled_color"));
+	NavigationServer3D::get_singleton()->set_debug_navigation_geometry_face_disabled_color(GLOBAL_GET("debug/shapes/navigation/3d/geometry_face_disabled_color"));
+	NavigationServer3D::get_singleton()->set_debug_navigation_enable_edge_connections(GLOBAL_GET("debug/shapes/navigation/3d/enable_edge_connections"));
+	NavigationServer3D::get_singleton()->set_debug_navigation_enable_edge_connections_xray(GLOBAL_GET("debug/shapes/navigation/3d/enable_edge_connections_xray"));
+	NavigationServer3D::get_singleton()->set_debug_navigation_enable_edge_lines(GLOBAL_GET("debug/shapes/navigation/3d/enable_edge_lines"));
+	NavigationServer3D::get_singleton()->set_debug_navigation_enable_edge_lines_xray(GLOBAL_GET("debug/shapes/navigation/3d/enable_edge_lines_xray"));
+	NavigationServer3D::get_singleton()->set_debug_navigation_enable_geometry_face_random_color(GLOBAL_GET("debug/shapes/navigation/3d/enable_geometry_face_random_color"));
 #endif // DEBUG_ENABLED
 }
 

main/main.cpp

@@ -754,7 +754,7 @@ Error Main::test_setup() {
 	// Default theme will be initialized later, after modules and ScriptServer are ready.
 	initialize_theme_db();
 
-	NavigationServer3DManager::initialize_server(); // 3D server first because 2D depends on it.
+	NavigationServer3DManager::initialize_server();
 	NavigationServer2DManager::initialize_server();
 
 	register_scene_types();
@@ -839,7 +839,7 @@ void Main::test_cleanup() {
 
 	finalize_theme_db();
 
-	NavigationServer2DManager::finalize_server(); // 2D goes first as it uses the 3D server behind the scene.
+	NavigationServer2DManager::finalize_server();
 	NavigationServer3DManager::finalize_server();
 
 	GDExtensionManager::get_singleton()->deinitialize_extensions(GDExtension::INITIALIZATION_LEVEL_SERVERS);
@@ -3498,7 +3498,7 @@ Error Main::setup2(bool p_show_boot_logo) {
 
 	MAIN_PRINT("Main: Load Navigation");
 
-	NavigationServer3DManager::initialize_server(); // 3D server first because 2D depends on it.
+	NavigationServer3DManager::initialize_server();
 	NavigationServer2DManager::initialize_server();
 
 	register_scene_types();
@@ -4104,12 +4104,16 @@ int Main::start() {
 		}
 		if (debug_navigation) {
 			sml->set_debug_navigation_hint(true);
+			NavigationServer2D::get_singleton()->set_debug_navigation_enabled(true);
 			NavigationServer3D::get_singleton()->set_debug_navigation_enabled(true);
 		}
 		if (debug_avoidance) {
+			NavigationServer2D::get_singleton()->set_debug_avoidance_enabled(true);
 			NavigationServer3D::get_singleton()->set_debug_avoidance_enabled(true);
 		}
 		if (debug_navigation || debug_avoidance) {
+			NavigationServer2D::get_singleton()->set_active(true);
+			NavigationServer2D::get_singleton()->set_debug_enabled(true);
 			NavigationServer3D::get_singleton()->set_active(true);
 			NavigationServer3D::get_singleton()->set_debug_enabled(true);
 		}
@@ -4604,6 +4608,7 @@ bool Main::iteration() {
 
 		uint64_t navigation_begin = OS::get_singleton()->get_ticks_usec();
 
+		NavigationServer2D::get_singleton()->process(physics_step * time_scale);
 		NavigationServer3D::get_singleton()->process(physics_step * time_scale);
 
 		navigation_process_ticks = MAX(navigation_process_ticks, OS::get_singleton()->get_ticks_usec() - navigation_begin); // keep the largest one for reference
@@ -4846,7 +4851,7 @@ void Main::cleanup(bool p_force) {
 	finalize_theme_db();
 
 	// Before deinitializing server extensions, finalize servers which may be loaded as extensions.
-	NavigationServer2DManager::finalize_server(); // 2D goes first as it uses the 3D server behind the scene.
+	NavigationServer2DManager::finalize_server();
 	NavigationServer3DManager::finalize_server();
 	finalize_physics();
 

main/performance.cpp

@@ -35,6 +35,7 @@
 #include "scene/main/node.h"
 #include "scene/main/scene_tree.h"
 #include "servers/audio_server.h"
+#include "servers/navigation_server_2d.h"
 #include "servers/navigation_server_3d.h"
 #include "servers/rendering_server.h"
 
@@ -98,6 +99,26 @@ void Performance::_bind_methods() {
 	BIND_ENUM_CONSTANT(PIPELINE_COMPILATIONS_SURFACE);
 	BIND_ENUM_CONSTANT(PIPELINE_COMPILATIONS_DRAW);
 	BIND_ENUM_CONSTANT(PIPELINE_COMPILATIONS_SPECIALIZATION);
+	BIND_ENUM_CONSTANT(NAVIGATION_2D_ACTIVE_MAPS);
+	BIND_ENUM_CONSTANT(NAVIGATION_2D_REGION_COUNT);
+	BIND_ENUM_CONSTANT(NAVIGATION_2D_AGENT_COUNT);
+	BIND_ENUM_CONSTANT(NAVIGATION_2D_LINK_COUNT);
+	BIND_ENUM_CONSTANT(NAVIGATION_2D_POLYGON_COUNT);
+	BIND_ENUM_CONSTANT(NAVIGATION_2D_EDGE_COUNT);
+	BIND_ENUM_CONSTANT(NAVIGATION_2D_EDGE_MERGE_COUNT);
+	BIND_ENUM_CONSTANT(NAVIGATION_2D_EDGE_CONNECTION_COUNT);
+	BIND_ENUM_CONSTANT(NAVIGATION_2D_EDGE_FREE_COUNT);
+	BIND_ENUM_CONSTANT(NAVIGATION_2D_OBSTACLE_COUNT);
+	BIND_ENUM_CONSTANT(NAVIGATION_3D_ACTIVE_MAPS);
+	BIND_ENUM_CONSTANT(NAVIGATION_3D_REGION_COUNT);
+	BIND_ENUM_CONSTANT(NAVIGATION_3D_AGENT_COUNT);
+	BIND_ENUM_CONSTANT(NAVIGATION_3D_LINK_COUNT);
+	BIND_ENUM_CONSTANT(NAVIGATION_3D_POLYGON_COUNT);
+	BIND_ENUM_CONSTANT(NAVIGATION_3D_EDGE_COUNT);
+	BIND_ENUM_CONSTANT(NAVIGATION_3D_EDGE_MERGE_COUNT);
+	BIND_ENUM_CONSTANT(NAVIGATION_3D_EDGE_CONNECTION_COUNT);
+	BIND_ENUM_CONSTANT(NAVIGATION_3D_EDGE_FREE_COUNT);
+	BIND_ENUM_CONSTANT(NAVIGATION_3D_OBSTACLE_COUNT);
 	BIND_ENUM_CONSTANT(MONITOR_MAX);
 }
 
@@ -152,6 +173,26 @@ String Performance::get_monitor_name(Monitor p_monitor) const {
 		PNAME("pipeline/compilations_surface"),
 		PNAME("pipeline/compilations_draw"),
 		PNAME("pipeline/compilations_specialization"),
+		PNAME("navigation_2d/active_maps"),
+		PNAME("navigation_2d/regions"),
+		PNAME("navigation_2d/agents"),
+		PNAME("navigation_2d/links"),
+		PNAME("navigation_2d/polygons"),
+		PNAME("navigation_2d/edges"),
+		PNAME("navigation_2d/edges_merged"),
+		PNAME("navigation_2d/edges_connected"),
+		PNAME("navigation_2d/edges_free"),
+		PNAME("navigation_2d/obstacles"),
+		PNAME("navigation_2d/active_maps"),
+		PNAME("navigation_3d/regions"),
+		PNAME("navigation_3d/agents"),
+		PNAME("navigation_3d/links"),
+		PNAME("navigation_3d/polygons"),
+		PNAME("navigation_3d/edges"),
+		PNAME("navigation_3d/edges_merged"),
+		PNAME("navigation_3d/edges_connected"),
+		PNAME("navigation_3d/edges_free"),
+		PNAME("navigation_3d/obstacles"),
 	};
 	static_assert(std::size(names) == MONITOR_MAX);
 
@@ -237,25 +278,78 @@ double Performance::get_monitor(Monitor p_monitor) const {
 
 		case AUDIO_OUTPUT_LATENCY:
 			return AudioServer::get_singleton()->get_output_latency();
+
 		case NAVIGATION_ACTIVE_MAPS:
-			return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_ACTIVE_MAPS);
+			return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_ACTIVE_MAPS) +
+					NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_ACTIVE_MAPS);
 		case NAVIGATION_REGION_COUNT:
-			return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_REGION_COUNT);
+			return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_REGION_COUNT) +
+					NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_REGION_COUNT);
 		case NAVIGATION_AGENT_COUNT:
-			return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_AGENT_COUNT);
+			return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_AGENT_COUNT) +
+					NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_AGENT_COUNT);
 		case NAVIGATION_LINK_COUNT:
-			return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_LINK_COUNT);
+			return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_LINK_COUNT) +
+					NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_LINK_COUNT);
 		case NAVIGATION_POLYGON_COUNT:
-			return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_POLYGON_COUNT);
+			return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_POLYGON_COUNT) +
+					NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_POLYGON_COUNT);
 		case NAVIGATION_EDGE_COUNT:
-			return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_EDGE_COUNT);
+			return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_EDGE_COUNT) +
+					NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_EDGE_COUNT);
 		case NAVIGATION_EDGE_MERGE_COUNT:
-			return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_EDGE_MERGE_COUNT);
+			return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_EDGE_MERGE_COUNT) +
+					NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_EDGE_MERGE_COUNT);
 		case NAVIGATION_EDGE_CONNECTION_COUNT:
-			return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_EDGE_CONNECTION_COUNT);
+			return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_EDGE_CONNECTION_COUNT) +
+					NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_EDGE_CONNECTION_COUNT);
 		case NAVIGATION_EDGE_FREE_COUNT:
-			return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_EDGE_FREE_COUNT);
+			return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_EDGE_FREE_COUNT) +
+					NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_EDGE_FREE_COUNT);
 		case NAVIGATION_OBSTACLE_COUNT:
+			return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_OBSTACLE_COUNT) +
+					NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_OBSTACLE_COUNT);
+
+		case NAVIGATION_2D_ACTIVE_MAPS:
+			return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_ACTIVE_MAPS);
+		case NAVIGATION_2D_REGION_COUNT:
+			return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_REGION_COUNT);
+		case NAVIGATION_2D_AGENT_COUNT:
+			return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_AGENT_COUNT);
+		case NAVIGATION_2D_LINK_COUNT:
+			return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_LINK_COUNT);
+		case NAVIGATION_2D_POLYGON_COUNT:
+			return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_POLYGON_COUNT);
+		case NAVIGATION_2D_EDGE_COUNT:
+			return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_EDGE_COUNT);
+		case NAVIGATION_2D_EDGE_MERGE_COUNT:
+			return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_EDGE_MERGE_COUNT);
+		case NAVIGATION_2D_EDGE_CONNECTION_COUNT:
+			return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_EDGE_CONNECTION_COUNT);
+		case NAVIGATION_2D_EDGE_FREE_COUNT:
+			return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_EDGE_FREE_COUNT);
+		case NAVIGATION_2D_OBSTACLE_COUNT:
+			return NavigationServer2D::get_singleton()->get_process_info(NavigationServer2D::INFO_OBSTACLE_COUNT);
+
+		case NAVIGATION_3D_ACTIVE_MAPS:
+			return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_ACTIVE_MAPS);
+		case NAVIGATION_3D_REGION_COUNT:
+			return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_REGION_COUNT);
+		case NAVIGATION_3D_AGENT_COUNT:
+			return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_AGENT_COUNT);
+		case NAVIGATION_3D_LINK_COUNT:
+			return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_LINK_COUNT);
+		case NAVIGATION_3D_POLYGON_COUNT:
+			return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_POLYGON_COUNT);
+		case NAVIGATION_3D_EDGE_COUNT:
+			return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_EDGE_COUNT);
+		case NAVIGATION_3D_EDGE_MERGE_COUNT:
+			return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_EDGE_MERGE_COUNT);
+		case NAVIGATION_3D_EDGE_CONNECTION_COUNT:
+			return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_EDGE_CONNECTION_COUNT);
+		case NAVIGATION_3D_EDGE_FREE_COUNT:
+			return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_EDGE_FREE_COUNT);
+		case NAVIGATION_3D_OBSTACLE_COUNT:
 			return NavigationServer3D::get_singleton()->get_process_info(NavigationServer3D::INFO_OBSTACLE_COUNT);
 
 		default: {
@@ -308,6 +402,26 @@ Performance::MonitorType Performance::get_monitor_type(Monitor p_monitor) const
 		MONITOR_TYPE_QUANTITY,
 		MONITOR_TYPE_QUANTITY,
 		MONITOR_TYPE_QUANTITY,
+		MONITOR_TYPE_QUANTITY,
+		MONITOR_TYPE_QUANTITY,
+		MONITOR_TYPE_QUANTITY,
+		MONITOR_TYPE_QUANTITY,
+		MONITOR_TYPE_QUANTITY,
+		MONITOR_TYPE_QUANTITY,
+		MONITOR_TYPE_QUANTITY,
+		MONITOR_TYPE_QUANTITY,
+		MONITOR_TYPE_QUANTITY,
+		MONITOR_TYPE_QUANTITY,
+		MONITOR_TYPE_QUANTITY,
+		MONITOR_TYPE_QUANTITY,
+		MONITOR_TYPE_QUANTITY,
+		MONITOR_TYPE_QUANTITY,
+		MONITOR_TYPE_QUANTITY,
+		MONITOR_TYPE_QUANTITY,
+		MONITOR_TYPE_QUANTITY,
+		MONITOR_TYPE_QUANTITY,
+		MONITOR_TYPE_QUANTITY,
+		MONITOR_TYPE_QUANTITY,
 
 	};
 	static_assert((sizeof(types) / sizeof(MonitorType)) == MONITOR_MAX);

main/performance.h

@@ -105,6 +105,26 @@ public:
 		PIPELINE_COMPILATIONS_SURFACE,
 		PIPELINE_COMPILATIONS_DRAW,
 		PIPELINE_COMPILATIONS_SPECIALIZATION,
+		NAVIGATION_2D_ACTIVE_MAPS,
+		NAVIGATION_2D_REGION_COUNT,
+		NAVIGATION_2D_AGENT_COUNT,
+		NAVIGATION_2D_LINK_COUNT,
+		NAVIGATION_2D_POLYGON_COUNT,
+		NAVIGATION_2D_EDGE_COUNT,
+		NAVIGATION_2D_EDGE_MERGE_COUNT,
+		NAVIGATION_2D_EDGE_CONNECTION_COUNT,
+		NAVIGATION_2D_EDGE_FREE_COUNT,
+		NAVIGATION_2D_OBSTACLE_COUNT,
+		NAVIGATION_3D_ACTIVE_MAPS,
+		NAVIGATION_3D_REGION_COUNT,
+		NAVIGATION_3D_AGENT_COUNT,
+		NAVIGATION_3D_LINK_COUNT,
+		NAVIGATION_3D_POLYGON_COUNT,
+		NAVIGATION_3D_EDGE_COUNT,
+		NAVIGATION_3D_EDGE_MERGE_COUNT,
+		NAVIGATION_3D_EDGE_CONNECTION_COUNT,
+		NAVIGATION_3D_EDGE_FREE_COUNT,
+		NAVIGATION_3D_OBSTACLE_COUNT,
 		MONITOR_MAX
 	};
 

misc/error_suppressions/tsan.txt

@@ -4,4 +4,5 @@
 deadlock:tests/core/templates/test_command_queue.h
 deadlock:modules/text_server_adv/text_server_adv.cpp
 deadlock:modules/text_server_fb/text_server_fb.cpp
-race:modules/navigation/nav_map_3d.cpp
+race:modules/navigation_2d/nav_map_2d.cpp
+race:modules/navigation_3d/nav_map_3d.cpp

modules/navigation/2d/godot_navigation_server_2d.cpp

@@ -1,552 +0,0 @@
-/**************************************************************************/
-/*  godot_navigation_server_2d.cpp                                        */
-/**************************************************************************/
-/*                         This file is part of:                          */
-/*                             GODOT ENGINE                               */
-/*                        https://godotengine.org                         */
-/**************************************************************************/
-/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
-/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
-/*                                                                        */
-/* Permission is hereby granted, free of charge, to any person obtaining  */
-/* a copy of this software and associated documentation files (the        */
-/* "Software"), to deal in the Software without restriction, including    */
-/* without limitation the rights to use, copy, modify, merge, publish,    */
-/* distribute, sublicense, and/or sell copies of the Software, and to     */
-/* permit persons to whom the Software is furnished to do so, subject to  */
-/* the following conditions:                                              */
-/*                                                                        */
-/* The above copyright notice and this permission notice shall be         */
-/* included in all copies or substantial portions of the Software.        */
-/*                                                                        */
-/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
-/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
-/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
-/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
-/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
-/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
-/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
-/**************************************************************************/
-
-#include "godot_navigation_server_2d.h"
-
-#ifdef CLIPPER2_ENABLED
-#include "nav_mesh_generator_2d.h"
-#endif // CLIPPER2_ENABLED
-
-#include "servers/navigation_server_3d.h"
-
-#define FORWARD_0(FUNC_NAME)                                     \
-	GodotNavigationServer2D::FUNC_NAME() {                       \
-		return NavigationServer3D::get_singleton()->FUNC_NAME(); \
-	}
-
-#define FORWARD_0_C(FUNC_NAME)                                   \
-	GodotNavigationServer2D::FUNC_NAME()                         \
-			const {                                              \
-		return NavigationServer3D::get_singleton()->FUNC_NAME(); \
-	}
-
-#define FORWARD_1(FUNC_NAME, T_0, D_0, CONV_0)                              \
-	GodotNavigationServer2D::FUNC_NAME(T_0 D_0) {                           \
-		return NavigationServer3D::get_singleton()->FUNC_NAME(CONV_0(D_0)); \
-	}
-
-#define FORWARD_1_C(FUNC_NAME, T_0, D_0, CONV_0)                            \
-	GodotNavigationServer2D::FUNC_NAME(T_0 D_0)                             \
-			const {                                                         \
-		return NavigationServer3D::get_singleton()->FUNC_NAME(CONV_0(D_0)); \
-	}
-
-#define FORWARD_1_R_C(CONV_R, FUNC_NAME, T_0, D_0, CONV_0)                          \
-	GodotNavigationServer2D::FUNC_NAME(T_0 D_0)                                     \
-			const {                                                                 \
-		return CONV_R(NavigationServer3D::get_singleton()->FUNC_NAME(CONV_0(D_0))); \
-	}
-
-#define FORWARD_2(FUNC_NAME, T_0, D_0, T_1, D_1, CONV_0, CONV_1)                         \
-	GodotNavigationServer2D::FUNC_NAME(T_0 D_0, T_1 D_1) {                               \
-		return NavigationServer3D::get_singleton()->FUNC_NAME(CONV_0(D_0), CONV_1(D_1)); \
-	}
-
-#define FORWARD_2_C(FUNC_NAME, T_0, D_0, T_1, D_1, CONV_0, CONV_1)                       \
-	GodotNavigationServer2D::FUNC_NAME(T_0 D_0, T_1 D_1)                                 \
-			const {                                                                      \
-		return NavigationServer3D::get_singleton()->FUNC_NAME(CONV_0(D_0), CONV_1(D_1)); \
-	}
-
-#define FORWARD_2_R_C(CONV_R, FUNC_NAME, T_0, D_0, T_1, D_1, CONV_0, CONV_1)                     \
-	GodotNavigationServer2D::FUNC_NAME(T_0 D_0, T_1 D_1)                                         \
-			const {                                                                              \
-		return CONV_R(NavigationServer3D::get_singleton()->FUNC_NAME(CONV_0(D_0), CONV_1(D_1))); \
-	}
-
-static RID rid_to_rid(const RID d) {
-	return d;
-}
-
-static bool bool_to_bool(const bool d) {
-	return d;
-}
-
-static int int_to_int(const int d) {
-	return d;
-}
-
-static uint32_t uint32_to_uint32(const uint32_t d) {
-	return d;
-}
-
-static real_t real_to_real(const real_t d) {
-	return d;
-}
-
-static Vector3 v2_to_v3(const Vector2 d) {
-	return Vector3(d.x, 0.0, d.y);
-}
-
-static Vector2 v3_to_v2(const Vector3 &d) {
-	return Vector2(d.x, d.z);
-}
-
-static Vector<Vector3> vector_v2_to_v3(const Vector<Vector2> &d) {
-	Vector<Vector3> nd;
-	nd.resize(d.size());
-	for (int i(0); i < nd.size(); i++) {
-		nd.write[i] = v2_to_v3(d[i]);
-	}
-	return nd;
-}
-
-static Vector<Vector2> vector_v3_to_v2(const Vector<Vector3> &d) {
-	Vector<Vector2> nd;
-	nd.resize(d.size());
-	for (int i(0); i < nd.size(); i++) {
-		nd.write[i] = v3_to_v2(d[i]);
-	}
-	return nd;
-}
-
-static Transform3D trf2_to_trf3(const Transform2D &d) {
-	Vector3 o(v2_to_v3(d.get_origin()));
-	Basis b;
-	b.rotate(Vector3(0, -1, 0), d.get_rotation());
-	b.scale(v2_to_v3(d.get_scale()));
-	return Transform3D(b, o);
-}
-
-static Transform2D trf3_to_trf2(const Transform3D &d) {
-	Vector3 o = d.get_origin();
-	Vector3 nx = d.xform(Vector3(1, 0, 0)) - o;
-	Vector3 nz = d.xform(Vector3(0, 0, 1)) - o;
-	return Transform2D(nx.x, nx.z, nz.x, nz.z, o.x, o.z);
-}
-
-static ObjectID id_to_id(const ObjectID &id) {
-	return id;
-}
-
-static Callable callable_to_callable(const Callable &c) {
-	return c;
-}
-
-static Ref<NavigationMesh> poly_to_mesh(Ref<NavigationPolygon> d) {
-	if (d.is_valid()) {
-		return d->get_navigation_mesh();
-	} else {
-		return Ref<NavigationMesh>();
-	}
-}
-
-static Rect2 aabb_to_rect2(AABB aabb) {
-	Rect2 rect2;
-	rect2.position = Vector2(aabb.position.x, aabb.position.z);
-	rect2.size = Vector2(aabb.size.x, aabb.size.z);
-	return rect2;
-}
-
-void GodotNavigationServer2D::init() {
-#ifdef CLIPPER2_ENABLED
-	navmesh_generator_2d = memnew(NavMeshGenerator2D);
-	ERR_FAIL_NULL_MSG(navmesh_generator_2d, "Failed to init NavMeshGenerator2D.");
-	RWLockRead read_lock(geometry_parser_rwlock);
-	navmesh_generator_2d->set_generator_parsers(generator_parsers);
-#endif // CLIPPER2_ENABLED
-}
-
-void GodotNavigationServer2D::sync() {
-#ifdef CLIPPER2_ENABLED
-	if (navmesh_generator_2d) {
-		navmesh_generator_2d->sync();
-	}
-#endif // CLIPPER2_ENABLED
-}
-
-void GodotNavigationServer2D::finish() {
-#ifdef CLIPPER2_ENABLED
-	if (navmesh_generator_2d) {
-		navmesh_generator_2d->finish();
-		memdelete(navmesh_generator_2d);
-		navmesh_generator_2d = nullptr;
-	}
-#endif // CLIPPER2_ENABLED
-}
-
-void GodotNavigationServer2D::parse_source_geometry_data(const Ref<NavigationPolygon> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData2D> &p_source_geometry_data, Node *p_root_node, const Callable &p_callback) {
-	ERR_FAIL_COND_MSG(!Thread::is_main_thread(), "The SceneTree can only be parsed on the main thread. Call this function from the main thread or use call_deferred().");
-	ERR_FAIL_COND_MSG(p_navigation_mesh.is_null(), "Invalid navigation polygon.");
-	ERR_FAIL_NULL_MSG(p_root_node, "No parsing root node specified.");
-	ERR_FAIL_COND_MSG(!p_root_node->is_inside_tree(), "The root node needs to be inside the SceneTree.");
-
-#ifdef CLIPPER2_ENABLED
-	ERR_FAIL_NULL(NavMeshGenerator2D::get_singleton());
-	NavMeshGenerator2D::get_singleton()->parse_source_geometry_data(p_navigation_mesh, p_source_geometry_data, p_root_node, p_callback);
-#endif // CLIPPER2_ENABLED
-}
-
-void GodotNavigationServer2D::bake_from_source_geometry_data(const Ref<NavigationPolygon> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData2D> &p_source_geometry_data, const Callable &p_callback) {
-	ERR_FAIL_COND_MSG(p_navigation_mesh.is_null(), "Invalid navigation polygon.");
-	ERR_FAIL_COND_MSG(p_source_geometry_data.is_null(), "Invalid NavigationMeshSourceGeometryData2D.");
-
-#ifdef CLIPPER2_ENABLED
-	ERR_FAIL_NULL(NavMeshGenerator2D::get_singleton());
-	NavMeshGenerator2D::get_singleton()->bake_from_source_geometry_data(p_navigation_mesh, p_source_geometry_data, p_callback);
-#endif // CLIPPER2_ENABLED
-}
-
-void GodotNavigationServer2D::bake_from_source_geometry_data_async(const Ref<NavigationPolygon> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData2D> &p_source_geometry_data, const Callable &p_callback) {
-	ERR_FAIL_COND_MSG(p_navigation_mesh.is_null(), "Invalid navigation mesh.");
-	ERR_FAIL_COND_MSG(p_source_geometry_data.is_null(), "Invalid NavigationMeshSourceGeometryData2D.");
-
-#ifdef CLIPPER2_ENABLED
-	ERR_FAIL_NULL(NavMeshGenerator2D::get_singleton());
-	NavMeshGenerator2D::get_singleton()->bake_from_source_geometry_data_async(p_navigation_mesh, p_source_geometry_data, p_callback);
-#endif // CLIPPER2_ENABLED
-}
-
-bool GodotNavigationServer2D::is_baking_navigation_polygon(Ref<NavigationPolygon> p_navigation_polygon) const {
-#ifdef CLIPPER2_ENABLED
-	return NavMeshGenerator2D::get_singleton()->is_baking(p_navigation_polygon);
-#else
-	return false;
-#endif
-}
-
-Vector<Vector2> GodotNavigationServer2D::simplify_path(const Vector<Vector2> &p_path, real_t p_epsilon) {
-	return vector_v3_to_v2(NavigationServer3D::get_singleton()->simplify_path(vector_v2_to_v3(p_path), p_epsilon));
-}
-
-GodotNavigationServer2D::GodotNavigationServer2D() {}
-
-GodotNavigationServer2D::~GodotNavigationServer2D() {}
-
-TypedArray<RID> FORWARD_0_C(get_maps);
-
-TypedArray<RID> FORWARD_1_C(map_get_links, RID, p_map, rid_to_rid);
-
-TypedArray<RID> FORWARD_1_C(map_get_regions, RID, p_map, rid_to_rid);
-
-TypedArray<RID> FORWARD_1_C(map_get_agents, RID, p_map, rid_to_rid);
-
-TypedArray<RID> FORWARD_1_C(map_get_obstacles, RID, p_map, rid_to_rid);
-
-RID FORWARD_1_C(region_get_map, RID, p_region, rid_to_rid);
-
-RID FORWARD_1_C(agent_get_map, RID, p_agent, rid_to_rid);
-
-RID FORWARD_0(map_create);
-
-void FORWARD_2(map_set_active, RID, p_map, bool, p_active, rid_to_rid, bool_to_bool);
-
-bool FORWARD_1_C(map_is_active, RID, p_map, rid_to_rid);
-
-void GodotNavigationServer2D::map_force_update(RID p_map) {
-	NavigationServer3D::get_singleton()->map_force_update(p_map);
-}
-
-uint32_t GodotNavigationServer2D::map_get_iteration_id(RID p_map) const {
-	return NavigationServer3D::get_singleton()->map_get_iteration_id(p_map);
-}
-
-void GodotNavigationServer2D::map_set_use_async_iterations(RID p_map, bool p_enabled) {
-	return NavigationServer3D::get_singleton()->map_set_use_async_iterations(p_map, p_enabled);
-}
-
-bool GodotNavigationServer2D::map_get_use_async_iterations(RID p_map) const {
-	return NavigationServer3D::get_singleton()->map_get_use_async_iterations(p_map);
-}
-
-void FORWARD_2(map_set_cell_size, RID, p_map, real_t, p_cell_size, rid_to_rid, real_to_real);
-real_t FORWARD_1_C(map_get_cell_size, RID, p_map, rid_to_rid);
-
-void FORWARD_2(map_set_use_edge_connections, RID, p_map, bool, p_enabled, rid_to_rid, bool_to_bool);
-bool FORWARD_1_C(map_get_use_edge_connections, RID, p_map, rid_to_rid);
-
-void FORWARD_2(map_set_edge_connection_margin, RID, p_map, real_t, p_connection_margin, rid_to_rid, real_to_real);
-real_t FORWARD_1_C(map_get_edge_connection_margin, RID, p_map, rid_to_rid);
-
-void FORWARD_2(map_set_link_connection_radius, RID, p_map, real_t, p_connection_radius, rid_to_rid, real_to_real);
-real_t FORWARD_1_C(map_get_link_connection_radius, RID, p_map, rid_to_rid);
-
-Vector<Vector2> GodotNavigationServer2D::map_get_path(RID p_map, Vector2 p_origin, Vector2 p_destination, bool p_optimize, uint32_t p_navigation_layers) {
-	return vector_v3_to_v2(NavigationServer3D::get_singleton()->map_get_path(p_map, v2_to_v3(p_origin), v2_to_v3(p_destination), p_optimize, p_navigation_layers));
-}
-
-Vector2 FORWARD_2_R_C(v3_to_v2, map_get_closest_point, RID, p_map, const Vector2 &, p_point, rid_to_rid, v2_to_v3);
-RID FORWARD_2_C(map_get_closest_point_owner, RID, p_map, const Vector2 &, p_point, rid_to_rid, v2_to_v3);
-
-Vector2 GodotNavigationServer2D::map_get_random_point(RID p_map, uint32_t p_naviation_layers, bool p_uniformly) const {
-	Vector3 result = NavigationServer3D::get_singleton()->map_get_random_point(p_map, p_naviation_layers, p_uniformly);
-	return v3_to_v2(result);
-}
-
-RID FORWARD_0(region_create);
-void FORWARD_2(region_set_enabled, RID, p_region, bool, p_enabled, rid_to_rid, bool_to_bool);
-bool FORWARD_1_C(region_get_enabled, RID, p_region, rid_to_rid);
-void FORWARD_2(region_set_use_edge_connections, RID, p_region, bool, p_enabled, rid_to_rid, bool_to_bool);
-bool FORWARD_1_C(region_get_use_edge_connections, RID, p_region, rid_to_rid);
-
-void FORWARD_2(region_set_enter_cost, RID, p_region, real_t, p_enter_cost, rid_to_rid, real_to_real);
-real_t FORWARD_1_C(region_get_enter_cost, RID, p_region, rid_to_rid);
-void FORWARD_2(region_set_travel_cost, RID, p_region, real_t, p_travel_cost, rid_to_rid, real_to_real);
-real_t FORWARD_1_C(region_get_travel_cost, RID, p_region, rid_to_rid);
-void FORWARD_2(region_set_owner_id, RID, p_region, ObjectID, p_owner_id, rid_to_rid, id_to_id);
-ObjectID FORWARD_1_C(region_get_owner_id, RID, p_region, rid_to_rid);
-bool FORWARD_2_C(region_owns_point, RID, p_region, const Vector2 &, p_point, rid_to_rid, v2_to_v3);
-
-void FORWARD_2(region_set_map, RID, p_region, RID, p_map, rid_to_rid, rid_to_rid);
-void FORWARD_2(region_set_navigation_layers, RID, p_region, uint32_t, p_navigation_layers, rid_to_rid, uint32_to_uint32);
-uint32_t FORWARD_1_C(region_get_navigation_layers, RID, p_region, rid_to_rid);
-void FORWARD_2(region_set_transform, RID, p_region, Transform2D, p_transform, rid_to_rid, trf2_to_trf3);
-
-Transform2D GodotNavigationServer2D::region_get_transform(RID p_region) const {
-	return trf3_to_trf2(NavigationServer3D::get_singleton()->region_get_transform(p_region));
-}
-
-void GodotNavigationServer2D::region_set_navigation_polygon(RID p_region, Ref<NavigationPolygon> p_navigation_polygon) {
-	NavigationServer3D::get_singleton()->region_set_navigation_mesh(p_region, poly_to_mesh(p_navigation_polygon));
-}
-
-int FORWARD_1_C(region_get_connections_count, RID, p_region, rid_to_rid);
-Vector2 FORWARD_2_R_C(v3_to_v2, region_get_connection_pathway_start, RID, p_region, int, p_connection_id, rid_to_rid, int_to_int);
-Vector2 FORWARD_2_R_C(v3_to_v2, region_get_connection_pathway_end, RID, p_region, int, p_connection_id, rid_to_rid, int_to_int);
-
-Vector2 GodotNavigationServer2D::region_get_closest_point(RID p_region, const Vector2 &p_point) const {
-	Vector3 result = NavigationServer3D::get_singleton()->region_get_closest_point(p_region, v2_to_v3(p_point));
-	return v3_to_v2(result);
-}
-
-Vector2 GodotNavigationServer2D::region_get_random_point(RID p_region, uint32_t p_navigation_layers, bool p_uniformly) const {
-	Vector3 result = NavigationServer3D::get_singleton()->region_get_random_point(p_region, p_navigation_layers, p_uniformly);
-	return v3_to_v2(result);
-}
-
-Rect2 GodotNavigationServer2D::region_get_bounds(RID p_region) const {
-	AABB bounds = NavigationServer3D::get_singleton()->region_get_bounds(p_region);
-	return aabb_to_rect2(bounds);
-}
-
-RID FORWARD_0(link_create);
-
-void FORWARD_2(link_set_map, RID, p_link, RID, p_map, rid_to_rid, rid_to_rid);
-RID FORWARD_1_C(link_get_map, RID, p_link, rid_to_rid);
-void FORWARD_2(link_set_enabled, RID, p_link, bool, p_enabled, rid_to_rid, bool_to_bool);
-bool FORWARD_1_C(link_get_enabled, RID, p_link, rid_to_rid);
-void FORWARD_2(link_set_bidirectional, RID, p_link, bool, p_bidirectional, rid_to_rid, bool_to_bool);
-bool FORWARD_1_C(link_is_bidirectional, RID, p_link, rid_to_rid);
-void FORWARD_2(link_set_navigation_layers, RID, p_link, uint32_t, p_navigation_layers, rid_to_rid, uint32_to_uint32);
-uint32_t FORWARD_1_C(link_get_navigation_layers, RID, p_link, rid_to_rid);
-void FORWARD_2(link_set_start_position, RID, p_link, Vector2, p_position, rid_to_rid, v2_to_v3);
-Vector2 FORWARD_1_R_C(v3_to_v2, link_get_start_position, RID, p_link, rid_to_rid);
-void FORWARD_2(link_set_end_position, RID, p_link, Vector2, p_position, rid_to_rid, v2_to_v3);
-Vector2 FORWARD_1_R_C(v3_to_v2, link_get_end_position, RID, p_link, rid_to_rid);
-void FORWARD_2(link_set_enter_cost, RID, p_link, real_t, p_enter_cost, rid_to_rid, real_to_real);
-real_t FORWARD_1_C(link_get_enter_cost, RID, p_link, rid_to_rid);
-void FORWARD_2(link_set_travel_cost, RID, p_link, real_t, p_travel_cost, rid_to_rid, real_to_real);
-real_t FORWARD_1_C(link_get_travel_cost, RID, p_link, rid_to_rid);
-void FORWARD_2(link_set_owner_id, RID, p_link, ObjectID, p_owner_id, rid_to_rid, id_to_id);
-ObjectID FORWARD_1_C(link_get_owner_id, RID, p_link, rid_to_rid);
-
-RID GodotNavigationServer2D::agent_create() {
-	RID agent = NavigationServer3D::get_singleton()->agent_create();
-	return agent;
-}
-
-void FORWARD_2(agent_set_avoidance_enabled, RID, p_agent, bool, p_enabled, rid_to_rid, bool_to_bool);
-bool FORWARD_1_C(agent_get_avoidance_enabled, RID, p_agent, rid_to_rid);
-void FORWARD_2(agent_set_map, RID, p_agent, RID, p_map, rid_to_rid, rid_to_rid);
-void FORWARD_2(agent_set_neighbor_distance, RID, p_agent, real_t, p_dist, rid_to_rid, real_to_real);
-real_t GodotNavigationServer2D::agent_get_neighbor_distance(RID p_agent) const {
-	return NavigationServer3D::get_singleton()->agent_get_neighbor_distance(p_agent);
-}
-void FORWARD_2(agent_set_max_neighbors, RID, p_agent, int, p_count, rid_to_rid, int_to_int);
-int GodotNavigationServer2D::agent_get_max_neighbors(RID p_agent) const {
-	return NavigationServer3D::get_singleton()->agent_get_max_neighbors(p_agent);
-}
-void FORWARD_2(agent_set_time_horizon_agents, RID, p_agent, real_t, p_time_horizon, rid_to_rid, real_to_real);
-real_t GodotNavigationServer2D::agent_get_time_horizon_agents(RID p_agent) const {
-	return NavigationServer3D::get_singleton()->agent_get_time_horizon_agents(p_agent);
-}
-void FORWARD_2(agent_set_time_horizon_obstacles, RID, p_agent, real_t, p_time_horizon, rid_to_rid, real_to_real);
-real_t GodotNavigationServer2D::agent_get_time_horizon_obstacles(RID p_agent) const {
-	return NavigationServer3D::get_singleton()->agent_get_time_horizon_obstacles(p_agent);
-}
-void FORWARD_2(agent_set_radius, RID, p_agent, real_t, p_radius, rid_to_rid, real_to_real);
-real_t GodotNavigationServer2D::agent_get_radius(RID p_agent) const {
-	return NavigationServer3D::get_singleton()->agent_get_radius(p_agent);
-}
-void FORWARD_2(agent_set_max_speed, RID, p_agent, real_t, p_max_speed, rid_to_rid, real_to_real);
-real_t GodotNavigationServer2D::agent_get_max_speed(RID p_agent) const {
-	return NavigationServer3D::get_singleton()->agent_get_max_speed(p_agent);
-}
-void FORWARD_2(agent_set_velocity_forced, RID, p_agent, Vector2, p_velocity, rid_to_rid, v2_to_v3);
-void FORWARD_2(agent_set_velocity, RID, p_agent, Vector2, p_velocity, rid_to_rid, v2_to_v3);
-Vector2 GodotNavigationServer2D::agent_get_velocity(RID p_agent) const {
-	return v3_to_v2(NavigationServer3D::get_singleton()->agent_get_velocity(p_agent));
-}
-void FORWARD_2(agent_set_position, RID, p_agent, Vector2, p_position, rid_to_rid, v2_to_v3);
-Vector2 GodotNavigationServer2D::agent_get_position(RID p_agent) const {
-	return v3_to_v2(NavigationServer3D::get_singleton()->agent_get_position(p_agent));
-}
-bool FORWARD_1_C(agent_is_map_changed, RID, p_agent, rid_to_rid);
-void FORWARD_2(agent_set_paused, RID, p_agent, bool, p_paused, rid_to_rid, bool_to_bool);
-bool FORWARD_1_C(agent_get_paused, RID, p_agent, rid_to_rid);
-
-void GodotNavigationServer2D::free(RID p_object) {
-	if (geometry_parser_owner.owns(p_object)) {
-		RWLockWrite write_lock(geometry_parser_rwlock);
-
-		NavMeshGeometryParser2D *parser = geometry_parser_owner.get_or_null(p_object);
-		ERR_FAIL_NULL(parser);
-
-		generator_parsers.erase(parser);
-#ifndef CLIPPER2_ENABLED
-		NavMeshGenerator2D::get_singleton()->set_generator_parsers(generator_parsers);
-#endif
-		geometry_parser_owner.free(parser->self);
-		return;
-	}
-	NavigationServer3D::get_singleton()->free(p_object);
-}
-
-void FORWARD_2(agent_set_avoidance_callback, RID, p_agent, Callable, p_callback, rid_to_rid, callable_to_callable);
-bool GodotNavigationServer2D::agent_has_avoidance_callback(RID p_agent) const {
-	return NavigationServer3D::get_singleton()->agent_has_avoidance_callback(p_agent);
-}
-
-void FORWARD_2(agent_set_avoidance_layers, RID, p_agent, uint32_t, p_layers, rid_to_rid, uint32_to_uint32);
-uint32_t GodotNavigationServer2D::agent_get_avoidance_layers(RID p_agent) const {
-	return NavigationServer3D::get_singleton()->agent_get_avoidance_layers(p_agent);
-}
-void FORWARD_2(agent_set_avoidance_mask, RID, p_agent, uint32_t, p_mask, rid_to_rid, uint32_to_uint32);
-uint32_t GodotNavigationServer2D::agent_get_avoidance_mask(RID p_agent) const {
-	return NavigationServer3D::get_singleton()->agent_get_avoidance_mask(p_agent);
-}
-void FORWARD_2(agent_set_avoidance_priority, RID, p_agent, real_t, p_priority, rid_to_rid, real_to_real);
-real_t GodotNavigationServer2D::agent_get_avoidance_priority(RID p_agent) const {
-	return NavigationServer3D::get_singleton()->agent_get_avoidance_priority(p_agent);
-}
-
-RID GodotNavigationServer2D::obstacle_create() {
-	RID obstacle = NavigationServer3D::get_singleton()->obstacle_create();
-	return obstacle;
-}
-void FORWARD_2(obstacle_set_avoidance_enabled, RID, p_obstacle, bool, p_enabled, rid_to_rid, bool_to_bool);
-bool FORWARD_1_C(obstacle_get_avoidance_enabled, RID, p_obstacle, rid_to_rid);
-void FORWARD_2(obstacle_set_map, RID, p_obstacle, RID, p_map, rid_to_rid, rid_to_rid);
-RID FORWARD_1_C(obstacle_get_map, RID, p_obstacle, rid_to_rid);
-void FORWARD_2(obstacle_set_paused, RID, p_obstacle, bool, p_paused, rid_to_rid, bool_to_bool);
-bool FORWARD_1_C(obstacle_get_paused, RID, p_obstacle, rid_to_rid);
-void FORWARD_2(obstacle_set_radius, RID, p_obstacle, real_t, p_radius, rid_to_rid, real_to_real);
-real_t GodotNavigationServer2D::obstacle_get_radius(RID p_obstacle) const {
-	return NavigationServer3D::get_singleton()->obstacle_get_radius(p_obstacle);
-}
-void FORWARD_2(obstacle_set_velocity, RID, p_obstacle, Vector2, p_velocity, rid_to_rid, v2_to_v3);
-Vector2 GodotNavigationServer2D::obstacle_get_velocity(RID p_obstacle) const {
-	return v3_to_v2(NavigationServer3D::get_singleton()->obstacle_get_velocity(p_obstacle));
-}
-void FORWARD_2(obstacle_set_position, RID, p_obstacle, Vector2, p_position, rid_to_rid, v2_to_v3);
-Vector2 GodotNavigationServer2D::obstacle_get_position(RID p_obstacle) const {
-	return v3_to_v2(NavigationServer3D::get_singleton()->obstacle_get_position(p_obstacle));
-}
-void FORWARD_2(obstacle_set_avoidance_layers, RID, p_obstacle, uint32_t, p_layers, rid_to_rid, uint32_to_uint32);
-uint32_t GodotNavigationServer2D::obstacle_get_avoidance_layers(RID p_obstacle) const {
-	return NavigationServer3D::get_singleton()->obstacle_get_avoidance_layers(p_obstacle);
-}
-
-void GodotNavigationServer2D::obstacle_set_vertices(RID p_obstacle, const Vector<Vector2> &p_vertices) {
-	NavigationServer3D::get_singleton()->obstacle_set_vertices(p_obstacle, vector_v2_to_v3(p_vertices));
-}
-Vector<Vector2> GodotNavigationServer2D::obstacle_get_vertices(RID p_obstacle) const {
-	return vector_v3_to_v2(NavigationServer3D::get_singleton()->obstacle_get_vertices(p_obstacle));
-}
-
-void GodotNavigationServer2D::query_path(const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result, const Callable &p_callback) {
-	ERR_FAIL_COND(p_query_parameters.is_null());
-	ERR_FAIL_COND(p_query_result.is_null());
-
-	Ref<NavigationPathQueryParameters3D> query_parameters;
-	query_parameters.instantiate();
-
-	query_parameters->set_map(p_query_parameters->get_map());
-	query_parameters->set_start_position(v2_to_v3(p_query_parameters->get_start_position()));
-	query_parameters->set_target_position(v2_to_v3(p_query_parameters->get_target_position()));
-	query_parameters->set_navigation_layers(p_query_parameters->get_navigation_layers());
-	query_parameters->set_pathfinding_algorithm(NavigationPathQueryParameters3D::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR);
-
-	switch (p_query_parameters->get_path_postprocessing()) {
-		case NavigationPathQueryParameters2D::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL: {
-			query_parameters->set_path_postprocessing(NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL);
-		} break;
-		case NavigationPathQueryParameters2D::PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED: {
-			query_parameters->set_path_postprocessing(NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED);
-		} break;
-		case NavigationPathQueryParameters2D::PathPostProcessing::PATH_POSTPROCESSING_NONE: {
-			query_parameters->set_path_postprocessing(NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_NONE);
-		} break;
-		default: {
-			WARN_PRINT("No match for used PathPostProcessing - fallback to default");
-			query_parameters->set_path_postprocessing(NavigationPathQueryParameters3D::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL);
-		} break;
-	}
-
-	query_parameters->set_metadata_flags((int64_t)p_query_parameters->get_metadata_flags());
-	query_parameters->set_simplify_path(p_query_parameters->get_simplify_path());
-	query_parameters->set_simplify_epsilon(p_query_parameters->get_simplify_epsilon());
-	query_parameters->set_excluded_regions(p_query_parameters->get_excluded_regions());
-	query_parameters->set_included_regions(p_query_parameters->get_included_regions());
-
-	Ref<NavigationPathQueryResult3D> query_result;
-	query_result.instantiate();
-
-	NavigationServer3D::get_singleton()->query_path(query_parameters, query_result, p_callback);
-
-	p_query_result->set_path(vector_v3_to_v2(query_result->get_path()));
-	p_query_result->set_path_types(query_result->get_path_types());
-	p_query_result->set_path_rids(query_result->get_path_rids());
-	p_query_result->set_path_owner_ids(query_result->get_path_owner_ids());
-}
-
-RID GodotNavigationServer2D::source_geometry_parser_create() {
-	RWLockWrite write_lock(geometry_parser_rwlock);
-
-	RID rid = geometry_parser_owner.make_rid();
-
-	NavMeshGeometryParser2D *parser = geometry_parser_owner.get_or_null(rid);
-	parser->self = rid;
-
-	generator_parsers.push_back(parser);
-#ifdef CLIPPER2_ENABLED
-	NavMeshGenerator2D::get_singleton()->set_generator_parsers(generator_parsers);
-#endif
-	return rid;
-}
-
-void GodotNavigationServer2D::source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) {
-	RWLockWrite write_lock(geometry_parser_rwlock);
-
-	NavMeshGeometryParser2D *parser = geometry_parser_owner.get_or_null(p_parser);
-	ERR_FAIL_NULL(parser);
-
-	parser->callback = p_callback;
-}

modules/navigation_2d/2d/godot_navigation_server_2d.cpp

@@ -0,0 +1,1394 @@
+/**************************************************************************/
+/*  godot_navigation_server_2d.cpp                                        */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "godot_navigation_server_2d.h"
+
+#include "core/os/mutex.h"
+#include "scene/main/node.h"
+
+#ifdef CLIPPER2_ENABLED
+#include "nav_mesh_generator_2d.h"
+#endif // CLIPPER2_ENABLED
+
+#define COMMAND_1(F_NAME, T_0, D_0)                                     \
+	struct MERGE(F_NAME, _command_2d) : public SetCommand2D {           \
+		T_0 d_0;                                                        \
+		MERGE(F_NAME, _command_2d)                                      \
+		(T_0 p_d_0) :                                                   \
+				d_0(p_d_0) {}                                           \
+		virtual void exec(GodotNavigationServer2D *p_server) override { \
+			p_server->MERGE(_cmd_, F_NAME)(d_0);                        \
+		}                                                               \
+	};                                                                  \
+	void GodotNavigationServer2D::F_NAME(T_0 D_0) {                     \
+		auto cmd = memnew(MERGE(F_NAME, _command_2d)(                   \
+				D_0));                                                  \
+		add_command(cmd);                                               \
+	}                                                                   \
+	void GodotNavigationServer2D::MERGE(_cmd_, F_NAME)(T_0 D_0)
+
+#define COMMAND_2(F_NAME, T_0, D_0, T_1, D_1)                           \
+	struct MERGE(F_NAME, _command_2d) : public SetCommand2D {           \
+		T_0 d_0;                                                        \
+		T_1 d_1;                                                        \
+		MERGE(F_NAME, _command_2d)                                      \
+		(                                                               \
+				T_0 p_d_0,                                              \
+				T_1 p_d_1) :                                            \
+				d_0(p_d_0),                                             \
+				d_1(p_d_1) {}                                           \
+		virtual void exec(GodotNavigationServer2D *p_server) override { \
+			p_server->MERGE(_cmd_, F_NAME)(d_0, d_1);                   \
+		}                                                               \
+	};                                                                  \
+	void GodotNavigationServer2D::F_NAME(T_0 D_0, T_1 D_1) {            \
+		auto cmd = memnew(MERGE(F_NAME, _command_2d)(                   \
+				D_0,                                                    \
+				D_1));                                                  \
+		add_command(cmd);                                               \
+	}                                                                   \
+	void GodotNavigationServer2D::MERGE(_cmd_, F_NAME)(T_0 D_0, T_1 D_1)
+
+void GodotNavigationServer2D::init() {
+#ifdef CLIPPER2_ENABLED
+	navmesh_generator_2d = memnew(NavMeshGenerator2D);
+	ERR_FAIL_NULL_MSG(navmesh_generator_2d, "Failed to init NavMeshGenerator2D.");
+	RWLockRead read_lock(geometry_parser_rwlock);
+	navmesh_generator_2d->set_generator_parsers(generator_parsers);
+#endif // CLIPPER2_ENABLED
+	// TODO
+}
+
+void GodotNavigationServer2D::sync() {
+#ifdef CLIPPER2_ENABLED
+	if (navmesh_generator_2d) {
+		navmesh_generator_2d->sync();
+	}
+#endif // CLIPPER2_ENABLED
+	// TODO
+}
+
+void GodotNavigationServer2D::finish() {
+#ifdef CLIPPER2_ENABLED
+	if (navmesh_generator_2d) {
+		navmesh_generator_2d->finish();
+		memdelete(navmesh_generator_2d);
+		navmesh_generator_2d = nullptr;
+	}
+#endif // CLIPPER2_ENABLED
+	// TODO
+}
+
+void GodotNavigationServer2D::parse_source_geometry_data(const Ref<NavigationPolygon> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData2D> &p_source_geometry_data, Node *p_root_node, const Callable &p_callback) {
+	ERR_FAIL_COND_MSG(!Thread::is_main_thread(), "The SceneTree can only be parsed on the main thread. Call this function from the main thread or use call_deferred().");
+	ERR_FAIL_COND_MSG(p_navigation_mesh.is_null(), "Invalid navigation polygon.");
+	ERR_FAIL_NULL_MSG(p_root_node, "No parsing root node specified.");
+	ERR_FAIL_COND_MSG(!p_root_node->is_inside_tree(), "The root node needs to be inside the SceneTree.");
+
+#ifdef CLIPPER2_ENABLED
+	ERR_FAIL_NULL(NavMeshGenerator2D::get_singleton());
+	NavMeshGenerator2D::get_singleton()->parse_source_geometry_data(p_navigation_mesh, p_source_geometry_data, p_root_node, p_callback);
+#endif // CLIPPER2_ENABLED
+}
+
+void GodotNavigationServer2D::bake_from_source_geometry_data(const Ref<NavigationPolygon> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData2D> &p_source_geometry_data, const Callable &p_callback) {
+	ERR_FAIL_COND_MSG(p_navigation_mesh.is_null(), "Invalid navigation polygon.");
+	ERR_FAIL_COND_MSG(p_source_geometry_data.is_null(), "Invalid NavigationMeshSourceGeometryData2D.");
+
+#ifdef CLIPPER2_ENABLED
+	ERR_FAIL_NULL(NavMeshGenerator2D::get_singleton());
+	NavMeshGenerator2D::get_singleton()->bake_from_source_geometry_data(p_navigation_mesh, p_source_geometry_data, p_callback);
+#endif // CLIPPER2_ENABLED
+}
+
+void GodotNavigationServer2D::bake_from_source_geometry_data_async(const Ref<NavigationPolygon> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData2D> &p_source_geometry_data, const Callable &p_callback) {
+	ERR_FAIL_COND_MSG(p_navigation_mesh.is_null(), "Invalid navigation mesh.");
+	ERR_FAIL_COND_MSG(p_source_geometry_data.is_null(), "Invalid NavigationMeshSourceGeometryData2D.");
+
+#ifdef CLIPPER2_ENABLED
+	ERR_FAIL_NULL(NavMeshGenerator2D::get_singleton());
+	NavMeshGenerator2D::get_singleton()->bake_from_source_geometry_data_async(p_navigation_mesh, p_source_geometry_data, p_callback);
+#endif // CLIPPER2_ENABLED
+}
+
+bool GodotNavigationServer2D::is_baking_navigation_polygon(Ref<NavigationPolygon> p_navigation_polygon) const {
+#ifdef CLIPPER2_ENABLED
+	return NavMeshGenerator2D::get_singleton()->is_baking(p_navigation_polygon);
+#else
+	return false;
+#endif
+}
+
+Vector<Vector2> GodotNavigationServer2D::simplify_path(const Vector<Vector2> &p_path, real_t p_epsilon) {
+	if (p_path.size() <= 2) {
+		return p_path;
+	}
+
+	p_epsilon = MAX(0.0, p_epsilon);
+
+	LocalVector<Vector2> source_path;
+	{
+		source_path.resize(p_path.size());
+		const Vector2 *r = p_path.ptr();
+		for (uint32_t i = 0; i < p_path.size(); i++) {
+			source_path[i] = r[i];
+		}
+	}
+
+	LocalVector<uint32_t> simplified_path_indices = NavMeshQueries2D::get_simplified_path_indices(source_path, p_epsilon);
+
+	uint32_t index_count = simplified_path_indices.size();
+
+	Vector<Vector2> simplified_path;
+	{
+		simplified_path.resize(index_count);
+		Vector2 *w = simplified_path.ptrw();
+		const Vector2 *r = source_path.ptr();
+		for (uint32_t i = 0; i < index_count; i++) {
+			w[i] = r[simplified_path_indices[i]];
+		}
+	}
+
+	return simplified_path;
+}
+
+GodotNavigationServer2D::GodotNavigationServer2D() {}
+
+GodotNavigationServer2D::~GodotNavigationServer2D() {
+	flush_queries();
+}
+
+void GodotNavigationServer2D::add_command(SetCommand2D *p_command) {
+	MutexLock lock(commands_mutex);
+
+	commands.push_back(p_command);
+}
+
+TypedArray<RID> GodotNavigationServer2D::get_maps() const {
+	TypedArray<RID> all_map_rids;
+	List<RID> maps_owned;
+	map_owner.get_owned_list(&maps_owned);
+	if (maps_owned.size()) {
+		for (const RID &E : maps_owned) {
+			all_map_rids.push_back(E);
+		}
+	}
+	return all_map_rids;
+}
+
+TypedArray<RID> GodotNavigationServer2D::map_get_links(RID p_map) const {
+	TypedArray<RID> link_rids;
+	const NavMap2D *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_NULL_V(map, link_rids);
+
+	const LocalVector<NavLink2D *> &links = map->get_links();
+	link_rids.resize(links.size());
+
+	for (uint32_t i = 0; i < links.size(); i++) {
+		link_rids[i] = links[i]->get_self();
+	}
+	return link_rids;
+}
+
+TypedArray<RID> GodotNavigationServer2D::map_get_regions(RID p_map) const {
+	TypedArray<RID> regions_rids;
+	const NavMap2D *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_NULL_V(map, regions_rids);
+
+	const LocalVector<NavRegion2D *> &regions = map->get_regions();
+	regions_rids.resize(regions.size());
+
+	for (uint32_t i = 0; i < regions.size(); i++) {
+		regions_rids[i] = regions[i]->get_self();
+	}
+	return regions_rids;
+}
+
+TypedArray<RID> GodotNavigationServer2D::map_get_agents(RID p_map) const {
+	TypedArray<RID> agents_rids;
+	const NavMap2D *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_NULL_V(map, agents_rids);
+
+	const LocalVector<NavAgent2D *> &agents = map->get_agents();
+	agents_rids.resize(agents.size());
+
+	for (uint32_t i = 0; i < agents.size(); i++) {
+		agents_rids[i] = agents[i]->get_self();
+	}
+	return agents_rids;
+}
+
+TypedArray<RID> GodotNavigationServer2D::map_get_obstacles(RID p_map) const {
+	TypedArray<RID> obstacles_rids;
+	const NavMap2D *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_NULL_V(map, obstacles_rids);
+	const LocalVector<NavObstacle2D *> obstacles = map->get_obstacles();
+	obstacles_rids.resize(obstacles.size());
+	for (uint32_t i = 0; i < obstacles.size(); i++) {
+		obstacles_rids[i] = obstacles[i]->get_self();
+	}
+	return obstacles_rids;
+}
+
+RID GodotNavigationServer2D::region_get_map(RID p_region) const {
+	NavRegion2D *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_NULL_V(region, RID());
+
+	if (region->get_map()) {
+		return region->get_map()->get_self();
+	}
+	return RID();
+}
+
+RID GodotNavigationServer2D::agent_get_map(RID p_agent) const {
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL_V(agent, RID());
+
+	if (agent->get_map()) {
+		return agent->get_map()->get_self();
+	}
+	return RID();
+}
+
+RID GodotNavigationServer2D::map_create() {
+	MutexLock lock(operations_mutex);
+
+	RID rid = map_owner.make_rid();
+	NavMap2D *map = map_owner.get_or_null(rid);
+	map->set_self(rid);
+	return rid;
+}
+
+COMMAND_2(map_set_active, RID, p_map, bool, p_active) {
+	NavMap2D *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_NULL(map);
+
+	if (p_active) {
+		if (!map_is_active(p_map)) {
+			active_maps.push_back(map);
+			active_maps_iteration_id.push_back(map->get_iteration_id());
+		}
+	} else {
+		int map_index = active_maps.find(map);
+		ERR_FAIL_COND(map_index < 0);
+		active_maps.remove_at(map_index);
+		active_maps_iteration_id.remove_at(map_index);
+	}
+}
+
+bool GodotNavigationServer2D::map_is_active(RID p_map) const {
+	NavMap2D *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_NULL_V(map, false);
+
+	return active_maps.has(map);
+}
+
+void GodotNavigationServer2D::map_force_update(RID p_map) {
+	NavMap2D *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_NULL(map);
+
+	flush_queries();
+
+	map->sync();
+}
+
+uint32_t GodotNavigationServer2D::map_get_iteration_id(RID p_map) const {
+	NavMap2D *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_NULL_V(map, 0);
+
+	return map->get_iteration_id();
+}
+
+COMMAND_2(map_set_use_async_iterations, RID, p_map, bool, p_enabled) {
+	NavMap2D *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_NULL(map);
+	map->set_use_async_iterations(p_enabled);
+}
+
+bool GodotNavigationServer2D::map_get_use_async_iterations(RID p_map) const {
+	const NavMap2D *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_NULL_V(map, false);
+
+	return map->get_use_async_iterations();
+}
+
+COMMAND_2(map_set_cell_size, RID, p_map, real_t, p_cell_size) {
+	NavMap2D *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_NULL(map);
+
+	map->set_cell_size(p_cell_size);
+}
+
+real_t GodotNavigationServer2D::map_get_cell_size(RID p_map) const {
+	const NavMap2D *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_NULL_V(map, 0);
+
+	return map->get_cell_size();
+}
+
+COMMAND_2(map_set_use_edge_connections, RID, p_map, bool, p_enabled) {
+	NavMap2D *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_NULL(map);
+
+	map->set_use_edge_connections(p_enabled);
+}
+
+bool GodotNavigationServer2D::map_get_use_edge_connections(RID p_map) const {
+	NavMap2D *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_NULL_V(map, false);
+
+	return map->get_use_edge_connections();
+}
+
+COMMAND_2(map_set_edge_connection_margin, RID, p_map, real_t, p_connection_margin) {
+	NavMap2D *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_NULL(map);
+
+	map->set_edge_connection_margin(p_connection_margin);
+}
+
+real_t GodotNavigationServer2D::map_get_edge_connection_margin(RID p_map) const {
+	const NavMap2D *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_NULL_V(map, 0);
+
+	return map->get_edge_connection_margin();
+}
+
+COMMAND_2(map_set_link_connection_radius, RID, p_map, real_t, p_connection_radius) {
+	NavMap2D *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_NULL(map);
+
+	map->set_link_connection_radius(p_connection_radius);
+}
+
+real_t GodotNavigationServer2D::map_get_link_connection_radius(RID p_map) const {
+	const NavMap2D *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_NULL_V(map, 0);
+
+	return map->get_link_connection_radius();
+}
+
+Vector<Vector2> GodotNavigationServer2D::map_get_path(RID p_map, Vector2 p_origin, Vector2 p_destination, bool p_optimize, uint32_t p_navigation_layers) {
+	const NavMap2D *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_NULL_V(map, Vector<Vector2>());
+
+	Ref<NavigationPathQueryParameters2D> query_parameters;
+	query_parameters.instantiate();
+
+	query_parameters->set_map(p_map);
+	query_parameters->set_start_position(p_origin);
+	query_parameters->set_target_position(p_destination);
+	query_parameters->set_navigation_layers(p_navigation_layers);
+	query_parameters->set_pathfinding_algorithm(NavigationPathQueryParameters2D::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR);
+	query_parameters->set_path_postprocessing(NavigationPathQueryParameters2D::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL);
+	if (!p_optimize) {
+		query_parameters->set_path_postprocessing(NavigationPathQueryParameters2D::PATH_POSTPROCESSING_EDGECENTERED);
+	}
+
+	Ref<NavigationPathQueryResult2D> query_result;
+	query_result.instantiate();
+
+	query_path(query_parameters, query_result);
+
+	return query_result->get_path();
+}
+
+Vector2 GodotNavigationServer2D::map_get_closest_point(RID p_map, const Vector2 &p_point) const {
+	const NavMap2D *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_NULL_V(map, Vector2());
+
+	return map->get_closest_point(p_point);
+}
+
+RID GodotNavigationServer2D::map_get_closest_point_owner(RID p_map, const Vector2 &p_point) const {
+	const NavMap2D *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_NULL_V(map, RID());
+
+	return map->get_closest_point_owner(p_point);
+}
+
+Vector2 GodotNavigationServer2D::map_get_random_point(RID p_map, uint32_t p_navigation_layers, bool p_uniformly) const {
+	const NavMap2D *map = map_owner.get_or_null(p_map);
+	ERR_FAIL_NULL_V(map, Vector2());
+
+	return map->get_random_point(p_navigation_layers, p_uniformly);
+}
+
+RID GodotNavigationServer2D::region_create() {
+	MutexLock lock(operations_mutex);
+
+	RID rid = region_owner.make_rid();
+	NavRegion2D *reg = region_owner.get_or_null(rid);
+	reg->set_self(rid);
+	return rid;
+}
+
+COMMAND_2(region_set_enabled, RID, p_region, bool, p_enabled) {
+	NavRegion2D *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_NULL(region);
+
+	region->set_enabled(p_enabled);
+}
+
+bool GodotNavigationServer2D::region_get_enabled(RID p_region) const {
+	const NavRegion2D *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_NULL_V(region, false);
+
+	return region->get_enabled();
+}
+
+COMMAND_2(region_set_use_edge_connections, RID, p_region, bool, p_enabled) {
+	NavRegion2D *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_NULL(region);
+
+	region->set_use_edge_connections(p_enabled);
+}
+
+bool GodotNavigationServer2D::region_get_use_edge_connections(RID p_region) const {
+	NavRegion2D *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_NULL_V(region, false);
+
+	return region->get_use_edge_connections();
+}
+
+COMMAND_2(region_set_enter_cost, RID, p_region, real_t, p_enter_cost) {
+	NavRegion2D *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_NULL(region);
+	ERR_FAIL_COND(p_enter_cost < 0.0);
+
+	region->set_enter_cost(p_enter_cost);
+}
+
+real_t GodotNavigationServer2D::region_get_enter_cost(RID p_region) const {
+	NavRegion2D *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_NULL_V(region, 0);
+
+	return region->get_enter_cost();
+}
+
+COMMAND_2(region_set_travel_cost, RID, p_region, real_t, p_travel_cost) {
+	NavRegion2D *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_NULL(region);
+	ERR_FAIL_COND(p_travel_cost < 0.0);
+
+	region->set_travel_cost(p_travel_cost);
+}
+
+real_t GodotNavigationServer2D::region_get_travel_cost(RID p_region) const {
+	NavRegion2D *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_NULL_V(region, 0);
+
+	return region->get_travel_cost();
+}
+
+COMMAND_2(region_set_owner_id, RID, p_region, ObjectID, p_owner_id) {
+	NavRegion2D *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_NULL(region);
+
+	region->set_owner_id(p_owner_id);
+}
+
+ObjectID GodotNavigationServer2D::region_get_owner_id(RID p_region) const {
+	const NavRegion2D *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_NULL_V(region, ObjectID());
+
+	return region->get_owner_id();
+}
+
+bool GodotNavigationServer2D::region_owns_point(RID p_region, const Vector2 &p_point) const {
+	const NavRegion2D *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_NULL_V(region, false);
+
+	if (region->get_map()) {
+		RID closest_point_owner = map_get_closest_point_owner(region->get_map()->get_self(), p_point);
+		return closest_point_owner == region->get_self();
+	}
+	return false;
+}
+
+COMMAND_2(region_set_map, RID, p_region, RID, p_map) {
+	NavRegion2D *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_NULL(region);
+
+	NavMap2D *map = map_owner.get_or_null(p_map);
+
+	region->set_map(map);
+}
+
+COMMAND_2(region_set_navigation_layers, RID, p_region, uint32_t, p_navigation_layers) {
+	NavRegion2D *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_NULL(region);
+
+	region->set_navigation_layers(p_navigation_layers);
+}
+
+uint32_t GodotNavigationServer2D::region_get_navigation_layers(RID p_region) const {
+	NavRegion2D *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_NULL_V(region, 0);
+
+	return region->get_navigation_layers();
+}
+
+COMMAND_2(region_set_transform, RID, p_region, Transform2D, p_transform) {
+	NavRegion2D *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_NULL(region);
+
+	region->set_transform(p_transform);
+}
+
+Transform2D GodotNavigationServer2D::region_get_transform(RID p_region) const {
+	NavRegion2D *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_NULL_V(region, Transform2D());
+
+	return region->get_transform();
+}
+
+void GodotNavigationServer2D::region_set_navigation_polygon(RID p_region, Ref<NavigationPolygon> p_navigation_polygon) {
+	NavRegion2D *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_NULL(region);
+
+	region->set_navigation_polygon(p_navigation_polygon);
+}
+
+int GodotNavigationServer2D::region_get_connections_count(RID p_region) const {
+	NavRegion2D *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_NULL_V(region, 0);
+	NavMap2D *map = region->get_map();
+	if (map) {
+		return map->get_region_connections_count(region);
+	}
+	return 0;
+}
+
+Vector2 GodotNavigationServer2D::region_get_connection_pathway_start(RID p_region, int p_connection_id) const {
+	NavRegion2D *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_NULL_V(region, Vector2());
+	NavMap2D *map = region->get_map();
+	if (map) {
+		return map->get_region_connection_pathway_start(region, p_connection_id);
+	}
+	return Vector2();
+}
+
+Vector2 GodotNavigationServer2D::region_get_connection_pathway_end(RID p_region, int p_connection_id) const {
+	NavRegion2D *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_NULL_V(region, Vector2());
+	NavMap2D *map = region->get_map();
+	if (map) {
+		return map->get_region_connection_pathway_end(region, p_connection_id);
+	}
+	return Vector2();
+}
+
+Vector2 GodotNavigationServer2D::region_get_closest_point(RID p_region, const Vector2 &p_point) const {
+	const NavRegion2D *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_NULL_V(region, Vector2());
+
+	return region->get_closest_point_info(p_point).point;
+}
+
+Vector2 GodotNavigationServer2D::region_get_random_point(RID p_region, uint32_t p_navigation_layers, bool p_uniformly) const {
+	const NavRegion2D *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_NULL_V(region, Vector2());
+
+	return region->get_random_point(p_navigation_layers, p_uniformly);
+}
+
+Rect2 GodotNavigationServer2D::region_get_bounds(RID p_region) const {
+	const NavRegion2D *region = region_owner.get_or_null(p_region);
+	ERR_FAIL_NULL_V(region, Rect2());
+
+	return region->get_bounds();
+}
+
+RID GodotNavigationServer2D::link_create() {
+	MutexLock lock(operations_mutex);
+
+	RID rid = link_owner.make_rid();
+	NavLink2D *link = link_owner.get_or_null(rid);
+	link->set_self(rid);
+	return rid;
+}
+
+COMMAND_2(link_set_map, RID, p_link, RID, p_map) {
+	NavLink2D *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_NULL(link);
+
+	NavMap2D *map = map_owner.get_or_null(p_map);
+
+	link->set_map(map);
+}
+
+RID GodotNavigationServer2D::link_get_map(const RID p_link) const {
+	const NavLink2D *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_NULL_V(link, RID());
+
+	if (link->get_map()) {
+		return link->get_map()->get_self();
+	}
+	return RID();
+}
+
+COMMAND_2(link_set_enabled, RID, p_link, bool, p_enabled) {
+	NavLink2D *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_NULL(link);
+
+	link->set_enabled(p_enabled);
+}
+
+bool GodotNavigationServer2D::link_get_enabled(RID p_link) const {
+	const NavLink2D *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_NULL_V(link, false);
+
+	return link->get_enabled();
+}
+
+COMMAND_2(link_set_bidirectional, RID, p_link, bool, p_bidirectional) {
+	NavLink2D *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_NULL(link);
+
+	link->set_bidirectional(p_bidirectional);
+}
+
+bool GodotNavigationServer2D::link_is_bidirectional(RID p_link) const {
+	const NavLink2D *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_NULL_V(link, false);
+
+	return link->is_bidirectional();
+}
+
+COMMAND_2(link_set_navigation_layers, RID, p_link, uint32_t, p_navigation_layers) {
+	NavLink2D *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_NULL(link);
+
+	link->set_navigation_layers(p_navigation_layers);
+}
+
+uint32_t GodotNavigationServer2D::link_get_navigation_layers(const RID p_link) const {
+	const NavLink2D *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_NULL_V(link, 0);
+
+	return link->get_navigation_layers();
+}
+
+COMMAND_2(link_set_start_position, RID, p_link, Vector2, p_position) {
+	NavLink2D *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_NULL(link);
+
+	link->set_start_position(p_position);
+}
+
+Vector2 GodotNavigationServer2D::link_get_start_position(RID p_link) const {
+	const NavLink2D *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_NULL_V(link, Vector2());
+
+	return link->get_start_position();
+}
+
+COMMAND_2(link_set_end_position, RID, p_link, Vector2, p_position) {
+	NavLink2D *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_NULL(link);
+
+	link->set_end_position(p_position);
+}
+
+Vector2 GodotNavigationServer2D::link_get_end_position(RID p_link) const {
+	const NavLink2D *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_NULL_V(link, Vector2());
+
+	return link->get_end_position();
+}
+
+COMMAND_2(link_set_enter_cost, RID, p_link, real_t, p_enter_cost) {
+	NavLink2D *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_NULL(link);
+
+	link->set_enter_cost(p_enter_cost);
+}
+
+real_t GodotNavigationServer2D::link_get_enter_cost(const RID p_link) const {
+	const NavLink2D *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_NULL_V(link, 0);
+
+	return link->get_enter_cost();
+}
+
+COMMAND_2(link_set_travel_cost, RID, p_link, real_t, p_travel_cost) {
+	NavLink2D *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_NULL(link);
+
+	link->set_travel_cost(p_travel_cost);
+}
+
+real_t GodotNavigationServer2D::link_get_travel_cost(const RID p_link) const {
+	const NavLink2D *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_NULL_V(link, 0);
+
+	return link->get_travel_cost();
+}
+
+COMMAND_2(link_set_owner_id, RID, p_link, ObjectID, p_owner_id) {
+	NavLink2D *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_NULL(link);
+
+	link->set_owner_id(p_owner_id);
+}
+
+ObjectID GodotNavigationServer2D::link_get_owner_id(RID p_link) const {
+	const NavLink2D *link = link_owner.get_or_null(p_link);
+	ERR_FAIL_NULL_V(link, ObjectID());
+
+	return link->get_owner_id();
+}
+
+RID GodotNavigationServer2D::agent_create() {
+	MutexLock lock(operations_mutex);
+
+	RID rid = agent_owner.make_rid();
+	NavAgent2D *agent = agent_owner.get_or_null(rid);
+	agent->set_self(rid);
+	return rid;
+}
+
+COMMAND_2(agent_set_avoidance_enabled, RID, p_agent, bool, p_enabled) {
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL(agent);
+
+	agent->set_avoidance_enabled(p_enabled);
+}
+
+bool GodotNavigationServer2D::agent_get_avoidance_enabled(RID p_agent) const {
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL_V(agent, false);
+
+	return agent->is_avoidance_enabled();
+}
+
+COMMAND_2(agent_set_map, RID, p_agent, RID, p_map) {
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL(agent);
+
+	NavMap2D *map = map_owner.get_or_null(p_map);
+
+	agent->set_map(map);
+}
+
+COMMAND_2(agent_set_neighbor_distance, RID, p_agent, real_t, p_distance) {
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL(agent);
+
+	agent->set_neighbor_distance(p_distance);
+}
+
+real_t GodotNavigationServer2D::agent_get_neighbor_distance(RID p_agent) const {
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL_V(agent, 0);
+
+	return agent->get_neighbor_distance();
+}
+
+COMMAND_2(agent_set_max_neighbors, RID, p_agent, int, p_count) {
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL(agent);
+
+	agent->set_max_neighbors(p_count);
+}
+
+int GodotNavigationServer2D::agent_get_max_neighbors(RID p_agent) const {
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL_V(agent, 0);
+
+	return agent->get_max_neighbors();
+}
+
+COMMAND_2(agent_set_time_horizon_agents, RID, p_agent, real_t, p_time_horizon) {
+	ERR_FAIL_COND_MSG(p_time_horizon < 0.0, "Time horizon must be positive.");
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL(agent);
+
+	agent->set_time_horizon_agents(p_time_horizon);
+}
+
+real_t GodotNavigationServer2D::agent_get_time_horizon_agents(RID p_agent) const {
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL_V(agent, 0);
+
+	return agent->get_time_horizon_agents();
+}
+
+COMMAND_2(agent_set_time_horizon_obstacles, RID, p_agent, real_t, p_time_horizon) {
+	ERR_FAIL_COND_MSG(p_time_horizon < 0.0, "Time horizon must be positive.");
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL(agent);
+
+	agent->set_time_horizon_obstacles(p_time_horizon);
+}
+
+real_t GodotNavigationServer2D::agent_get_time_horizon_obstacles(RID p_agent) const {
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL_V(agent, 0);
+
+	return agent->get_time_horizon_obstacles();
+}
+
+COMMAND_2(agent_set_radius, RID, p_agent, real_t, p_radius) {
+	ERR_FAIL_COND_MSG(p_radius < 0.0, "Radius must be positive.");
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL(agent);
+
+	agent->set_radius(p_radius);
+}
+
+real_t GodotNavigationServer2D::agent_get_radius(RID p_agent) const {
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL_V(agent, 0);
+
+	return agent->get_radius();
+}
+
+COMMAND_2(agent_set_max_speed, RID, p_agent, real_t, p_max_speed) {
+	ERR_FAIL_COND_MSG(p_max_speed < 0.0, "Max speed must be positive.");
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL(agent);
+
+	agent->set_max_speed(p_max_speed);
+}
+
+real_t GodotNavigationServer2D::agent_get_max_speed(RID p_agent) const {
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL_V(agent, 0);
+
+	return agent->get_max_speed();
+}
+
+COMMAND_2(agent_set_velocity_forced, RID, p_agent, Vector2, p_velocity) {
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL(agent);
+
+	agent->set_velocity_forced(p_velocity);
+}
+
+COMMAND_2(agent_set_velocity, RID, p_agent, Vector2, p_velocity) {
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL(agent);
+
+	agent->set_velocity(p_velocity);
+}
+
+Vector2 GodotNavigationServer2D::agent_get_velocity(RID p_agent) const {
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL_V(agent, Vector2());
+
+	return agent->get_velocity();
+}
+
+COMMAND_2(agent_set_position, RID, p_agent, Vector2, p_position) {
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL(agent);
+
+	agent->set_position(p_position);
+}
+
+Vector2 GodotNavigationServer2D::agent_get_position(RID p_agent) const {
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL_V(agent, Vector2());
+
+	return agent->get_position();
+}
+
+bool GodotNavigationServer2D::agent_is_map_changed(RID p_agent) const {
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL_V(agent, false);
+
+	return agent->is_map_changed();
+}
+
+COMMAND_2(agent_set_avoidance_callback, RID, p_agent, Callable, p_callback) {
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL(agent);
+
+	agent->set_avoidance_callback(p_callback);
+
+	if (agent->get_map()) {
+		if (p_callback.is_valid()) {
+			agent->get_map()->set_agent_as_controlled(agent);
+		} else {
+			agent->get_map()->remove_agent_as_controlled(agent);
+		}
+	}
+}
+
+bool GodotNavigationServer2D::agent_has_avoidance_callback(RID p_agent) const {
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL_V(agent, false);
+
+	return agent->has_avoidance_callback();
+}
+
+COMMAND_2(agent_set_avoidance_layers, RID, p_agent, uint32_t, p_layers) {
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL(agent);
+	agent->set_avoidance_layers(p_layers);
+}
+
+uint32_t GodotNavigationServer2D::agent_get_avoidance_layers(RID p_agent) const {
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL_V(agent, 0);
+
+	return agent->get_avoidance_layers();
+}
+
+COMMAND_2(agent_set_avoidance_mask, RID, p_agent, uint32_t, p_mask) {
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL(agent);
+	agent->set_avoidance_mask(p_mask);
+}
+
+uint32_t GodotNavigationServer2D::agent_get_avoidance_mask(RID p_agent) const {
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL_V(agent, 0);
+
+	return agent->get_avoidance_mask();
+}
+
+COMMAND_2(agent_set_avoidance_priority, RID, p_agent, real_t, p_priority) {
+	ERR_FAIL_COND_MSG(p_priority < 0.0, "Avoidance priority must be between 0.0 and 1.0 inclusive.");
+	ERR_FAIL_COND_MSG(p_priority > 1.0, "Avoidance priority must be between 0.0 and 1.0 inclusive.");
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL(agent);
+	agent->set_avoidance_priority(p_priority);
+}
+
+real_t GodotNavigationServer2D::agent_get_avoidance_priority(RID p_agent) const {
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL_V(agent, 0);
+
+	return agent->get_avoidance_priority();
+}
+
+COMMAND_2(agent_set_paused, RID, p_agent, bool, p_paused) {
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL(agent);
+
+	agent->set_paused(p_paused);
+}
+
+bool GodotNavigationServer2D::agent_get_paused(RID p_agent) const {
+	NavAgent2D *agent = agent_owner.get_or_null(p_agent);
+	ERR_FAIL_NULL_V(agent, false);
+
+	return agent->get_paused();
+}
+
+RID GodotNavigationServer2D::obstacle_create() {
+	MutexLock lock(operations_mutex);
+
+	RID rid = obstacle_owner.make_rid();
+	NavObstacle2D *obstacle = obstacle_owner.get_or_null(rid);
+	obstacle->set_self(rid);
+
+	RID agent_rid = agent_owner.make_rid();
+	NavAgent2D *agent = agent_owner.get_or_null(agent_rid);
+	agent->set_self(agent_rid);
+
+	obstacle->set_agent(agent);
+
+	return rid;
+}
+
+COMMAND_2(obstacle_set_avoidance_enabled, RID, p_obstacle, bool, p_enabled) {
+	NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+	ERR_FAIL_NULL(obstacle);
+
+	obstacle->set_avoidance_enabled(p_enabled);
+}
+
+bool GodotNavigationServer2D::obstacle_get_avoidance_enabled(RID p_obstacle) const {
+	NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+	ERR_FAIL_NULL_V(obstacle, false);
+
+	return obstacle->is_avoidance_enabled();
+}
+
+COMMAND_2(obstacle_set_map, RID, p_obstacle, RID, p_map) {
+	NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+	ERR_FAIL_NULL(obstacle);
+
+	NavMap2D *map = map_owner.get_or_null(p_map);
+
+	obstacle->set_map(map);
+}
+
+RID GodotNavigationServer2D::obstacle_get_map(RID p_obstacle) const {
+	NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+	ERR_FAIL_NULL_V(obstacle, RID());
+	if (obstacle->get_map()) {
+		return obstacle->get_map()->get_self();
+	}
+	return RID();
+}
+
+COMMAND_2(obstacle_set_paused, RID, p_obstacle, bool, p_paused) {
+	NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+	ERR_FAIL_NULL(obstacle);
+
+	obstacle->set_paused(p_paused);
+}
+
+bool GodotNavigationServer2D::obstacle_get_paused(RID p_obstacle) const {
+	NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+	ERR_FAIL_NULL_V(obstacle, false);
+
+	return obstacle->get_paused();
+}
+
+COMMAND_2(obstacle_set_radius, RID, p_obstacle, real_t, p_radius) {
+	ERR_FAIL_COND_MSG(p_radius < 0.0, "Radius must be positive.");
+	NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+	ERR_FAIL_NULL(obstacle);
+
+	obstacle->set_radius(p_radius);
+}
+
+real_t GodotNavigationServer2D::obstacle_get_radius(RID p_obstacle) const {
+	NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+	ERR_FAIL_NULL_V(obstacle, 0);
+
+	return obstacle->get_radius();
+}
+
+COMMAND_2(obstacle_set_velocity, RID, p_obstacle, Vector2, p_velocity) {
+	NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+	ERR_FAIL_NULL(obstacle);
+
+	obstacle->set_velocity(p_velocity);
+}
+
+Vector2 GodotNavigationServer2D::obstacle_get_velocity(RID p_obstacle) const {
+	NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+	ERR_FAIL_NULL_V(obstacle, Vector2());
+
+	return obstacle->get_velocity();
+}
+
+COMMAND_2(obstacle_set_position, RID, p_obstacle, Vector2, p_position) {
+	NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+	ERR_FAIL_NULL(obstacle);
+	obstacle->set_position(p_position);
+}
+
+Vector2 GodotNavigationServer2D::obstacle_get_position(RID p_obstacle) const {
+	NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+	ERR_FAIL_NULL_V(obstacle, Vector2());
+
+	return obstacle->get_position();
+}
+
+COMMAND_2(obstacle_set_avoidance_layers, RID, p_obstacle, uint32_t, p_layers) {
+	NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+	ERR_FAIL_NULL(obstacle);
+	obstacle->set_avoidance_layers(p_layers);
+}
+
+uint32_t GodotNavigationServer2D::obstacle_get_avoidance_layers(RID p_obstacle) const {
+	NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+	ERR_FAIL_NULL_V(obstacle, 0);
+
+	return obstacle->get_avoidance_layers();
+}
+
+void GodotNavigationServer2D::obstacle_set_vertices(RID p_obstacle, const Vector<Vector2> &p_vertices) {
+	NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+	ERR_FAIL_NULL(obstacle);
+	obstacle->set_vertices(p_vertices);
+}
+
+Vector<Vector2> GodotNavigationServer2D::obstacle_get_vertices(RID p_obstacle) const {
+	NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_obstacle);
+	ERR_FAIL_NULL_V(obstacle, Vector<Vector2>());
+
+	return obstacle->get_vertices();
+}
+
+void GodotNavigationServer2D::flush_queries() {
+	// In c++ we can't be sure that this is performed in the main thread
+	// even with mutable functions.
+	MutexLock lock(commands_mutex);
+	MutexLock lock2(operations_mutex);
+
+	for (SetCommand2D *command : commands) {
+		command->exec(this);
+		memdelete(command);
+	}
+	commands.clear();
+}
+
+COMMAND_1(free, RID, p_object) {
+	if (geometry_parser_owner.owns(p_object)) {
+		RWLockWrite write_lock(geometry_parser_rwlock);
+
+		NavMeshGeometryParser2D *parser = geometry_parser_owner.get_or_null(p_object);
+		ERR_FAIL_NULL(parser);
+
+		generator_parsers.erase(parser);
+#ifndef CLIPPER2_ENABLED
+		NavMeshGenerator2D::get_singleton()->set_generator_parsers(generator_parsers);
+#endif
+		geometry_parser_owner.free(parser->self);
+		return;
+	}
+	if (map_owner.owns(p_object)) {
+		NavMap2D *map = map_owner.get_or_null(p_object);
+
+		// Removes any assigned region.
+		for (NavRegion2D *region : map->get_regions()) {
+			map->remove_region(region);
+			region->set_map(nullptr);
+		}
+
+		// Removes any assigned links.
+		for (NavLink2D *link : map->get_links()) {
+			map->remove_link(link);
+			link->set_map(nullptr);
+		}
+
+		// Remove any assigned agent.
+		for (NavAgent2D *agent : map->get_agents()) {
+			map->remove_agent(agent);
+			agent->set_map(nullptr);
+		}
+
+		// Remove any assigned obstacles.
+		for (NavObstacle2D *obstacle : map->get_obstacles()) {
+			map->remove_obstacle(obstacle);
+			obstacle->set_map(nullptr);
+		}
+
+		int map_index = active_maps.find(map);
+		if (map_index >= 0) {
+			active_maps.remove_at(map_index);
+			active_maps_iteration_id.remove_at(map_index);
+		}
+		map_owner.free(p_object);
+
+	} else if (region_owner.owns(p_object)) {
+		NavRegion2D *region = region_owner.get_or_null(p_object);
+
+		// Removes this region from the map if assigned.
+		if (region->get_map() != nullptr) {
+			region->get_map()->remove_region(region);
+			region->set_map(nullptr);
+		}
+
+		region_owner.free(p_object);
+
+	} else if (link_owner.owns(p_object)) {
+		NavLink2D *link = link_owner.get_or_null(p_object);
+
+		// Removes this link from the map if assigned.
+		if (link->get_map() != nullptr) {
+			link->get_map()->remove_link(link);
+			link->set_map(nullptr);
+		}
+
+		link_owner.free(p_object);
+
+	} else if (agent_owner.owns(p_object)) {
+		internal_free_agent(p_object);
+
+	} else if (obstacle_owner.owns(p_object)) {
+		internal_free_obstacle(p_object);
+
+	} else {
+		ERR_PRINT("Attempted to free a NavigationServer RID that did not exist (or was already freed).");
+	}
+}
+
+void GodotNavigationServer2D::internal_free_agent(RID p_object) {
+	NavAgent2D *agent = agent_owner.get_or_null(p_object);
+	if (agent) {
+		if (agent->get_map() != nullptr) {
+			agent->get_map()->remove_agent(agent);
+			agent->set_map(nullptr);
+		}
+		agent_owner.free(p_object);
+	}
+}
+
+void GodotNavigationServer2D::internal_free_obstacle(RID p_object) {
+	NavObstacle2D *obstacle = obstacle_owner.get_or_null(p_object);
+	if (obstacle) {
+		NavAgent2D *obstacle_agent = obstacle->get_agent();
+		if (obstacle_agent) {
+			RID _agent_rid = obstacle_agent->get_self();
+			internal_free_agent(_agent_rid);
+			obstacle->set_agent(nullptr);
+		}
+		if (obstacle->get_map() != nullptr) {
+			obstacle->get_map()->remove_obstacle(obstacle);
+			obstacle->set_map(nullptr);
+		}
+		obstacle_owner.free(p_object);
+	}
+}
+
+void GodotNavigationServer2D::process(real_t p_delta_time) {
+	flush_queries();
+
+	if (!active) {
+		return;
+	}
+
+	int _new_pm_region_count = 0;
+	int _new_pm_agent_count = 0;
+	int _new_pm_link_count = 0;
+	int _new_pm_polygon_count = 0;
+	int _new_pm_edge_count = 0;
+	int _new_pm_edge_merge_count = 0;
+	int _new_pm_edge_connection_count = 0;
+	int _new_pm_edge_free_count = 0;
+	int _new_pm_obstacle_count = 0;
+
+	// In c++ we can't be sure that this is performed in the main thread
+	// even with mutable functions.
+	MutexLock lock(operations_mutex);
+	for (uint32_t i(0); i < active_maps.size(); i++) {
+		active_maps[i]->sync();
+		active_maps[i]->step(p_delta_time);
+		active_maps[i]->dispatch_callbacks();
+
+		_new_pm_region_count += active_maps[i]->get_pm_region_count();
+		_new_pm_agent_count += active_maps[i]->get_pm_agent_count();
+		_new_pm_link_count += active_maps[i]->get_pm_link_count();
+		_new_pm_polygon_count += active_maps[i]->get_pm_polygon_count();
+		_new_pm_edge_count += active_maps[i]->get_pm_edge_count();
+		_new_pm_edge_merge_count += active_maps[i]->get_pm_edge_merge_count();
+		_new_pm_edge_connection_count += active_maps[i]->get_pm_edge_connection_count();
+		_new_pm_edge_free_count += active_maps[i]->get_pm_edge_free_count();
+		_new_pm_obstacle_count += active_maps[i]->get_pm_obstacle_count();
+
+		// Emit a signal if a map changed.
+		const uint32_t new_map_iteration_id = active_maps[i]->get_iteration_id();
+		if (new_map_iteration_id != active_maps_iteration_id[i]) {
+			emit_signal(SNAME("map_changed"), active_maps[i]->get_self());
+			active_maps_iteration_id[i] = new_map_iteration_id;
+		}
+	}
+
+	pm_region_count = _new_pm_region_count;
+	pm_agent_count = _new_pm_agent_count;
+	pm_link_count = _new_pm_link_count;
+	pm_polygon_count = _new_pm_polygon_count;
+	pm_edge_count = _new_pm_edge_count;
+	pm_edge_merge_count = _new_pm_edge_merge_count;
+	pm_edge_connection_count = _new_pm_edge_connection_count;
+	pm_edge_free_count = _new_pm_edge_free_count;
+	pm_obstacle_count = _new_pm_obstacle_count;
+}
+
+void GodotNavigationServer2D::set_active(bool p_active) {
+	MutexLock lock(operations_mutex);
+
+	active = p_active;
+}
+
+void GodotNavigationServer2D::query_path(const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result, const Callable &p_callback) {
+	ERR_FAIL_COND(p_query_parameters.is_null());
+	ERR_FAIL_COND(p_query_result.is_null());
+
+	NavMap2D *map = map_owner.get_or_null(p_query_parameters->get_map());
+	ERR_FAIL_NULL(map);
+
+	NavMeshQueries2D::map_query_path(map, p_query_parameters, p_query_result, p_callback);
+}
+
+RID GodotNavigationServer2D::source_geometry_parser_create() {
+	RWLockWrite write_lock(geometry_parser_rwlock);
+
+	RID rid = geometry_parser_owner.make_rid();
+
+	NavMeshGeometryParser2D *parser = geometry_parser_owner.get_or_null(rid);
+	parser->self = rid;
+
+	generator_parsers.push_back(parser);
+#ifdef CLIPPER2_ENABLED
+	NavMeshGenerator2D::get_singleton()->set_generator_parsers(generator_parsers);
+#endif
+	return rid;
+}
+
+void GodotNavigationServer2D::source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) {
+	RWLockWrite write_lock(geometry_parser_rwlock);
+
+	NavMeshGeometryParser2D *parser = geometry_parser_owner.get_or_null(p_parser);
+	ERR_FAIL_NULL(parser);
+
+	parser->callback = p_callback;
+}
+
+int GodotNavigationServer2D::get_process_info(ProcessInfo p_info) const {
+	switch (p_info) {
+		case INFO_ACTIVE_MAPS: {
+			return active_maps.size();
+		} break;
+		case INFO_REGION_COUNT: {
+			return pm_region_count;
+		} break;
+		case INFO_AGENT_COUNT: {
+			return pm_agent_count;
+		} break;
+		case INFO_LINK_COUNT: {
+			return pm_link_count;
+		} break;
+		case INFO_POLYGON_COUNT: {
+			return pm_polygon_count;
+		} break;
+		case INFO_EDGE_COUNT: {
+			return pm_edge_count;
+		} break;
+		case INFO_EDGE_MERGE_COUNT: {
+			return pm_edge_merge_count;
+		} break;
+		case INFO_EDGE_CONNECTION_COUNT: {
+			return pm_edge_connection_count;
+		} break;
+		case INFO_EDGE_FREE_COUNT: {
+			return pm_edge_free_count;
+		} break;
+		case INFO_OBSTACLE_COUNT: {
+			return pm_obstacle_count;
+		} break;
+	}
+
+	return 0;
+}
+
+#undef COMMAND_1
+#undef COMMAND_2

modules/navigation/2d/godot_navigation_server_2d.h → modules/navigation_2d/2d/godot_navigation_server_2d.h

@@ -30,75 +30,145 @@
 
 #pragma once
 
-#include "../nav_agent_3d.h"
-#include "../nav_link_3d.h"
-#include "../nav_map_3d.h"
-#include "../nav_obstacle_3d.h"
-#include "../nav_region_3d.h"
-
+#include "../nav_agent_2d.h"
+#include "../nav_link_2d.h"
+#include "../nav_map_2d.h"
+#include "../nav_obstacle_2d.h"
+#include "../nav_region_2d.h"
+
+#include "core/templates/local_vector.h"
+#include "core/templates/rid.h"
+#include "core/templates/rid_owner.h"
+#include "servers/navigation/navigation_path_query_parameters_2d.h"
+#include "servers/navigation/navigation_path_query_result_2d.h"
 #include "servers/navigation_server_2d.h"
 
+/// The commands are functions executed during the `sync` phase.
+
+#define MERGE_INTERNAL(A, B) A##B
+#define MERGE(A, B) MERGE_INTERNAL(A, B)
+
+#define COMMAND_1(F_NAME, T_0, D_0)        \
+	virtual void F_NAME(T_0 D_0) override; \
+	void MERGE(_cmd_, F_NAME)(T_0 D_0)
+
+#define COMMAND_2(F_NAME, T_0, D_0, T_1, D_1)       \
+	virtual void F_NAME(T_0 D_0, T_1 D_1) override; \
+	void MERGE(_cmd_, F_NAME)(T_0 D_0, T_1 D_1)
+
+class GodotNavigationServer2D;
 #ifdef CLIPPER2_ENABLED
 class NavMeshGenerator2D;
 #endif // CLIPPER2_ENABLED
 
+struct SetCommand2D {
+	virtual ~SetCommand2D() {}
+	virtual void exec(GodotNavigationServer2D *p_server) = 0;
+};
+
 // This server exposes the `NavigationServer3D` features in the 2D world.
 class GodotNavigationServer2D : public NavigationServer2D {
 	GDCLASS(GodotNavigationServer2D, NavigationServer2D);
 
+	Mutex commands_mutex;
+	/// Mutex used to make any operation threadsafe.
+	Mutex operations_mutex;
+
+	LocalVector<SetCommand2D *> commands;
+
+	mutable RID_Owner<NavLink2D> link_owner;
+	mutable RID_Owner<NavMap2D> map_owner;
+	mutable RID_Owner<NavRegion2D> region_owner;
+	mutable RID_Owner<NavAgent2D> agent_owner;
+	mutable RID_Owner<NavObstacle2D> obstacle_owner;
+
+	bool active = true;
+	LocalVector<NavMap2D *> active_maps;
+	LocalVector<uint32_t> active_maps_iteration_id;
+
 #ifdef CLIPPER2_ENABLED
 	NavMeshGenerator2D *navmesh_generator_2d = nullptr;
 #endif // CLIPPER2_ENABLED
 
+	// Performance Monitor.
+	int pm_region_count = 0;
+	int pm_agent_count = 0;
+	int pm_link_count = 0;
+	int pm_polygon_count = 0;
+	int pm_edge_count = 0;
+	int pm_edge_merge_count = 0;
+	int pm_edge_connection_count = 0;
+	int pm_edge_free_count = 0;
+	int pm_obstacle_count = 0;
+
 public:
 	GodotNavigationServer2D();
 	virtual ~GodotNavigationServer2D();
 
+	void add_command(SetCommand2D *p_command);
+
 	virtual TypedArray<RID> get_maps() const override;
 
 	virtual RID map_create() override;
-	virtual void map_set_active(RID p_map, bool p_active) override;
+	COMMAND_2(map_set_active, RID, p_map, bool, p_active);
 	virtual bool map_is_active(RID p_map) const override;
-	virtual void map_set_cell_size(RID p_map, real_t p_cell_size) override;
+
+	COMMAND_2(map_set_cell_size, RID, p_map, real_t, p_cell_size);
 	virtual real_t map_get_cell_size(RID p_map) const override;
-	virtual void map_set_use_edge_connections(RID p_map, bool p_enabled) override;
+
+	COMMAND_2(map_set_use_edge_connections, RID, p_map, bool, p_enabled);
 	virtual bool map_get_use_edge_connections(RID p_map) const override;
-	virtual void map_set_edge_connection_margin(RID p_map, real_t p_connection_margin) override;
+
+	COMMAND_2(map_set_edge_connection_margin, RID, p_map, real_t, p_connection_margin);
 	virtual real_t map_get_edge_connection_margin(RID p_map) const override;
-	virtual void map_set_link_connection_radius(RID p_map, real_t p_connection_radius) override;
+
+	COMMAND_2(map_set_link_connection_radius, RID, p_map, real_t, p_connection_radius);
 	virtual real_t map_get_link_connection_radius(RID p_map) const override;
+
 	virtual Vector<Vector2> map_get_path(RID p_map, Vector2 p_origin, Vector2 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) override;
+
 	virtual Vector2 map_get_closest_point(RID p_map, const Vector2 &p_point) const override;
+
 	virtual RID map_get_closest_point_owner(RID p_map, const Vector2 &p_point) const override;
+
 	virtual TypedArray<RID> map_get_links(RID p_map) const override;
 	virtual TypedArray<RID> map_get_regions(RID p_map) const override;
 	virtual TypedArray<RID> map_get_agents(RID p_map) const override;
 	virtual TypedArray<RID> map_get_obstacles(RID p_map) const override;
+
 	virtual void map_force_update(RID p_map) override;
-	virtual Vector2 map_get_random_point(RID p_map, uint32_t p_navigation_layers, bool p_uniformly) const override;
 	virtual uint32_t map_get_iteration_id(RID p_map) const override;
-	virtual void map_set_use_async_iterations(RID p_map, bool p_enabled) override;
+
+	COMMAND_2(map_set_use_async_iterations, RID, p_map, bool, p_enabled);
 	virtual bool map_get_use_async_iterations(RID p_map) const override;
 
+	virtual Vector2 map_get_random_point(RID p_map, uint32_t p_navigation_layers, bool p_uniformly) const override;
+
 	virtual RID region_create() override;
-	virtual void region_set_enabled(RID p_region, bool p_enabled) override;
+
+	COMMAND_2(region_set_enabled, RID, p_region, bool, p_enabled);
 	virtual bool region_get_enabled(RID p_region) const override;
-	virtual void region_set_use_edge_connections(RID p_region, bool p_enabled) override;
+
+	COMMAND_2(region_set_use_edge_connections, RID, p_region, bool, p_enabled);
 	virtual bool region_get_use_edge_connections(RID p_region) const override;
-	virtual void region_set_enter_cost(RID p_region, real_t p_enter_cost) override;
+
+	COMMAND_2(region_set_enter_cost, RID, p_region, real_t, p_enter_cost);
 	virtual real_t region_get_enter_cost(RID p_region) const override;
-	virtual void region_set_travel_cost(RID p_region, real_t p_travel_cost) override;
+	COMMAND_2(region_set_travel_cost, RID, p_region, real_t, p_travel_cost);
 	virtual real_t region_get_travel_cost(RID p_region) const override;
-	virtual void region_set_owner_id(RID p_region, ObjectID p_owner_id) override;
+
+	COMMAND_2(region_set_owner_id, RID, p_region, ObjectID, p_owner_id);
 	virtual ObjectID region_get_owner_id(RID p_region) const override;
+
 	virtual bool region_owns_point(RID p_region, const Vector2 &p_point) const override;
-	virtual void region_set_map(RID p_region, RID p_map) override;
+
+	COMMAND_2(region_set_map, RID, p_region, RID, p_map);
 	virtual RID region_get_map(RID p_region) const override;
-	virtual void region_set_navigation_layers(RID p_region, uint32_t p_navigation_layers) override;
+	COMMAND_2(region_set_navigation_layers, RID, p_region, uint32_t, p_navigation_layers);
 	virtual uint32_t region_get_navigation_layers(RID p_region) const override;
-	virtual void region_set_transform(RID p_region, Transform2D p_transform) override;
+	COMMAND_2(region_set_transform, RID, p_region, Transform2D, p_transform);
 	virtual Transform2D region_get_transform(RID p_region) const override;
-	virtual void region_set_navigation_polygon(RID p_region, Ref<NavigationPolygon> p_navigation_polygon) override;
+	COMMAND_2(region_set_navigation_polygon, RID, p_region, Ref<NavigationPolygon>, p_navigation_polygon);
 	virtual int region_get_connections_count(RID p_region) const override;
 	virtual Vector2 region_get_connection_pathway_start(RID p_region, int p_connection_id) const override;
 	virtual Vector2 region_get_connection_pathway_end(RID p_region, int p_connection_id) const override;
@@ -109,51 +179,50 @@ public:
 	virtual RID link_create() override;
 
 	/// Set the map of this link.
-	virtual void link_set_map(RID p_link, RID p_map) override;
+	COMMAND_2(link_set_map, RID, p_link, RID, p_map);
 	virtual RID link_get_map(RID p_link) const override;
-
-	virtual void link_set_enabled(RID p_link, bool p_enabled) override;
+	COMMAND_2(link_set_enabled, RID, p_link, bool, p_enabled);
 	virtual bool link_get_enabled(RID p_link) const override;
 
 	/// Set whether this link travels in both directions.
-	virtual void link_set_bidirectional(RID p_link, bool p_bidirectional) override;
+	COMMAND_2(link_set_bidirectional, RID, p_link, bool, p_bidirectional);
 	virtual bool link_is_bidirectional(RID p_link) const override;
 
 	/// Set the link's layers.
-	virtual void link_set_navigation_layers(RID p_link, uint32_t p_navigation_layers) override;
+	COMMAND_2(link_set_navigation_layers, RID, p_link, uint32_t, p_navigation_layers);
 	virtual uint32_t link_get_navigation_layers(RID p_link) const override;
 
 	/// Set the start position of the link.
-	virtual void link_set_start_position(RID p_link, Vector2 p_position) override;
+	COMMAND_2(link_set_start_position, RID, p_link, Vector2, p_position);
 	virtual Vector2 link_get_start_position(RID p_link) const override;
 
 	/// Set the end position of the link.
-	virtual void link_set_end_position(RID p_link, Vector2 p_position) override;
+	COMMAND_2(link_set_end_position, RID, p_link, Vector2, p_position);
 	virtual Vector2 link_get_end_position(RID p_link) const override;
 
 	/// Set the enter cost of the link.
-	virtual void link_set_enter_cost(RID p_link, real_t p_enter_cost) override;
+	COMMAND_2(link_set_enter_cost, RID, p_link, real_t, p_enter_cost);
 	virtual real_t link_get_enter_cost(RID p_link) const override;
 
 	/// Set the travel cost of the link.
-	virtual void link_set_travel_cost(RID p_link, real_t p_travel_cost) override;
+	COMMAND_2(link_set_travel_cost, RID, p_link, real_t, p_travel_cost);
 	virtual real_t link_get_travel_cost(RID p_link) const override;
 
 	/// Set the node which manages this link.
-	virtual void link_set_owner_id(RID p_link, ObjectID p_owner_id) override;
+	COMMAND_2(link_set_owner_id, RID, p_link, ObjectID, p_owner_id);
 	virtual ObjectID link_get_owner_id(RID p_link) const override;
 
 	/// Creates the agent.
 	virtual RID agent_create() override;
 
 	/// Put the agent in the map.
-	virtual void agent_set_map(RID p_agent, RID p_map) override;
+	COMMAND_2(agent_set_map, RID, p_agent, RID, p_map);
 	virtual RID agent_get_map(RID p_agent) const override;
 
-	virtual void agent_set_paused(RID p_agent, bool p_paused) override;
+	COMMAND_2(agent_set_paused, RID, p_agent, bool, p_paused);
 	virtual bool agent_get_paused(RID p_agent) const override;
 
-	virtual void agent_set_avoidance_enabled(RID p_agent, bool p_enabled) override;
+	COMMAND_2(agent_set_avoidance_enabled, RID, p_agent, bool, p_enabled);
 	virtual bool agent_get_avoidance_enabled(RID p_agent) const override;
 
 	/// The maximum distance (center point to
@@ -163,7 +232,7 @@ public:
 	/// time of the simulation. If the number is too
 	/// low, the simulation will not be safe.
 	/// Must be non-negative.
-	virtual void agent_set_neighbor_distance(RID p_agent, real_t p_distance) override;
+	COMMAND_2(agent_set_neighbor_distance, RID, p_agent, real_t, p_distance);
 	virtual real_t agent_get_neighbor_distance(RID p_agent) const override;
 
 	/// The maximum number of other agents this
@@ -172,7 +241,7 @@ public:
 	/// running time of the simulation. If the
 	/// number is too low, the simulation will not
 	/// be safe.
-	virtual void agent_set_max_neighbors(RID p_agent, int p_count) override;
+	COMMAND_2(agent_set_max_neighbors, RID, p_agent, int, p_count);
 	virtual int agent_get_max_neighbors(RID p_agent) const override;
 
 	/// The minimal amount of time for which this
@@ -183,73 +252,80 @@ public:
 	/// other agents, but the less freedom this
 	/// agent has in choosing its velocities.
 	/// Must be positive.
-	virtual void agent_set_time_horizon_agents(RID p_agent, real_t p_time_horizon) override;
+	COMMAND_2(agent_set_time_horizon_agents, RID, p_agent, real_t, p_time_horizon);
 	virtual real_t agent_get_time_horizon_agents(RID p_agent) const override;
-	virtual void agent_set_time_horizon_obstacles(RID p_agent, real_t p_time_horizon) override;
+	COMMAND_2(agent_set_time_horizon_obstacles, RID, p_agent, real_t, p_time_horizon);
 	virtual real_t agent_get_time_horizon_obstacles(RID p_agent) const override;
 
 	/// The radius of this agent.
 	/// Must be non-negative.
-	virtual void agent_set_radius(RID p_agent, real_t p_radius) override;
+	COMMAND_2(agent_set_radius, RID, p_agent, real_t, p_radius);
 	virtual real_t agent_get_radius(RID p_agent) const override;
 
 	/// The maximum speed of this agent.
 	/// Must be non-negative.
-	virtual void agent_set_max_speed(RID p_agent, real_t p_max_speed) override;
+	COMMAND_2(agent_set_max_speed, RID, p_agent, real_t, p_max_speed);
 	virtual real_t agent_get_max_speed(RID p_agent) const override;
 
 	/// forces and agent velocity change in the avoidance simulation, adds simulation instability if done recklessly
-	virtual void agent_set_velocity_forced(RID p_agent, Vector2 p_velocity) override;
+	COMMAND_2(agent_set_velocity_forced, RID, p_agent, Vector2, p_velocity);
 
 	/// The wanted velocity for the agent as a "suggestion" to the avoidance simulation.
 	/// The simulation will try to fulfill this velocity wish if possible but may change the velocity depending on other agent's and obstacles'.
-	virtual void agent_set_velocity(RID p_agent, Vector2 p_velocity) override;
+	COMMAND_2(agent_set_velocity, RID, p_agent, Vector2, p_velocity);
 	virtual Vector2 agent_get_velocity(RID p_agent) const override;
 
 	/// Position of the agent in world space.
-	virtual void agent_set_position(RID p_agent, Vector2 p_position) override;
+	COMMAND_2(agent_set_position, RID, p_agent, Vector2, p_position);
 	virtual Vector2 agent_get_position(RID p_agent) const override;
 
 	/// Returns true if the map got changed the previous frame.
 	virtual bool agent_is_map_changed(RID p_agent) const override;
 
 	/// Callback called at the end of the RVO process
-	virtual void agent_set_avoidance_callback(RID p_agent, Callable p_callback) override;
+	COMMAND_2(agent_set_avoidance_callback, RID, p_agent, Callable, p_callback);
 	virtual bool agent_has_avoidance_callback(RID p_agent) const override;
 
-	virtual void agent_set_avoidance_layers(RID p_agent, uint32_t p_layers) override;
+	COMMAND_2(agent_set_avoidance_layers, RID, p_agent, uint32_t, p_layers);
 	virtual uint32_t agent_get_avoidance_layers(RID p_agent) const override;
 
-	virtual void agent_set_avoidance_mask(RID p_agent, uint32_t p_mask) override;
+	COMMAND_2(agent_set_avoidance_mask, RID, p_agent, uint32_t, p_mask);
 	virtual uint32_t agent_get_avoidance_mask(RID p_agent) const override;
 
-	virtual void agent_set_avoidance_priority(RID p_agent, real_t p_priority) override;
+	COMMAND_2(agent_set_avoidance_priority, RID, p_agent, real_t, p_priority);
 	virtual real_t agent_get_avoidance_priority(RID p_agent) const override;
 
 	virtual RID obstacle_create() override;
-	virtual void obstacle_set_avoidance_enabled(RID p_obstacle, bool p_enabled) override;
+	COMMAND_2(obstacle_set_avoidance_enabled, RID, p_obstacle, bool, p_enabled);
 	virtual bool obstacle_get_avoidance_enabled(RID p_obstacle) const override;
-	virtual void obstacle_set_map(RID p_obstacle, RID p_map) override;
+	COMMAND_2(obstacle_set_map, RID, p_obstacle, RID, p_map);
 	virtual RID obstacle_get_map(RID p_obstacle) const override;
-	virtual void obstacle_set_paused(RID p_obstacle, bool p_paused) override;
+	COMMAND_2(obstacle_set_paused, RID, p_obstacle, bool, p_paused);
 	virtual bool obstacle_get_paused(RID p_obstacle) const override;
-	virtual void obstacle_set_radius(RID p_obstacle, real_t p_radius) override;
+	COMMAND_2(obstacle_set_radius, RID, p_obstacle, real_t, p_radius);
 	virtual real_t obstacle_get_radius(RID p_obstacle) const override;
-	virtual void obstacle_set_velocity(RID p_obstacle, Vector2 p_velocity) override;
+	COMMAND_2(obstacle_set_velocity, RID, p_obstacle, Vector2, p_velocity);
 	virtual Vector2 obstacle_get_velocity(RID p_obstacle) const override;
-	virtual void obstacle_set_position(RID p_obstacle, Vector2 p_position) override;
+	COMMAND_2(obstacle_set_position, RID, p_obstacle, Vector2, p_position);
 	virtual Vector2 obstacle_get_position(RID p_obstacle) const override;
-	virtual void obstacle_set_vertices(RID p_obstacle, const Vector<Vector2> &p_vertices) override;
+	COMMAND_2(obstacle_set_vertices, RID, p_obstacle, const Vector<Vector2> &, p_vertices);
 	virtual Vector<Vector2> obstacle_get_vertices(RID p_obstacle) const override;
-	virtual void obstacle_set_avoidance_layers(RID p_obstacle, uint32_t p_layers) override;
+	COMMAND_2(obstacle_set_avoidance_layers, RID, p_obstacle, uint32_t, p_layers);
 	virtual uint32_t obstacle_get_avoidance_layers(RID p_obstacle) const override;
 
-	virtual void query_path(const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result, const Callable &p_callback) override;
+	virtual void query_path(const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result, const Callable &p_callback = Callable()) override;
+
+	COMMAND_1(free, RID, p_object);
 
+	virtual void set_active(bool p_active) override;
+
+	void flush_queries();
+	virtual void process(real_t p_delta_time) override;
 	virtual void init() override;
 	virtual void sync() override;
 	virtual void finish() override;
-	virtual void free(RID p_object) override;
+
+	virtual int get_process_info(ProcessInfo p_info) const override;
 
 	virtual void parse_source_geometry_data(const Ref<NavigationPolygon> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData2D> &p_source_geometry_data, Node *p_root_node, const Callable &p_callback = Callable()) override;
 	virtual void bake_from_source_geometry_data(const Ref<NavigationPolygon> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData2D> &p_source_geometry_data, const Callable &p_callback = Callable()) override;
@@ -260,4 +336,11 @@ public:
 	virtual void source_geometry_parser_set_callback(RID p_parser, const Callable &p_callback) override;
 
 	virtual Vector<Vector2> simplify_path(const Vector<Vector2> &p_path, real_t p_epsilon) override;
+
+private:
+	void internal_free_agent(RID p_object);
+	void internal_free_obstacle(RID p_object);
 };
+
+#undef COMMAND_1
+#undef COMMAND_2

modules/navigation_2d/2d/nav_base_iteration_2d.h

@@ -0,0 +1,58 @@
+/**************************************************************************/
+/*  nav_base_iteration_2d.h                                               */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "../nav_utils_2d.h"
+
+#include "servers/navigation/navigation_utilities.h"
+
+struct NavBaseIteration2D {
+	uint32_t id = UINT32_MAX;
+	bool enabled = true;
+	uint32_t navigation_layers = 1;
+	real_t enter_cost = 0.0;
+	real_t travel_cost = 1.0;
+	NavigationUtilities::PathSegmentType owner_type;
+	ObjectID owner_object_id;
+	RID owner_rid;
+	bool owner_use_edge_connections = false;
+	LocalVector<nav_2d::Polygon> navmesh_polygons;
+
+	bool get_enabled() const { return enabled; }
+	NavigationUtilities::PathSegmentType get_type() const { return owner_type; }
+	RID get_self() const { return owner_rid; }
+	ObjectID get_owner_id() const { return owner_object_id; }
+	uint32_t get_navigation_layers() const { return navigation_layers; }
+	real_t get_enter_cost() const { return enter_cost; }
+	real_t get_travel_cost() const { return travel_cost; }
+	bool get_use_edge_connections() const { return owner_use_edge_connections; }
+	const LocalVector<nav_2d::Polygon> &get_navmesh_polygons() const { return navmesh_polygons; }
+};

modules/navigation_2d/2d/nav_map_builder_2d.cpp

@@ -0,0 +1,405 @@
+/**************************************************************************/
+/*  nav_map_builder_2d.cpp                                                */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "nav_map_builder_2d.h"
+
+#include "../nav_link_2d.h"
+#include "../nav_map_2d.h"
+#include "../nav_region_2d.h"
+#include "../triangle2.h"
+#include "nav_map_iteration_2d.h"
+#include "nav_region_iteration_2d.h"
+
+using namespace nav_2d;
+
+PointKey NavMapBuilder2D::get_point_key(const Vector2 &p_pos, const Vector2 &p_cell_size) {
+	const int x = static_cast<int>(Math::floor(p_pos.x / p_cell_size.x));
+	const int y = static_cast<int>(Math::floor(p_pos.y / p_cell_size.y));
+
+	PointKey p;
+	p.key = 0;
+	p.x = x;
+	p.y = y;
+	return p;
+}
+
+void NavMapBuilder2D::build_navmap_iteration(NavMapIterationBuild2D &r_build) {
+	PerformanceData &performance_data = r_build.performance_data;
+
+	performance_data.pm_polygon_count = 0;
+	performance_data.pm_edge_count = 0;
+	performance_data.pm_edge_merge_count = 0;
+	performance_data.pm_edge_connection_count = 0;
+	performance_data.pm_edge_free_count = 0;
+
+	_build_step_gather_region_polygons(r_build);
+
+	_build_step_find_edge_connection_pairs(r_build);
+
+	_build_step_merge_edge_connection_pairs(r_build);
+
+	_build_step_edge_connection_margin_connections(r_build);
+
+	_build_step_navlink_connections(r_build);
+
+	_build_update_map_iteration(r_build);
+}
+
+void NavMapBuilder2D::_build_step_gather_region_polygons(NavMapIterationBuild2D &r_build) {
+	PerformanceData &performance_data = r_build.performance_data;
+	NavMapIteration2D *map_iteration = r_build.map_iteration;
+
+	LocalVector<NavRegionIteration2D> &regions = map_iteration->region_iterations;
+	HashMap<uint32_t, LocalVector<Edge::Connection>> &region_external_connections = map_iteration->external_region_connections;
+
+	// Remove regions connections.
+	region_external_connections.clear();
+	for (const NavRegionIteration2D &region : regions) {
+		region_external_connections[region.id] = LocalVector<Edge::Connection>();
+	}
+
+	// Copy all region polygons in the map.
+	int polygon_count = 0;
+	for (NavRegionIteration2D &region : regions) {
+		if (!region.get_enabled()) {
+			continue;
+		}
+		LocalVector<Polygon> &polygons_source = region.navmesh_polygons;
+		for (uint32_t n = 0; n < polygons_source.size(); n++) {
+			polygons_source[n].id = polygon_count;
+			polygon_count++;
+		}
+	}
+
+	performance_data.pm_polygon_count = polygon_count;
+	r_build.polygon_count = polygon_count;
+}
+
+void NavMapBuilder2D::_build_step_find_edge_connection_pairs(NavMapIterationBuild2D &r_build) {
+	PerformanceData &performance_data = r_build.performance_data;
+	NavMapIteration2D *map_iteration = r_build.map_iteration;
+	int polygon_count = r_build.polygon_count;
+
+	HashMap<EdgeKey, EdgeConnectionPair, EdgeKey> &connection_pairs_map = r_build.iter_connection_pairs_map;
+
+	// Group all edges per key.
+	connection_pairs_map.clear();
+	connection_pairs_map.reserve(polygon_count);
+	int free_edges_count = 0; // How many ConnectionPairs have only one Connection.
+
+	for (NavRegionIteration2D &region : map_iteration->region_iterations) {
+		if (!region.get_enabled()) {
+			continue;
+		}
+
+		for (Polygon &poly : region.navmesh_polygons) {
+			for (uint32_t p = 0; p < poly.points.size(); p++) {
+				const int next_point = (p + 1) % poly.points.size();
+				const EdgeKey ek(poly.points[p].key, poly.points[next_point].key);
+
+				HashMap<EdgeKey, EdgeConnectionPair, EdgeKey>::Iterator pair_it = connection_pairs_map.find(ek);
+				if (!pair_it) {
+					pair_it = connection_pairs_map.insert(ek, EdgeConnectionPair());
+					performance_data.pm_edge_count += 1;
+					++free_edges_count;
+				}
+				EdgeConnectionPair &pair = pair_it->value;
+				if (pair.size < 2) {
+					// Add the polygon/edge tuple to this key.
+					Edge::Connection new_connection;
+					new_connection.polygon = &poly;
+					new_connection.edge = p;
+					new_connection.pathway_start = poly.points[p].pos;
+					new_connection.pathway_end = poly.points[next_point].pos;
+
+					pair.connections[pair.size] = new_connection;
+					++pair.size;
+					if (pair.size == 2) {
+						--free_edges_count;
+					}
+
+				} else {
+					// The edge is already connected with another edge, skip.
+					ERR_PRINT_ONCE("Navigation map synchronization error. Attempted to merge a navigation mesh polygon edge with another already-merged edge. This is usually caused by crossing edges, overlapping polygons, or a mismatch of the NavigationMesh / NavigationPolygon baked 'cell_size' and navigation map 'cell_size'. If you're certain none of above is the case, change 'navigation/3d/merge_rasterizer_cell_scale' to 0.001.");
+				}
+			}
+		}
+	}
+
+	r_build.free_edge_count = free_edges_count;
+}
+
+void NavMapBuilder2D::_build_step_merge_edge_connection_pairs(NavMapIterationBuild2D &r_build) {
+	PerformanceData &performance_data = r_build.performance_data;
+
+	HashMap<EdgeKey, EdgeConnectionPair, EdgeKey> &connection_pairs_map = r_build.iter_connection_pairs_map;
+	LocalVector<Edge::Connection> &free_edges = r_build.iter_free_edges;
+	int free_edges_count = r_build.free_edge_count;
+	bool use_edge_connections = r_build.use_edge_connections;
+
+	free_edges.clear();
+	free_edges.reserve(free_edges_count);
+
+	for (const KeyValue<EdgeKey, EdgeConnectionPair> &pair_it : connection_pairs_map) {
+		const EdgeConnectionPair &pair = pair_it.value;
+		if (pair.size == 2) {
+			// Connect edge that are shared in different polygons.
+			const Edge::Connection &c1 = pair.connections[0];
+			const Edge::Connection &c2 = pair.connections[1];
+			c1.polygon->edges[c1.edge].connections.push_back(c2);
+			c2.polygon->edges[c2.edge].connections.push_back(c1);
+			// Note: The pathway_start/end are full for those connection and do not need to be modified.
+			performance_data.pm_edge_merge_count += 1;
+		} else {
+			CRASH_COND_MSG(pair.size != 1, vformat("Number of connection != 1. Found: %d", pair.size));
+			if (use_edge_connections && pair.connections[0].polygon->owner->get_use_edge_connections()) {
+				free_edges.push_back(pair.connections[0]);
+			}
+		}
+	}
+}
+
+void NavMapBuilder2D::_build_step_edge_connection_margin_connections(NavMapIterationBuild2D &r_build) {
+	PerformanceData &performance_data = r_build.performance_data;
+	NavMapIteration2D *map_iteration = r_build.map_iteration;
+
+	real_t edge_connection_margin = r_build.edge_connection_margin;
+	LocalVector<Edge::Connection> &free_edges = r_build.iter_free_edges;
+	HashMap<uint32_t, LocalVector<Edge::Connection>> &region_external_connections = map_iteration->external_region_connections;
+
+	// Find the compatible near edges.
+	//
+	// Note:
+	// Considering that the edges must be compatible (for obvious reasons)
+	// to be connected, create new polygons to remove that small gap is
+	// not really useful and would result in wasteful computation during
+	// connection, integration and path finding.
+	performance_data.pm_edge_free_count = free_edges.size();
+
+	const real_t edge_connection_margin_squared = edge_connection_margin * edge_connection_margin;
+
+	for (uint32_t i = 0; i < free_edges.size(); i++) {
+		const Edge::Connection &free_edge = free_edges[i];
+		Vector2 edge_p1 = free_edge.polygon->points[free_edge.edge].pos;
+		Vector2 edge_p2 = free_edge.polygon->points[(free_edge.edge + 1) % free_edge.polygon->points.size()].pos;
+
+		for (uint32_t j = 0; j < free_edges.size(); j++) {
+			const Edge::Connection &other_edge = free_edges[j];
+			if (i == j || free_edge.polygon->owner == other_edge.polygon->owner) {
+				continue;
+			}
+
+			Vector2 other_edge_p1 = other_edge.polygon->points[other_edge.edge].pos;
+			Vector2 other_edge_p2 = other_edge.polygon->points[(other_edge.edge + 1) % other_edge.polygon->points.size()].pos;
+
+			// Compute the projection of the opposite edge on the current one.
+			Vector2 edge_vector = edge_p2 - edge_p1;
+			real_t projected_p1_ratio = edge_vector.dot(other_edge_p1 - edge_p1) / edge_vector.length_squared();
+			real_t projected_p2_ratio = edge_vector.dot(other_edge_p2 - edge_p1) / edge_vector.length_squared();
+			if ((projected_p1_ratio < 0.0 && projected_p2_ratio < 0.0) || (projected_p1_ratio > 1.0 && projected_p2_ratio > 1.0)) {
+				continue;
+			}
+
+			// Check if the two edges are close to each other enough and compute a pathway between the two regions.
+			Vector2 self1 = edge_vector * CLAMP(projected_p1_ratio, 0.0, 1.0) + edge_p1;
+			Vector2 other1;
+			if (projected_p1_ratio >= 0.0 && projected_p1_ratio <= 1.0) {
+				other1 = other_edge_p1;
+			} else {
+				other1 = other_edge_p1.lerp(other_edge_p2, (1.0 - projected_p1_ratio) / (projected_p2_ratio - projected_p1_ratio));
+			}
+			if (other1.distance_squared_to(self1) > edge_connection_margin_squared) {
+				continue;
+			}
+
+			Vector2 self2 = edge_vector * CLAMP(projected_p2_ratio, 0.0, 1.0) + edge_p1;
+			Vector2 other2;
+			if (projected_p2_ratio >= 0.0 && projected_p2_ratio <= 1.0) {
+				other2 = other_edge_p2;
+			} else {
+				other2 = other_edge_p1.lerp(other_edge_p2, (0.0 - projected_p1_ratio) / (projected_p2_ratio - projected_p1_ratio));
+			}
+			if (other2.distance_squared_to(self2) > edge_connection_margin_squared) {
+				continue;
+			}
+
+			// The edges can now be connected.
+			Edge::Connection new_connection = other_edge;
+			new_connection.pathway_start = (self1 + other1) / 2.0;
+			new_connection.pathway_end = (self2 + other2) / 2.0;
+			free_edge.polygon->edges[free_edge.edge].connections.push_back(new_connection);
+
+			// Add the connection to the region_connection map.
+			region_external_connections[(uint32_t)free_edge.polygon->owner->id].push_back(new_connection);
+			performance_data.pm_edge_connection_count += 1;
+		}
+	}
+}
+
+void NavMapBuilder2D::_build_step_navlink_connections(NavMapIterationBuild2D &r_build) {
+	NavMapIteration2D *map_iteration = r_build.map_iteration;
+
+	real_t link_connection_radius = r_build.link_connection_radius;
+	Vector2 merge_rasterizer_cell_size = r_build.merge_rasterizer_cell_size;
+
+	LocalVector<Polygon> &link_polygons = map_iteration->link_polygons;
+	LocalVector<NavLinkIteration2D> &links = map_iteration->link_iterations;
+	int polygon_count = r_build.polygon_count;
+
+	real_t link_connection_radius_sqr = link_connection_radius * link_connection_radius;
+	uint32_t link_poly_idx = 0;
+	link_polygons.resize(links.size());
+
+	// Search for polygons within range of a nav link.
+	for (const NavLinkIteration2D &link : links) {
+		if (!link.get_enabled()) {
+			continue;
+		}
+		const Vector2 link_start_pos = link.get_start_position();
+		const Vector2 link_end_pos = link.get_end_position();
+
+		Polygon *closest_start_polygon = nullptr;
+		real_t closest_start_sqr_dist = link_connection_radius_sqr;
+		Vector2 closest_start_point;
+
+		Polygon *closest_end_polygon = nullptr;
+		real_t closest_end_sqr_dist = link_connection_radius_sqr;
+		Vector2 closest_end_point;
+
+		for (NavRegionIteration2D &region : map_iteration->region_iterations) {
+			if (!region.get_enabled()) {
+				continue;
+			}
+			Rect2 region_bounds = region.get_bounds().grow(link_connection_radius);
+			if (!region_bounds.has_point(link_start_pos) && !region_bounds.has_point(link_end_pos)) {
+				continue;
+			}
+
+			for (Polygon &polyon : region.navmesh_polygons) {
+				for (uint32_t point_id = 2; point_id < polyon.points.size(); point_id += 1) {
+					const Triangle2 triangle(polyon.points[0].pos, polyon.points[point_id - 1].pos, polyon.points[point_id].pos);
+
+					{
+						const Vector2 start_point = triangle.get_closest_point_to(link_start_pos);
+						const real_t sqr_dist = start_point.distance_squared_to(link_start_pos);
+
+						// Pick the polygon that is within our radius and is closer than anything we've seen yet.
+						if (sqr_dist < closest_start_sqr_dist) {
+							closest_start_sqr_dist = sqr_dist;
+							closest_start_point = start_point;
+							closest_start_polygon = &polyon;
+						}
+					}
+
+					{
+						const Vector2 end_point = triangle.get_closest_point_to(link_end_pos);
+						const real_t sqr_dist = end_point.distance_squared_to(link_end_pos);
+
+						// Pick the polygon that is within our radius and is closer than anything we've seen yet.
+						if (sqr_dist < closest_end_sqr_dist) {
+							closest_end_sqr_dist = sqr_dist;
+							closest_end_point = end_point;
+							closest_end_polygon = &polyon;
+						}
+					}
+				}
+			}
+		}
+
+		// If we have both a start and end point, then create a synthetic polygon to route through.
+		if (closest_start_polygon && closest_end_polygon) {
+			Polygon &new_polygon = link_polygons[link_poly_idx++];
+			new_polygon.id = polygon_count++;
+			new_polygon.owner = &link;
+
+			new_polygon.edges.clear();
+			new_polygon.edges.resize(4);
+			new_polygon.points.resize(4);
+
+			// Build a set of vertices that create a thin polygon going from the start to the end point.
+			new_polygon.points[0] = { closest_start_point, get_point_key(closest_start_point, merge_rasterizer_cell_size) };
+			new_polygon.points[1] = { closest_start_point, get_point_key(closest_start_point, merge_rasterizer_cell_size) };
+			new_polygon.points[2] = { closest_end_point, get_point_key(closest_end_point, merge_rasterizer_cell_size) };
+			new_polygon.points[3] = { closest_end_point, get_point_key(closest_end_point, merge_rasterizer_cell_size) };
+
+			// Setup connections to go forward in the link.
+			{
+				Edge::Connection entry_connection;
+				entry_connection.polygon = &new_polygon;
+				entry_connection.edge = -1;
+				entry_connection.pathway_start = new_polygon.points[0].pos;
+				entry_connection.pathway_end = new_polygon.points[1].pos;
+				closest_start_polygon->edges[0].connections.push_back(entry_connection);
+
+				Edge::Connection exit_connection;
+				exit_connection.polygon = closest_end_polygon;
+				exit_connection.edge = -1;
+				exit_connection.pathway_start = new_polygon.points[2].pos;
+				exit_connection.pathway_end = new_polygon.points[3].pos;
+				new_polygon.edges[2].connections.push_back(exit_connection);
+			}
+
+			// If the link is bi-directional, create connections from the end to the start.
+			if (link.is_bidirectional()) {
+				Edge::Connection entry_connection;
+				entry_connection.polygon = &new_polygon;
+				entry_connection.edge = -1;
+				entry_connection.pathway_start = new_polygon.points[2].pos;
+				entry_connection.pathway_end = new_polygon.points[3].pos;
+				closest_end_polygon->edges[0].connections.push_back(entry_connection);
+
+				Edge::Connection exit_connection;
+				exit_connection.polygon = closest_start_polygon;
+				exit_connection.edge = -1;
+				exit_connection.pathway_start = new_polygon.points[0].pos;
+				exit_connection.pathway_end = new_polygon.points[1].pos;
+				new_polygon.edges[0].connections.push_back(exit_connection);
+			}
+		}
+	}
+}
+
+void NavMapBuilder2D::_build_update_map_iteration(NavMapIterationBuild2D &r_build) {
+	NavMapIteration2D *map_iteration = r_build.map_iteration;
+
+	LocalVector<Polygon> &link_polygons = map_iteration->link_polygons;
+
+	map_iteration->navmesh_polygon_count = r_build.polygon_count;
+	map_iteration->link_polygon_count = link_polygons.size();
+
+	map_iteration->path_query_slots_mutex.lock();
+	for (NavMeshQueries2D::PathQuerySlot &p_path_query_slot : map_iteration->path_query_slots) {
+		p_path_query_slot.traversable_polys.clear();
+		p_path_query_slot.traversable_polys.reserve(map_iteration->navmesh_polygon_count * 0.25);
+		p_path_query_slot.path_corridor.clear();
+		p_path_query_slot.path_corridor.resize(map_iteration->navmesh_polygon_count + map_iteration->link_polygon_count);
+	}
+	map_iteration->path_query_slots_mutex.unlock();
+}

modules/navigation_2d/2d/nav_map_builder_2d.h

@@ -0,0 +1,49 @@
+/**************************************************************************/
+/*  nav_map_builder_2d.h                                                  */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "../nav_utils_2d.h"
+
+struct NavMapIterationBuild2D;
+
+class NavMapBuilder2D {
+	static void _build_step_gather_region_polygons(NavMapIterationBuild2D &r_build);
+	static void _build_step_find_edge_connection_pairs(NavMapIterationBuild2D &r_build);
+	static void _build_step_merge_edge_connection_pairs(NavMapIterationBuild2D &r_build);
+	static void _build_step_edge_connection_margin_connections(NavMapIterationBuild2D &r_build);
+	static void _build_step_navlink_connections(NavMapIterationBuild2D &r_build);
+	static void _build_update_map_iteration(NavMapIterationBuild2D &r_build);
+
+public:
+	static nav_2d::PointKey get_point_key(const Vector2 &p_pos, const Vector2 &p_cell_size);
+
+	static void build_navmap_iteration(NavMapIterationBuild2D &r_build);
+};

modules/navigation_2d/2d/nav_map_iteration_2d.h

@@ -0,0 +1,110 @@
+/**************************************************************************/
+/*  nav_map_iteration_2d.h                                                */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "../nav_rid_2d.h"
+#include "../nav_utils_2d.h"
+#include "nav_mesh_queries_2d.h"
+
+#include "core/math/math_defs.h"
+#include "core/os/semaphore.h"
+
+struct NavLinkIteration2D;
+class NavRegion2D;
+struct NavRegionIteration2D;
+struct NavMapIteration2D;
+
+struct NavMapIterationBuild2D {
+	Vector2 merge_rasterizer_cell_size;
+	bool use_edge_connections = true;
+	real_t edge_connection_margin;
+	real_t link_connection_radius;
+	nav_2d::PerformanceData performance_data;
+	int polygon_count = 0;
+	int free_edge_count = 0;
+
+	HashMap<nav_2d::EdgeKey, nav_2d::EdgeConnectionPair, nav_2d::EdgeKey> iter_connection_pairs_map;
+	LocalVector<nav_2d::Edge::Connection> iter_free_edges;
+
+	NavMapIteration2D *map_iteration = nullptr;
+
+	int navmesh_polygon_count = 0;
+	int link_polygon_count = 0;
+
+	void reset() {
+		performance_data.reset();
+
+		iter_connection_pairs_map.clear();
+		iter_free_edges.clear();
+		polygon_count = 0;
+		free_edge_count = 0;
+
+		navmesh_polygon_count = 0;
+		link_polygon_count = 0;
+	}
+};
+
+struct NavMapIteration2D {
+	mutable SafeNumeric<uint32_t> users;
+	RWLock rwlock;
+
+	LocalVector<nav_2d::Polygon> link_polygons;
+
+	LocalVector<NavRegionIteration2D> region_iterations;
+	LocalVector<NavLinkIteration2D> link_iterations;
+
+	int navmesh_polygon_count = 0;
+	int link_polygon_count = 0;
+
+	// The edge connections that the map builds on top with the edge connection margin.
+	HashMap<uint32_t, LocalVector<nav_2d::Edge::Connection>> external_region_connections;
+
+	HashMap<NavRegion2D *, uint32_t> region_ptr_to_region_id;
+
+	LocalVector<NavMeshQueries2D::PathQuerySlot> path_query_slots;
+	Mutex path_query_slots_mutex;
+	Semaphore path_query_slots_semaphore;
+};
+
+class NavMapIterationRead2D {
+	const NavMapIteration2D &map_iteration;
+
+public:
+	_ALWAYS_INLINE_ NavMapIterationRead2D(const NavMapIteration2D &p_iteration) :
+			map_iteration(p_iteration) {
+		map_iteration.rwlock.read_lock();
+		map_iteration.users.increment();
+	}
+	_ALWAYS_INLINE_ ~NavMapIterationRead2D() {
+		map_iteration.users.decrement();
+		map_iteration.rwlock.read_unlock();
+	}
+};

modules/navigation_2d/2d/nav_mesh_queries_2d.cpp

@@ -0,0 +1,1062 @@
+/**************************************************************************/
+/*  nav_mesh_queries_2d.cpp                                               */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "nav_mesh_queries_2d.h"
+
+#include "../nav_base_2d.h"
+#include "../nav_map_2d.h"
+#include "../triangle2.h"
+#include "nav_region_iteration_2d.h"
+
+#include "core/math/geometry_2d.h"
+#include "servers/navigation/navigation_utilities.h"
+
+using namespace nav_2d;
+
+#define THREE_POINTS_CROSS_PRODUCT(m_a, m_b, m_c) (((m_c) - (m_a)).cross((m_b) - (m_a)))
+
+bool NavMeshQueries2D::emit_callback(const Callable &p_callback) {
+	ERR_FAIL_COND_V(!p_callback.is_valid(), false);
+
+	Callable::CallError ce;
+	Variant result;
+	p_callback.callp(nullptr, 0, result, ce);
+
+	return ce.error == Callable::CallError::CALL_OK;
+}
+
+Vector2 NavMeshQueries2D::polygons_get_random_point(const LocalVector<Polygon> &p_polygons, uint32_t p_navigation_layers, bool p_uniformly) {
+	const LocalVector<Polygon> &region_polygons = p_polygons;
+
+	if (region_polygons.is_empty()) {
+		return Vector2();
+	}
+
+	if (p_uniformly) {
+		real_t accumulated_area = 0;
+		RBMap<real_t, uint32_t> region_area_map;
+
+		for (uint32_t rp_index = 0; rp_index < region_polygons.size(); rp_index++) {
+			const Polygon &region_polygon = region_polygons[rp_index];
+			real_t polyon_area = region_polygon.surface_area;
+
+			if (polyon_area == 0.0) {
+				continue;
+			}
+			region_area_map[accumulated_area] = rp_index;
+			accumulated_area += polyon_area;
+		}
+		if (region_area_map.is_empty() || accumulated_area == 0) {
+			// All polygons have no real surface / no area.
+			return Vector2();
+		}
+
+		real_t region_area_map_pos = Math::random(real_t(0), accumulated_area);
+
+		RBMap<real_t, uint32_t>::Iterator region_E = region_area_map.find_closest(region_area_map_pos);
+		ERR_FAIL_COND_V(!region_E, Vector2());
+		uint32_t rrp_polygon_index = region_E->value;
+		ERR_FAIL_UNSIGNED_INDEX_V(rrp_polygon_index, region_polygons.size(), Vector2());
+
+		const Polygon &rr_polygon = region_polygons[rrp_polygon_index];
+
+		real_t accumulated_polygon_area = 0;
+		RBMap<real_t, uint32_t> polygon_area_map;
+
+		for (uint32_t rpp_index = 2; rpp_index < rr_polygon.points.size(); rpp_index++) {
+			real_t triangle_area = Triangle2(rr_polygon.points[0].pos, rr_polygon.points[rpp_index - 1].pos, rr_polygon.points[rpp_index].pos).get_area();
+
+			if (triangle_area == 0.0) {
+				continue;
+			}
+			polygon_area_map[accumulated_polygon_area] = rpp_index;
+			accumulated_polygon_area += triangle_area;
+		}
+		if (polygon_area_map.is_empty() || accumulated_polygon_area == 0) {
+			// All faces have no real surface / no area.
+			return Vector2();
+		}
+
+		real_t polygon_area_map_pos = Math::random(real_t(0), accumulated_polygon_area);
+
+		RBMap<real_t, uint32_t>::Iterator polygon_E = polygon_area_map.find_closest(polygon_area_map_pos);
+		ERR_FAIL_COND_V(!polygon_E, Vector2());
+		uint32_t rrp_face_index = polygon_E->value;
+		ERR_FAIL_UNSIGNED_INDEX_V(rrp_face_index, rr_polygon.points.size(), Vector2());
+
+		const Triangle2 triangle(rr_polygon.points[0].pos, rr_polygon.points[rrp_face_index - 1].pos, rr_polygon.points[rrp_face_index].pos);
+
+		Vector2 triangle_random_position = triangle.get_random_point_inside();
+		return triangle_random_position;
+
+	} else {
+		uint32_t rrp_polygon_index = Math::random(int(0), region_polygons.size() - 1);
+
+		const Polygon &rr_polygon = region_polygons[rrp_polygon_index];
+
+		uint32_t rrp_face_index = Math::random(int(2), rr_polygon.points.size() - 1);
+
+		const Triangle2 triangle(rr_polygon.points[0].pos, rr_polygon.points[rrp_face_index - 1].pos, rr_polygon.points[rrp_face_index].pos);
+
+		Vector2 triangle_random_position = triangle.get_random_point_inside();
+		return triangle_random_position;
+	}
+}
+
+void NavMeshQueries2D::_query_task_push_back_point_with_metadata(NavMeshPathQueryTask2D &p_query_task, const Vector2 &p_point, const Polygon *p_point_polygon) {
+	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) {
+		p_query_task.path_meta_point_types.push_back(p_point_polygon->owner->get_type());
+	}
+
+	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) {
+		p_query_task.path_meta_point_rids.push_back(p_point_polygon->owner->get_self());
+	}
+
+	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) {
+		p_query_task.path_meta_point_owners.push_back(p_point_polygon->owner->get_owner_id());
+	}
+
+	p_query_task.path_points.push_back(p_point);
+}
+
+void NavMeshQueries2D::map_query_path(NavMap2D *p_map, const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result, const Callable &p_callback) {
+	ERR_FAIL_NULL(p_map);
+	ERR_FAIL_COND(p_query_parameters.is_null());
+	ERR_FAIL_COND(p_query_result.is_null());
+
+	using namespace NavigationUtilities;
+
+	NavMeshQueries2D::NavMeshPathQueryTask2D query_task;
+	query_task.start_position = p_query_parameters->get_start_position();
+	query_task.target_position = p_query_parameters->get_target_position();
+	query_task.navigation_layers = p_query_parameters->get_navigation_layers();
+	query_task.callback = p_callback;
+
+	const TypedArray<RID> &_excluded_regions = p_query_parameters->get_excluded_regions();
+	const TypedArray<RID> &_included_regions = p_query_parameters->get_included_regions();
+
+	uint32_t _excluded_region_count = _excluded_regions.size();
+	uint32_t _included_region_count = _included_regions.size();
+
+	query_task.exclude_regions = _excluded_region_count > 0;
+	query_task.include_regions = _included_region_count > 0;
+
+	if (query_task.exclude_regions) {
+		query_task.excluded_regions.resize(_excluded_region_count);
+		for (uint32_t i = 0; i < _excluded_region_count; i++) {
+			query_task.excluded_regions[i] = _excluded_regions[i];
+		}
+	}
+
+	if (query_task.include_regions) {
+		query_task.included_regions.resize(_included_region_count);
+		for (uint32_t i = 0; i < _included_region_count; i++) {
+			query_task.included_regions[i] = _included_regions[i];
+		}
+	}
+
+	switch (p_query_parameters->get_pathfinding_algorithm()) {
+		case NavigationPathQueryParameters2D::PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR: {
+			query_task.pathfinding_algorithm = PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR;
+		} break;
+		default: {
+			WARN_PRINT("No match for used PathfindingAlgorithm - fallback to default");
+			query_task.pathfinding_algorithm = PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR;
+		} break;
+	}
+
+	switch (p_query_parameters->get_path_postprocessing()) {
+		case NavigationPathQueryParameters2D::PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL: {
+			query_task.path_postprocessing = PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL;
+		} break;
+		case NavigationPathQueryParameters2D::PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED: {
+			query_task.path_postprocessing = PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED;
+		} break;
+		case NavigationPathQueryParameters2D::PathPostProcessing::PATH_POSTPROCESSING_NONE: {
+			query_task.path_postprocessing = PathPostProcessing::PATH_POSTPROCESSING_NONE;
+		} break;
+		default: {
+			WARN_PRINT("No match for used PathPostProcessing - fallback to default");
+			query_task.path_postprocessing = PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL;
+		} break;
+	}
+
+	query_task.metadata_flags = (int64_t)p_query_parameters->get_metadata_flags();
+	query_task.simplify_path = p_query_parameters->get_simplify_path();
+	query_task.simplify_epsilon = p_query_parameters->get_simplify_epsilon();
+	query_task.status = NavMeshPathQueryTask2D::TaskStatus::QUERY_STARTED;
+
+	p_map->query_path(query_task);
+
+	p_query_result->set_data(
+			query_task.path_points,
+			query_task.path_meta_point_types,
+			query_task.path_meta_point_rids,
+			query_task.path_meta_point_owners);
+
+	if (query_task.callback.is_valid()) {
+		if (emit_callback(query_task.callback)) {
+			query_task.status = NavMeshPathQueryTask2D::TaskStatus::CALLBACK_DISPATCHED;
+		} else {
+			query_task.status = NavMeshPathQueryTask2D::TaskStatus::CALLBACK_FAILED;
+		}
+	}
+}
+
+void NavMeshQueries2D::_query_task_find_start_end_positions(NavMeshPathQueryTask2D &p_query_task, const NavMapIteration2D &p_map_iteration) {
+	real_t begin_d = FLT_MAX;
+	real_t end_d = FLT_MAX;
+
+	const LocalVector<NavRegionIteration2D> &regions = p_map_iteration.region_iterations;
+
+	for (const NavRegionIteration2D &region : regions) {
+		if (!region.get_enabled()) {
+			continue;
+		}
+
+		if (p_query_task.exclude_regions && p_query_task.excluded_regions.has(region.get_self())) {
+			continue;
+		}
+		if (p_query_task.include_regions && !p_query_task.included_regions.has(region.get_self())) {
+			continue;
+		}
+
+		// Find the initial poly and the end poly on this map.
+		for (const Polygon &p : region.get_navmesh_polygons()) {
+			// Only consider the polygon if it in a region with compatible layers.
+			if ((p_query_task.navigation_layers & p.owner->get_navigation_layers()) == 0) {
+				continue;
+			}
+
+			// For each triangle check the distance between the origin/destination.
+			for (size_t point_id = 2; point_id < p.points.size(); point_id++) {
+				const Triangle2 triangle(p.points[0].pos, p.points[point_id - 1].pos, p.points[point_id].pos);
+
+				Vector2 point = triangle.get_closest_point_to(p_query_task.start_position);
+				real_t distance_to_point = point.distance_to(p_query_task.start_position);
+				if (distance_to_point < begin_d) {
+					begin_d = distance_to_point;
+					p_query_task.begin_polygon = &p;
+					p_query_task.begin_position = point;
+				}
+
+				point = triangle.get_closest_point_to(p_query_task.target_position);
+				distance_to_point = point.distance_to(p_query_task.target_position);
+				if (distance_to_point < end_d) {
+					end_d = distance_to_point;
+					p_query_task.end_polygon = &p;
+					p_query_task.end_position = point;
+				}
+			}
+		}
+	}
+}
+
+void NavMeshQueries2D::_query_task_build_path_corridor(NavMeshPathQueryTask2D &p_query_task) {
+	const Vector2 p_target_position = p_query_task.target_position;
+	const Polygon *begin_poly = p_query_task.begin_polygon;
+	const Polygon *end_poly = p_query_task.end_polygon;
+	Vector2 begin_point = p_query_task.begin_position;
+	Vector2 end_point = p_query_task.end_position;
+
+	// Heap of polygons to travel next.
+	Heap<NavigationPoly *, NavPolyTravelCostGreaterThan, NavPolyHeapIndexer>
+			&traversable_polys = p_query_task.path_query_slot->traversable_polys;
+	traversable_polys.clear();
+
+	LocalVector<NavigationPoly> &navigation_polys = p_query_task.path_query_slot->path_corridor;
+	for (NavigationPoly &polygon : navigation_polys) {
+		polygon.reset();
+	}
+
+	// Initialize the matching navigation polygon.
+	NavigationPoly &begin_navigation_poly = navigation_polys[begin_poly->id];
+	begin_navigation_poly.poly = begin_poly;
+	begin_navigation_poly.entry = begin_point;
+	begin_navigation_poly.back_navigation_edge_pathway_start = begin_point;
+	begin_navigation_poly.back_navigation_edge_pathway_end = begin_point;
+	begin_navigation_poly.traveled_distance = 0.0;
+
+	// This is an implementation of the A* algorithm.
+	uint32_t least_cost_id = begin_poly->id;
+	bool found_route = false;
+
+	const Polygon *reachable_end = nullptr;
+	real_t distance_to_reachable_end = FLT_MAX;
+	bool is_reachable = true;
+	real_t poly_enter_cost = 0.0;
+
+	while (true) {
+		const NavigationPoly &least_cost_poly = navigation_polys[least_cost_id];
+		real_t poly_travel_cost = least_cost_poly.poly->owner->get_travel_cost();
+
+		// Takes the current least_cost_poly neighbors (iterating over its edges) and compute the traveled_distance.
+		for (const Edge &edge : least_cost_poly.poly->edges) {
+			// Iterate over connections in this edge, then compute the new optimized travel distance assigned to this polygon.
+			for (uint32_t connection_index = 0; connection_index < edge.connections.size(); connection_index++) {
+				const Edge::Connection &connection = edge.connections[connection_index];
+
+				const NavBaseIteration2D *connection_owner = connection.polygon->owner;
+				const bool owner_is_usable = _query_task_is_connection_owner_usable(p_query_task, connection_owner);
+				if (!owner_is_usable) {
+					continue;
+				}
+
+				Vector2 pathway[2] = { connection.pathway_start, connection.pathway_end };
+				const Vector2 new_entry = Geometry2D::get_closest_point_to_segment(least_cost_poly.entry, pathway);
+				const real_t new_traveled_distance = least_cost_poly.entry.distance_to(new_entry) * poly_travel_cost + poly_enter_cost + least_cost_poly.traveled_distance;
+
+				// Check if the neighbor polygon has already been processed.
+				NavigationPoly &neighbor_poly = navigation_polys[connection.polygon->id];
+				if (new_traveled_distance < neighbor_poly.traveled_distance) {
+					// Add the polygon to the heap of polygons to traverse next.
+					neighbor_poly.back_navigation_poly_id = least_cost_id;
+					neighbor_poly.back_navigation_edge = connection.edge;
+					neighbor_poly.back_navigation_edge_pathway_start = connection.pathway_start;
+					neighbor_poly.back_navigation_edge_pathway_end = connection.pathway_end;
+					neighbor_poly.traveled_distance = new_traveled_distance;
+					neighbor_poly.distance_to_destination =
+							new_entry.distance_to(end_point) *
+							connection_owner->get_travel_cost();
+					neighbor_poly.entry = new_entry;
+
+					if (neighbor_poly.traversable_poly_index != traversable_polys.INVALID_INDEX) {
+						traversable_polys.shift(neighbor_poly.traversable_poly_index);
+					} else {
+						neighbor_poly.poly = connection.polygon;
+						traversable_polys.push(&neighbor_poly);
+					}
+				}
+			}
+		}
+
+		poly_enter_cost = 0;
+		// When the heap of traversable polygons is empty at this point it means the end polygon is
+		// unreachable.
+		if (traversable_polys.is_empty()) {
+			// Thus use the further reachable polygon
+			ERR_BREAK_MSG(is_reachable == false, "It's not expect to not find the most reachable polygons");
+			is_reachable = false;
+			if (reachable_end == nullptr) {
+				// The path is not found and there is not a way out.
+				break;
+			}
+
+			// Set as end point the furthest reachable point.
+			end_poly = reachable_end;
+			real_t end_d = FLT_MAX;
+			for (size_t point_id = 2; point_id < end_poly->points.size(); point_id++) {
+				Triangle2 t(end_poly->points[0].pos, end_poly->points[point_id - 1].pos, end_poly->points[point_id].pos);
+				Vector2 spoint = t.get_closest_point_to(p_target_position);
+				real_t dpoint = spoint.distance_squared_to(p_target_position);
+				if (dpoint < end_d) {
+					end_point = spoint;
+					end_d = dpoint;
+				}
+			}
+
+			// Search all faces of start polygon as well.
+			bool closest_point_on_start_poly = false;
+			for (size_t point_id = 2; point_id < begin_poly->points.size(); point_id++) {
+				Triangle2 t(begin_poly->points[0].pos, begin_poly->points[point_id - 1].pos, begin_poly->points[point_id].pos);
+				Vector2 spoint = t.get_closest_point_to(p_target_position);
+				real_t dpoint = spoint.distance_squared_to(p_target_position);
+				if (dpoint < end_d) {
+					end_point = spoint;
+					end_d = dpoint;
+					closest_point_on_start_poly = true;
+				}
+			}
+
+			if (closest_point_on_start_poly) {
+				// No point to run PostProcessing when start and end convex polygon is the same.
+				p_query_task.path_clear();
+
+				_query_task_push_back_point_with_metadata(p_query_task, begin_point, begin_poly);
+				_query_task_push_back_point_with_metadata(p_query_task, end_point, begin_poly);
+				p_query_task.status = NavMeshPathQueryTask2D::TaskStatus::QUERY_FINISHED;
+				return;
+			}
+
+			for (NavigationPoly &nav_poly : navigation_polys) {
+				nav_poly.poly = nullptr;
+				nav_poly.traveled_distance = FLT_MAX;
+			}
+			navigation_polys[begin_poly->id].poly = begin_poly;
+			navigation_polys[begin_poly->id].traveled_distance = 0;
+			least_cost_id = begin_poly->id;
+			reachable_end = nullptr;
+		} else {
+			// Pop the polygon with the lowest travel cost from the heap of traversable polygons.
+			least_cost_id = traversable_polys.pop()->poly->id;
+
+			// Store the farthest reachable end polygon in case our goal is not reachable.
+			if (is_reachable) {
+				real_t distance = navigation_polys[least_cost_id].entry.distance_squared_to(p_target_position);
+				if (distance_to_reachable_end > distance) {
+					distance_to_reachable_end = distance;
+					reachable_end = navigation_polys[least_cost_id].poly;
+				}
+			}
+
+			// Check if we reached the end
+			if (navigation_polys[least_cost_id].poly == end_poly) {
+				found_route = true;
+				break;
+			}
+
+			if (navigation_polys[least_cost_id].poly->owner->get_self() != least_cost_poly.poly->owner->get_self()) {
+				poly_enter_cost = least_cost_poly.poly->owner->get_enter_cost();
+			}
+		}
+	}
+
+	// We did not find a route but we have both a start polygon and an end polygon at this point.
+	// Usually this happens because there was not a single external or internal connected edge, e.g. our start polygon is an isolated, single convex polygon.
+	if (!found_route) {
+		real_t end_d = FLT_MAX;
+		// Search all faces of the start polygon for the closest point to our target position.
+		for (size_t point_id = 2; point_id < begin_poly->points.size(); point_id++) {
+			Triangle2 t(begin_poly->points[0].pos, begin_poly->points[point_id - 1].pos, begin_poly->points[point_id].pos);
+			Vector2 spoint = t.get_closest_point_to(p_target_position);
+			real_t dpoint = spoint.distance_squared_to(p_target_position);
+			if (dpoint < end_d) {
+				end_point = spoint;
+				end_d = dpoint;
+			}
+		}
+
+		p_query_task.path_clear();
+
+		_query_task_push_back_point_with_metadata(p_query_task, begin_point, begin_poly);
+		_query_task_push_back_point_with_metadata(p_query_task, end_point, begin_poly);
+		p_query_task.status = NavMeshPathQueryTask2D::TaskStatus::QUERY_FINISHED;
+	} else {
+		p_query_task.end_position = end_point;
+		p_query_task.end_polygon = end_poly;
+		p_query_task.begin_position = begin_point;
+		p_query_task.begin_polygon = begin_poly;
+		p_query_task.least_cost_id = least_cost_id;
+	}
+}
+
+void NavMeshQueries2D::query_task_map_iteration_get_path(NavMeshPathQueryTask2D &p_query_task, const NavMapIteration2D &p_map_iteration) {
+	p_query_task.path_clear();
+
+	_query_task_find_start_end_positions(p_query_task, p_map_iteration);
+
+	// Check for trivial cases.
+	if (!p_query_task.begin_polygon || !p_query_task.end_polygon) {
+		p_query_task.status = NavMeshPathQueryTask2D::TaskStatus::QUERY_FINISHED;
+		return;
+	}
+	if (p_query_task.begin_polygon == p_query_task.end_polygon) {
+		p_query_task.path_clear();
+		_query_task_push_back_point_with_metadata(p_query_task, p_query_task.begin_position, p_query_task.begin_polygon);
+		_query_task_push_back_point_with_metadata(p_query_task, p_query_task.end_position, p_query_task.end_polygon);
+		p_query_task.status = NavMeshPathQueryTask2D::TaskStatus::QUERY_FINISHED;
+		return;
+	}
+
+	_query_task_build_path_corridor(p_query_task);
+
+	if (p_query_task.status == NavMeshPathQueryTask2D::TaskStatus::QUERY_FINISHED || p_query_task.status == NavMeshPathQueryTask2D::TaskStatus::QUERY_FAILED) {
+		return;
+	}
+
+	// Post-Process path.
+	switch (p_query_task.path_postprocessing) {
+		case PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL: {
+			_query_task_post_process_corridorfunnel(p_query_task);
+		} break;
+		case PathPostProcessing::PATH_POSTPROCESSING_EDGECENTERED: {
+			_query_task_post_process_edgecentered(p_query_task);
+		} break;
+		case PathPostProcessing::PATH_POSTPROCESSING_NONE: {
+			_query_task_post_process_nopostprocessing(p_query_task);
+		} break;
+		default: {
+			WARN_PRINT("No match for used PathPostProcessing - fallback to default");
+			_query_task_post_process_corridorfunnel(p_query_task);
+		} break;
+	}
+
+	p_query_task.path_reverse();
+
+	if (p_query_task.simplify_path) {
+		_query_task_simplified_path_points(p_query_task);
+	}
+
+#ifdef DEBUG_ENABLED
+	// Ensure post conditions as path meta arrays if used MUST match in array size with the path points.
+	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) {
+		DEV_ASSERT(p_query_task.path_points.size() == p_query_task.path_meta_point_types.size());
+	}
+
+	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) {
+		DEV_ASSERT(p_query_task.path_points.size() == p_query_task.path_meta_point_rids.size());
+	}
+
+	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) {
+		DEV_ASSERT(p_query_task.path_points.size() == p_query_task.path_meta_point_owners.size());
+	}
+#endif // DEBUG_ENABLED
+
+	p_query_task.status = NavMeshPathQueryTask2D::TaskStatus::QUERY_FINISHED;
+}
+
+void NavMeshQueries2D::_query_task_simplified_path_points(NavMeshPathQueryTask2D &p_query_task) {
+	if (!p_query_task.simplify_path || p_query_task.path_points.size() <= 2) {
+		return;
+	}
+
+	const LocalVector<uint32_t> &simplified_path_indices = NavMeshQueries2D::get_simplified_path_indices(p_query_task.path_points, p_query_task.simplify_epsilon);
+
+	uint32_t index_count = simplified_path_indices.size();
+
+	{
+		Vector2 *points_ptr = p_query_task.path_points.ptr();
+		for (uint32_t i = 0; i < index_count; i++) {
+			points_ptr[i] = points_ptr[simplified_path_indices[i]];
+		}
+		p_query_task.path_points.resize(index_count);
+	}
+
+	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_TYPES)) {
+		int32_t *types_ptr = p_query_task.path_meta_point_types.ptr();
+		for (uint32_t i = 0; i < index_count; i++) {
+			types_ptr[i] = types_ptr[simplified_path_indices[i]];
+		}
+		p_query_task.path_meta_point_types.resize(index_count);
+	}
+
+	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_RIDS)) {
+		RID *rids_ptr = p_query_task.path_meta_point_rids.ptr();
+		for (uint32_t i = 0; i < index_count; i++) {
+			rids_ptr[i] = rids_ptr[simplified_path_indices[i]];
+		}
+		p_query_task.path_meta_point_rids.resize(index_count);
+	}
+
+	if (p_query_task.metadata_flags.has_flag(PathMetadataFlags::PATH_INCLUDE_OWNERS)) {
+		int64_t *owners_ptr = p_query_task.path_meta_point_owners.ptr();
+		for (uint32_t i = 0; i < index_count; i++) {
+			owners_ptr[i] = owners_ptr[simplified_path_indices[i]];
+		}
+		p_query_task.path_meta_point_owners.resize(index_count);
+	}
+}
+
+void NavMeshQueries2D::_query_task_post_process_corridorfunnel(NavMeshPathQueryTask2D &p_query_task) {
+	Vector2 end_point = p_query_task.end_position;
+	const Polygon *end_poly = p_query_task.end_polygon;
+	Vector2 begin_point = p_query_task.begin_position;
+	const Polygon *begin_poly = p_query_task.begin_polygon;
+	uint32_t least_cost_id = p_query_task.least_cost_id;
+	LocalVector<NavigationPoly> &navigation_polys = p_query_task.path_query_slot->path_corridor;
+
+	// Set the apex poly/point to the end point.
+	NavigationPoly *apex_poly = &navigation_polys[least_cost_id];
+
+	Vector2 back_pathway[2] = { apex_poly->back_navigation_edge_pathway_start, apex_poly->back_navigation_edge_pathway_end };
+	const Vector2 back_edge_closest_point = Geometry2D::get_closest_point_to_segment(end_point, back_pathway);
+	if (end_point.is_equal_approx(back_edge_closest_point)) {
+		// The end point is basically on top of the last crossed edge, funneling around the corners would at best do nothing.
+		// At worst it would add an unwanted path point before the last point due to precision issues so skip to the next polygon.
+		if (apex_poly->back_navigation_poly_id != -1) {
+			apex_poly = &navigation_polys[apex_poly->back_navigation_poly_id];
+		}
+	}
+
+	Vector2 apex_point = end_point;
+
+	NavigationPoly *left_poly = apex_poly;
+	Vector2 left_portal = apex_point;
+	NavigationPoly *right_poly = apex_poly;
+	Vector2 right_portal = apex_point;
+
+	NavigationPoly *p = apex_poly;
+
+	_query_task_push_back_point_with_metadata(p_query_task, end_point, end_poly);
+
+	while (p) {
+		// Set left and right points of the pathway between polygons.
+		Vector2 left = p->back_navigation_edge_pathway_start;
+		Vector2 right = p->back_navigation_edge_pathway_end;
+		if (THREE_POINTS_CROSS_PRODUCT(apex_point, left, right) < 0) {
+			SWAP(left, right);
+		}
+
+		bool skip = false;
+		if (THREE_POINTS_CROSS_PRODUCT(apex_point, left_portal, left) >= 0) {
+			// Process.
+			if (left_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, left, right_portal) > 0) {
+				left_poly = p;
+				left_portal = left;
+			} else {
+				_query_task_clip_path(p_query_task, apex_poly, right_portal, right_poly);
+
+				apex_point = right_portal;
+				p = right_poly;
+				left_poly = p;
+				apex_poly = p;
+				left_portal = apex_point;
+				right_portal = apex_point;
+
+				_query_task_push_back_point_with_metadata(p_query_task, apex_point, apex_poly->poly);
+				skip = true;
+			}
+		}
+
+		if (!skip && THREE_POINTS_CROSS_PRODUCT(apex_point, right_portal, right) <= 0) {
+			// Process.
+			if (right_portal == apex_point || THREE_POINTS_CROSS_PRODUCT(apex_point, right, left_portal) < 0) {
+				right_poly = p;
+				right_portal = right;
+			} else {
+				_query_task_clip_path(p_query_task, apex_poly, left_portal, left_poly);
+
+				apex_point = left_portal;
+				p = left_poly;
+				right_poly = p;
+				apex_poly = p;
+				right_portal = apex_point;
+				left_portal = apex_point;
+
+				_query_task_push_back_point_with_metadata(p_query_task, apex_point, apex_poly->poly);
+			}
+		}
+
+		// Go to the previous polygon.
+		if (p->back_navigation_poly_id != -1) {
+			p = &navigation_polys[p->back_navigation_poly_id];
+		} else {
+			// The end
+			p = nullptr;
+		}
+	}
+
+	// If the last point is not the begin point, add it to the list.
+	if (p_query_task.path_points[p_query_task.path_points.size() - 1] != begin_point) {
+		_query_task_push_back_point_with_metadata(p_query_task, begin_point, begin_poly);
+	}
+}
+
+void NavMeshQueries2D::_query_task_post_process_edgecentered(NavMeshPathQueryTask2D &p_query_task) {
+	Vector2 end_point = p_query_task.end_position;
+	const Polygon *end_poly = p_query_task.end_polygon;
+	Vector2 begin_point = p_query_task.begin_position;
+	const Polygon *begin_poly = p_query_task.begin_polygon;
+	uint32_t least_cost_id = p_query_task.least_cost_id;
+	LocalVector<NavigationPoly> &navigation_polys = p_query_task.path_query_slot->path_corridor;
+
+	_query_task_push_back_point_with_metadata(p_query_task, end_point, end_poly);
+
+	// Add mid points.
+	int np_id = least_cost_id;
+	while (np_id != -1 && navigation_polys[np_id].back_navigation_poly_id != -1) {
+		if (navigation_polys[np_id].back_navigation_edge != -1) {
+			int prev = navigation_polys[np_id].back_navigation_edge;
+			int prev_n = (navigation_polys[np_id].back_navigation_edge + 1) % navigation_polys[np_id].poly->points.size();
+			Vector2 point = (navigation_polys[np_id].poly->points[prev].pos + navigation_polys[np_id].poly->points[prev_n].pos) * 0.5;
+
+			_query_task_push_back_point_with_metadata(p_query_task, point, navigation_polys[np_id].poly);
+		} else {
+			_query_task_push_back_point_with_metadata(p_query_task, navigation_polys[np_id].entry, navigation_polys[np_id].poly);
+		}
+
+		np_id = navigation_polys[np_id].back_navigation_poly_id;
+	}
+
+	_query_task_push_back_point_with_metadata(p_query_task, begin_point, begin_poly);
+}
+
+void NavMeshQueries2D::_query_task_post_process_nopostprocessing(NavMeshPathQueryTask2D &p_query_task) {
+	Vector2 end_point = p_query_task.end_position;
+	const Polygon *end_poly = p_query_task.end_polygon;
+	Vector2 begin_point = p_query_task.begin_position;
+	const Polygon *begin_poly = p_query_task.begin_polygon;
+	uint32_t least_cost_id = p_query_task.least_cost_id;
+	LocalVector<NavigationPoly> &navigation_polys = p_query_task.path_query_slot->path_corridor;
+
+	_query_task_push_back_point_with_metadata(p_query_task, end_point, end_poly);
+
+	// Add mid points.
+	int np_id = least_cost_id;
+	while (np_id != -1 && navigation_polys[np_id].back_navigation_poly_id != -1) {
+		_query_task_push_back_point_with_metadata(p_query_task, navigation_polys[np_id].entry, navigation_polys[np_id].poly);
+
+		np_id = navigation_polys[np_id].back_navigation_poly_id;
+	}
+
+	_query_task_push_back_point_with_metadata(p_query_task, begin_point, begin_poly);
+}
+
+Vector2 NavMeshQueries2D::map_iteration_get_closest_point(const NavMapIteration2D &p_map_iteration, const Vector2 &p_point) {
+	ClosestPointQueryResult cp = map_iteration_get_closest_point_info(p_map_iteration, p_point);
+	return cp.point;
+}
+
+RID NavMeshQueries2D::map_iteration_get_closest_point_owner(const NavMapIteration2D &p_map_iteration, const Vector2 &p_point) {
+	ClosestPointQueryResult cp = map_iteration_get_closest_point_info(p_map_iteration, p_point);
+	return cp.owner;
+}
+
+ClosestPointQueryResult NavMeshQueries2D::map_iteration_get_closest_point_info(const NavMapIteration2D &p_map_iteration, const Vector2 &p_point) {
+	ClosestPointQueryResult result;
+	real_t closest_point_distance_squared = FLT_MAX;
+
+	// TODO: Check for further 2D improvements.
+
+	const LocalVector<NavRegionIteration2D> &regions = p_map_iteration.region_iterations;
+	for (const NavRegionIteration2D &region : regions) {
+		for (const Polygon &polygon : region.get_navmesh_polygons()) {
+			real_t cross = (polygon.points[1].pos - polygon.points[0].pos).cross(polygon.points[2].pos - polygon.points[0].pos);
+			Vector2 closest_on_polygon;
+			real_t closest = FLT_MAX;
+			bool inside = true;
+			Vector2 previous = polygon.points[polygon.points.size() - 1].pos;
+			for (size_t point_id = 0; point_id < polygon.points.size(); ++point_id) {
+				Vector2 edge = polygon.points[point_id].pos - previous;
+				Vector2 to_point = p_point - previous;
+				real_t edge_to_point_cross = edge.cross(to_point);
+				bool clockwise = (edge_to_point_cross * cross) > 0;
+				// If we are not clockwise, the point will never be inside the polygon and so the closest point will be on an edge.
+				if (!clockwise) {
+					inside = false;
+					real_t point_projected_on_edge = edge.dot(to_point);
+					real_t edge_square = edge.length_squared();
+
+					if (point_projected_on_edge > edge_square) {
+						real_t distance = polygon.points[point_id].pos.distance_squared_to(p_point);
+						if (distance < closest) {
+							closest_on_polygon = polygon.points[point_id].pos;
+							closest = distance;
+						}
+					} else if (point_projected_on_edge < 0.0) {
+						real_t distance = previous.distance_squared_to(p_point);
+						if (distance < closest) {
+							closest_on_polygon = previous;
+							closest = distance;
+						}
+					} else {
+						// If we project on this edge, this will be the closest point.
+						real_t percent = point_projected_on_edge / edge_square;
+						closest_on_polygon = previous + percent * edge;
+						break;
+					}
+				}
+				previous = polygon.points[point_id].pos;
+			}
+
+			if (inside) {
+				closest_point_distance_squared = 0.0;
+				result.point = p_point;
+				result.owner = polygon.owner->get_self();
+
+				break;
+			} else {
+				real_t distance = closest_on_polygon.distance_squared_to(p_point);
+				if (distance < closest_point_distance_squared) {
+					closest_point_distance_squared = distance;
+					result.point = closest_on_polygon;
+					result.owner = polygon.owner->get_self();
+				}
+			}
+		}
+	}
+
+	return result;
+}
+
+Vector2 NavMeshQueries2D::map_iteration_get_random_point(const NavMapIteration2D &p_map_iteration, uint32_t p_navigation_layers, bool p_uniformly) {
+	if (p_map_iteration.region_iterations.is_empty()) {
+		return Vector2();
+	}
+
+	LocalVector<uint32_t> accessible_regions;
+	accessible_regions.reserve(p_map_iteration.region_iterations.size());
+
+	for (uint32_t i = 0; i < p_map_iteration.region_iterations.size(); i++) {
+		const NavRegionIteration2D &region = p_map_iteration.region_iterations[i];
+		if (!region.enabled || (p_navigation_layers & region.navigation_layers) == 0) {
+			continue;
+		}
+		accessible_regions.push_back(i);
+	}
+
+	if (accessible_regions.is_empty()) {
+		// All existing region polygons are disabled.
+		return Vector2();
+	}
+
+	if (p_uniformly) {
+		real_t accumulated_region_surface_area = 0;
+		RBMap<real_t, uint32_t> accessible_regions_area_map;
+
+		for (uint32_t accessible_region_index = 0; accessible_region_index < accessible_regions.size(); accessible_region_index++) {
+			const NavRegionIteration2D &region = p_map_iteration.region_iterations[accessible_regions[accessible_region_index]];
+
+			real_t region_surface_area = region.surface_area;
+
+			if (region_surface_area == 0.0f) {
+				continue;
+			}
+
+			accessible_regions_area_map[accumulated_region_surface_area] = accessible_region_index;
+			accumulated_region_surface_area += region_surface_area;
+		}
+		if (accessible_regions_area_map.is_empty() || accumulated_region_surface_area == 0) {
+			// All faces have no real surface / no area.
+			return Vector2();
+		}
+
+		real_t random_accessible_regions_area_map = Math::random(real_t(0), accumulated_region_surface_area);
+
+		RBMap<real_t, uint32_t>::Iterator E = accessible_regions_area_map.find_closest(random_accessible_regions_area_map);
+		ERR_FAIL_COND_V(!E, Vector2());
+		uint32_t random_region_index = E->value;
+		ERR_FAIL_UNSIGNED_INDEX_V(random_region_index, accessible_regions.size(), Vector2());
+
+		const NavRegionIteration2D &random_region = p_map_iteration.region_iterations[accessible_regions[random_region_index]];
+
+		return NavMeshQueries2D::polygons_get_random_point(random_region.navmesh_polygons, p_navigation_layers, p_uniformly);
+
+	} else {
+		uint32_t random_region_index = Math::random(int(0), accessible_regions.size() - 1);
+
+		const NavRegionIteration2D &random_region = p_map_iteration.region_iterations[accessible_regions[random_region_index]];
+
+		return NavMeshQueries2D::polygons_get_random_point(random_region.navmesh_polygons, p_navigation_layers, p_uniformly);
+	}
+}
+
+Vector2 NavMeshQueries2D::polygons_get_closest_point(const LocalVector<Polygon> &p_polygons, const Vector2 &p_point) {
+	ClosestPointQueryResult cp = polygons_get_closest_point_info(p_polygons, p_point);
+	return cp.point;
+}
+
+ClosestPointQueryResult NavMeshQueries2D::polygons_get_closest_point_info(const LocalVector<Polygon> &p_polygons, const Vector2 &p_point) {
+	ClosestPointQueryResult result;
+	real_t closest_point_distance_squared = FLT_MAX;
+
+	// TODO: Check for further 2D improvements.
+
+	for (const Polygon &polygon : p_polygons) {
+		real_t cross = (polygon.points[1].pos - polygon.points[0].pos).cross(polygon.points[2].pos - polygon.points[0].pos);
+		Vector2 closest_on_polygon;
+		real_t closest = FLT_MAX;
+		bool inside = true;
+		Vector2 previous = polygon.points[polygon.points.size() - 1].pos;
+		for (size_t point_id = 0; point_id < polygon.points.size(); ++point_id) {
+			Vector2 edge = polygon.points[point_id].pos - previous;
+			Vector2 to_point = p_point - previous;
+			real_t edge_to_point_cross = edge.cross(to_point);
+			bool clockwise = (edge_to_point_cross * cross) > 0;
+			// If we are not clockwise, the point will never be inside the polygon and so the closest point will be on an edge.
+			if (!clockwise) {
+				inside = false;
+				real_t point_projected_on_edge = edge.dot(to_point);
+				real_t edge_square = edge.length_squared();
+
+				if (point_projected_on_edge > edge_square) {
+					real_t distance = polygon.points[point_id].pos.distance_squared_to(p_point);
+					if (distance < closest) {
+						closest_on_polygon = polygon.points[point_id].pos;
+						closest = distance;
+					}
+				} else if (point_projected_on_edge < 0.0) {
+					real_t distance = previous.distance_squared_to(p_point);
+					if (distance < closest) {
+						closest_on_polygon = previous;
+						closest = distance;
+					}
+				} else {
+					// If we project on this edge, this will be the closest point.
+					real_t percent = point_projected_on_edge / edge_square;
+					closest_on_polygon = previous + percent * edge;
+					break;
+				}
+			}
+			previous = polygon.points[point_id].pos;
+		}
+
+		if (inside) {
+			closest_point_distance_squared = 0.0;
+			result.point = p_point;
+			result.owner = polygon.owner->get_self();
+			break;
+		} else {
+			real_t distance = closest_on_polygon.distance_squared_to(p_point);
+			if (distance < closest_point_distance_squared) {
+				closest_point_distance_squared = distance;
+				result.point = closest_on_polygon;
+				result.owner = polygon.owner->get_self();
+			}
+		}
+	}
+
+	return result;
+}
+
+RID NavMeshQueries2D::polygons_get_closest_point_owner(const LocalVector<Polygon> &p_polygons, const Vector2 &p_point) {
+	ClosestPointQueryResult cp = polygons_get_closest_point_info(p_polygons, p_point);
+	return cp.owner;
+}
+
+static bool _line_intersects_segment(const Vector2 &p_line_normal, real_t p_line_d, const Vector2 &p_segment_begin, const Vector2 &p_segment_end, Vector2 &r_intersection) {
+	Vector2 segment = p_segment_begin - p_segment_end;
+	real_t den = p_line_normal.dot(segment);
+
+	if (Math::is_zero_approx(den)) {
+		return false;
+	}
+
+	real_t dist = (p_line_normal.dot(p_segment_begin) - p_line_d) / den;
+
+	if (dist < (real_t)-CMP_EPSILON || dist > (1.0 + (real_t)CMP_EPSILON)) {
+		return false;
+	}
+
+	r_intersection = p_segment_begin - segment * dist;
+	return true;
+}
+
+void NavMeshQueries2D::_query_task_clip_path(NavMeshPathQueryTask2D &p_query_task, const NavigationPoly *p_from_poly, const Vector2 &p_to_point, const NavigationPoly *p_to_poly) {
+	Vector2 from = p_query_task.path_points[p_query_task.path_points.size() - 1];
+	const LocalVector<NavigationPoly> &p_navigation_polys = p_query_task.path_query_slot->path_corridor;
+
+	if (from.is_equal_approx(p_to_point)) {
+		return;
+	}
+
+	// Compute line parameters (equivalent to the Plane case in 3D).
+	const Vector2 normal = -(from - p_to_point).orthogonal().normalized();
+	const real_t d = normal.dot(from);
+
+	while (p_from_poly != p_to_poly) {
+		Vector2 pathway_start = p_from_poly->back_navigation_edge_pathway_start;
+		Vector2 pathway_end = p_from_poly->back_navigation_edge_pathway_end;
+
+		ERR_FAIL_COND(p_from_poly->back_navigation_poly_id == -1);
+		p_from_poly = &p_navigation_polys[p_from_poly->back_navigation_poly_id];
+
+		if (!pathway_start.is_equal_approx(pathway_end)) {
+			Vector2 inters;
+			if (_line_intersects_segment(normal, d, pathway_start, pathway_end, inters)) {
+				if (!inters.is_equal_approx(p_to_point) && !inters.is_equal_approx(p_query_task.path_points[p_query_task.path_points.size() - 1])) {
+					_query_task_push_back_point_with_metadata(p_query_task, inters, p_from_poly->poly);
+				}
+			}
+		}
+	}
+}
+
+bool NavMeshQueries2D::_query_task_is_connection_owner_usable(const NavMeshPathQueryTask2D &p_query_task, const NavBaseIteration2D *p_owner) {
+	bool owner_usable = true;
+
+	if ((p_query_task.navigation_layers & p_owner->get_navigation_layers()) == 0) {
+		// Not usable. No matching bit between task filter bitmask and owner bitmask.
+		owner_usable = false;
+		return owner_usable;
+	}
+
+	if (p_query_task.exclude_regions || p_query_task.include_regions) {
+		switch (p_owner->get_type()) {
+			case NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_REGION: {
+				if (p_query_task.exclude_regions && p_query_task.excluded_regions.has(p_owner->get_self())) {
+					// Not usable. Exclude region filter is active and this region is excluded.
+					owner_usable = false;
+				} else if (p_query_task.include_regions && !p_query_task.included_regions.has(p_owner->get_self())) {
+					// Not usable. Include region filter is active and this region is not included.
+					owner_usable = false;
+				}
+			} break;
+			case NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_LINK: {
+				const LocalVector<Polygon> &link_polygons = p_owner->get_navmesh_polygons();
+				if (link_polygons.size() != 2) {
+					// Not usable. Whatever this is, it is not a valid connected link.
+					owner_usable = false;
+				} else {
+					const RID link_start_region = link_polygons[0].owner->get_self();
+					const RID link_end_region = link_polygons[1].owner->get_self();
+					if (p_query_task.exclude_regions && (p_query_task.excluded_regions.has(link_start_region) || p_query_task.excluded_regions.has(link_end_region))) {
+						// Not usable. Exclude region filter is active and at least one region of the link is excluded.
+						owner_usable = false;
+					}
+					if (p_query_task.include_regions && (!p_query_task.included_regions.has(link_start_region) || !p_query_task.excluded_regions.has(link_end_region))) {
+						// Not usable. Include region filter is active and not both regions of the links are included.
+						owner_usable = false;
+					}
+				}
+			} break;
+		}
+	}
+
+	return owner_usable;
+}
+
+LocalVector<uint32_t> NavMeshQueries2D::get_simplified_path_indices(const LocalVector<Vector2> &p_path, real_t p_epsilon) {
+	p_epsilon = MAX(0.0, p_epsilon);
+	real_t squared_epsilon = p_epsilon * p_epsilon;
+
+	LocalVector<uint32_t> simplified_path_indices;
+	simplified_path_indices.reserve(p_path.size());
+	simplified_path_indices.push_back(0);
+	simplify_path_segment(0, p_path.size() - 1, p_path, squared_epsilon, simplified_path_indices);
+	simplified_path_indices.push_back(p_path.size() - 1);
+
+	return simplified_path_indices;
+}
+
+void NavMeshQueries2D::simplify_path_segment(int p_start_inx, int p_end_inx, const LocalVector<Vector2> &p_points, real_t p_epsilon, LocalVector<uint32_t> &r_simplified_path_indices) {
+	Vector2 path_segment[2] = { p_points[p_start_inx], p_points[p_end_inx] };
+
+	real_t point_max_distance = 0.0;
+	int point_max_index = 0;
+
+	for (int i = p_start_inx; i < p_end_inx; i++) {
+		const Vector2 &checked_point = p_points[i];
+
+		const Vector2 closest_point = Geometry2D::get_closest_point_to_segment(checked_point, path_segment);
+		real_t distance_squared = closest_point.distance_squared_to(checked_point);
+
+		if (distance_squared > point_max_distance) {
+			point_max_index = i;
+			point_max_distance = distance_squared;
+		}
+	}
+
+	if (point_max_distance > p_epsilon) {
+		simplify_path_segment(p_start_inx, point_max_index, p_points, p_epsilon, r_simplified_path_indices);
+		r_simplified_path_indices.push_back(point_max_index);
+		simplify_path_segment(point_max_index, p_end_inx, p_points, p_epsilon, r_simplified_path_indices);
+	}
+}

modules/navigation_2d/2d/nav_mesh_queries_2d.h

@@ -0,0 +1,141 @@
+/**************************************************************************/
+/*  nav_mesh_queries_2d.h                                                 */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "../nav_utils_2d.h"
+
+#include "servers/navigation/navigation_path_query_parameters_2d.h"
+#include "servers/navigation/navigation_path_query_result_2d.h"
+#include "servers/navigation/navigation_utilities.h"
+
+using namespace NavigationUtilities;
+
+class NavMap2D;
+struct NavMapIteration2D;
+
+class NavMeshQueries2D {
+public:
+	struct PathQuerySlot {
+		LocalVector<nav_2d::NavigationPoly> path_corridor;
+		Heap<nav_2d::NavigationPoly *, nav_2d::NavPolyTravelCostGreaterThan, nav_2d::NavPolyHeapIndexer> traversable_polys;
+		bool in_use = false;
+		uint32_t slot_index = 0;
+	};
+
+	struct NavMeshPathQueryTask2D {
+		enum TaskStatus {
+			QUERY_STARTED,
+			QUERY_FINISHED,
+			QUERY_FAILED,
+			CALLBACK_DISPATCHED,
+			CALLBACK_FAILED,
+		};
+
+		// Parameters.
+		Vector2 start_position;
+		Vector2 target_position;
+		uint32_t navigation_layers;
+		BitField<PathMetadataFlags> metadata_flags = PathMetadataFlags::PATH_INCLUDE_ALL;
+		PathfindingAlgorithm pathfinding_algorithm = PathfindingAlgorithm::PATHFINDING_ALGORITHM_ASTAR;
+		PathPostProcessing path_postprocessing = PathPostProcessing::PATH_POSTPROCESSING_CORRIDORFUNNEL;
+		bool simplify_path = false;
+		real_t simplify_epsilon = 0.0;
+		bool exclude_regions = false;
+		bool include_regions = false;
+		LocalVector<RID> excluded_regions;
+		LocalVector<RID> included_regions;
+
+		// Path building.
+		Vector2 begin_position;
+		Vector2 end_position;
+		const nav_2d::Polygon *begin_polygon = nullptr;
+		const nav_2d::Polygon *end_polygon = nullptr;
+		uint32_t least_cost_id = 0;
+
+		// Map.
+		NavMap2D *map = nullptr;
+		PathQuerySlot *path_query_slot = nullptr;
+
+		// Path points.
+		LocalVector<Vector2> path_points;
+		LocalVector<int32_t> path_meta_point_types;
+		LocalVector<RID> path_meta_point_rids;
+		LocalVector<int64_t> path_meta_point_owners;
+
+		Ref<NavigationPathQueryParameters2D> query_parameters;
+		Ref<NavigationPathQueryResult2D> query_result;
+		Callable callback;
+		NavMeshPathQueryTask2D::TaskStatus status = NavMeshPathQueryTask2D::TaskStatus::QUERY_STARTED;
+
+		void path_clear() {
+			path_points.clear();
+			path_meta_point_types.clear();
+			path_meta_point_rids.clear();
+			path_meta_point_owners.clear();
+		}
+
+		void path_reverse() {
+			path_points.invert();
+			path_meta_point_types.invert();
+			path_meta_point_rids.invert();
+			path_meta_point_owners.invert();
+		}
+	};
+
+	static bool emit_callback(const Callable &p_callback);
+
+	static Vector2 polygons_get_random_point(const LocalVector<nav_2d::Polygon> &p_polygons, uint32_t p_navigation_layers, bool p_uniformly);
+
+	static Vector2 polygons_get_closest_point(const LocalVector<nav_2d::Polygon> &p_polygons, const Vector2 &p_point);
+	static nav_2d::ClosestPointQueryResult polygons_get_closest_point_info(const LocalVector<nav_2d::Polygon> &p_polygons, const Vector2 &p_point);
+	static RID polygons_get_closest_point_owner(const LocalVector<nav_2d::Polygon> &p_polygons, const Vector2 &p_point);
+
+	static Vector2 map_iteration_get_closest_point(const NavMapIteration2D &p_map_iteration, const Vector2 &p_point);
+	static RID map_iteration_get_closest_point_owner(const NavMapIteration2D &p_map_iteration, const Vector2 &p_point);
+	static nav_2d::ClosestPointQueryResult map_iteration_get_closest_point_info(const NavMapIteration2D &p_map_iteration, const Vector2 &p_point);
+	static Vector2 map_iteration_get_random_point(const NavMapIteration2D &p_map_iteration, uint32_t p_navigation_layers, bool p_uniformly);
+
+	static void map_query_path(NavMap2D *p_map, const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result, const Callable &p_callback);
+
+	static void query_task_map_iteration_get_path(NavMeshPathQueryTask2D &p_query_task, const NavMapIteration2D &p_map_iteration);
+	static void _query_task_push_back_point_with_metadata(NavMeshPathQueryTask2D &p_query_task, const Vector2 &p_point, const nav_2d::Polygon *p_point_polygon);
+	static void _query_task_find_start_end_positions(NavMeshPathQueryTask2D &p_query_task, const NavMapIteration2D &p_map_iteration);
+	static void _query_task_build_path_corridor(NavMeshPathQueryTask2D &p_query_task);
+	static void _query_task_post_process_corridorfunnel(NavMeshPathQueryTask2D &p_query_task);
+	static void _query_task_post_process_edgecentered(NavMeshPathQueryTask2D &p_query_task);
+	static void _query_task_post_process_nopostprocessing(NavMeshPathQueryTask2D &p_query_task);
+	static void _query_task_clip_path(NavMeshPathQueryTask2D &p_query_task, const nav_2d::NavigationPoly *p_from_poly, const Vector2 &p_to_point, const nav_2d::NavigationPoly *p_to_poly);
+	static void _query_task_simplified_path_points(NavMeshPathQueryTask2D &p_query_task);
+	static bool _query_task_is_connection_owner_usable(const NavMeshPathQueryTask2D &p_query_task, const NavBaseIteration2D *p_owner);
+
+	static void simplify_path_segment(int p_start_inx, int p_end_inx, const LocalVector<Vector2> &p_points, real_t p_epsilon, LocalVector<uint32_t> &r_simplified_path_indices);
+	static LocalVector<uint32_t> get_simplified_path_indices(const LocalVector<Vector2> &p_path, real_t p_epsilon);
+};

modules/navigation_2d/2d/nav_region_iteration_2d.h

@@ -0,0 +1,46 @@
+/**************************************************************************/
+/*  nav_region_iteration_2d.h                                             */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "../nav_utils_2d.h"
+#include "nav_base_iteration_2d.h"
+
+#include "core/math/rect2.h"
+
+struct NavRegionIteration2D : NavBaseIteration2D {
+	Transform2D transform;
+	real_t surface_area = 0.0;
+	Rect2 bounds;
+
+	const Transform2D &get_transform() const { return transform; }
+	real_t get_surface_area() const { return surface_area; }
+	Rect2 get_bounds() const { return bounds; }
+};

modules/navigation_2d/SCsub

@@ -0,0 +1,42 @@
+#!/usr/bin/env python
+from misc.utility.scons_hints import *
+
+Import("env")
+Import("env_modules")
+
+env_navigation_2d = env_modules.Clone()
+
+# Thirdparty source files
+
+thirdparty_obj = []
+
+# RVO 2D Thirdparty source files
+if env["builtin_rvo2_2d"]:
+    thirdparty_dir = "#thirdparty/rvo2/rvo2_2d/"
+    thirdparty_sources = [
+        "Agent2d.cpp",
+        "Obstacle2d.cpp",
+        "KdTree2d.cpp",
+        "RVOSimulator2d.cpp",
+    ]
+    thirdparty_sources = [thirdparty_dir + file for file in thirdparty_sources]
+
+    env_navigation_2d.Prepend(CPPPATH=[thirdparty_dir])
+
+    env_thirdparty = env_navigation_2d.Clone()
+    env_thirdparty.disable_warnings()
+    env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources)
+
+
+env.modules_sources += thirdparty_obj
+
+# Godot source files
+
+module_obj = []
+
+env_navigation_2d.add_source_files(module_obj, "*.cpp")
+env_navigation_2d.add_source_files(module_obj, "2d/*.cpp")
+env.modules_sources += module_obj
+
+# Needed to force rebuilding the module files when the thirdparty library is updated.
+env.Depends(module_obj, thirdparty_obj)

modules/navigation_2d/config.py

@@ -0,0 +1,6 @@
+def can_build(env, platform):
+    return True
+
+
+def configure(env):
+    pass

modules/navigation_2d/nav_agent_2d.cpp

@@ -0,0 +1,315 @@
+/**************************************************************************/
+/*  nav_agent_2d.cpp                                                      */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "nav_agent_2d.h"
+
+#include "nav_map_2d.h"
+
+void NavAgent2D::set_avoidance_enabled(bool p_enabled) {
+	avoidance_enabled = p_enabled;
+	_update_rvo_agent_properties();
+}
+
+void NavAgent2D::_update_rvo_agent_properties() {
+	rvo_agent.neighborDist_ = neighbor_distance;
+	rvo_agent.maxNeighbors_ = max_neighbors;
+	rvo_agent.timeHorizon_ = time_horizon_agents;
+	rvo_agent.timeHorizonObst_ = time_horizon_obstacles;
+	rvo_agent.radius_ = radius;
+	rvo_agent.maxSpeed_ = max_speed;
+	rvo_agent.position_ = RVO2D::Vector2(position.x, position.y);
+	// Replacing the internal velocity directly causes major jitter / bugs due to unpredictable velocity jumps, left line here for testing.
+	//rvo_agent.velocity_ = RVO2D::Vector2(velocity.x, velocity.y);
+	rvo_agent.prefVelocity_ = RVO2D::Vector2(velocity.x, velocity.y);
+	rvo_agent.avoidance_layers_ = avoidance_layers;
+	rvo_agent.avoidance_mask_ = avoidance_mask;
+	rvo_agent.avoidance_priority_ = avoidance_priority;
+
+	if (map != nullptr) {
+		if (avoidance_enabled) {
+			map->set_agent_as_controlled(this);
+		} else {
+			map->remove_agent_as_controlled(this);
+		}
+	}
+	agent_dirty = true;
+
+	request_sync();
+}
+
+void NavAgent2D::set_map(NavMap2D *p_map) {
+	if (map == p_map) {
+		return;
+	}
+
+	cancel_sync_request();
+
+	if (map) {
+		map->remove_agent(this);
+	}
+
+	map = p_map;
+	agent_dirty = true;
+
+	if (map) {
+		map->add_agent(this);
+		if (avoidance_enabled) {
+			map->set_agent_as_controlled(this);
+		}
+
+		request_sync();
+	}
+}
+
+bool NavAgent2D::is_map_changed() {
+	if (map) {
+		bool is_changed = map->get_iteration_id() != last_map_iteration_id;
+		last_map_iteration_id = map->get_iteration_id();
+		return is_changed;
+	} else {
+		return false;
+	}
+}
+
+void NavAgent2D::set_avoidance_callback(Callable p_callback) {
+	avoidance_callback = p_callback;
+}
+
+bool NavAgent2D::has_avoidance_callback() const {
+	return avoidance_callback.is_valid();
+}
+
+void NavAgent2D::dispatch_avoidance_callback() {
+	if (!avoidance_callback.is_valid()) {
+		return;
+	}
+
+	Vector3 new_velocity;
+
+	new_velocity = Vector3(rvo_agent.velocity_.x(), 0.0, rvo_agent.velocity_.y());
+
+	if (clamp_speed) {
+		new_velocity = new_velocity.limit_length(max_speed);
+	}
+
+	// Invoke the callback with the new velocity.
+	avoidance_callback.call(new_velocity);
+}
+
+void NavAgent2D::set_neighbor_distance(real_t p_neighbor_distance) {
+	neighbor_distance = p_neighbor_distance;
+	rvo_agent.neighborDist_ = neighbor_distance;
+	agent_dirty = true;
+
+	request_sync();
+}
+
+void NavAgent2D::set_max_neighbors(int p_max_neighbors) {
+	max_neighbors = p_max_neighbors;
+	rvo_agent.maxNeighbors_ = max_neighbors;
+	agent_dirty = true;
+
+	request_sync();
+}
+
+void NavAgent2D::set_time_horizon_agents(real_t p_time_horizon) {
+	time_horizon_agents = p_time_horizon;
+	rvo_agent.timeHorizon_ = time_horizon_agents;
+	agent_dirty = true;
+
+	request_sync();
+}
+
+void NavAgent2D::set_time_horizon_obstacles(real_t p_time_horizon) {
+	time_horizon_obstacles = p_time_horizon;
+	rvo_agent.timeHorizonObst_ = time_horizon_obstacles;
+	agent_dirty = true;
+
+	request_sync();
+}
+
+void NavAgent2D::set_radius(real_t p_radius) {
+	radius = p_radius;
+	rvo_agent.radius_ = radius;
+	agent_dirty = true;
+
+	request_sync();
+}
+
+void NavAgent2D::set_max_speed(real_t p_max_speed) {
+	max_speed = p_max_speed;
+	if (avoidance_enabled) {
+		rvo_agent.maxSpeed_ = max_speed;
+	}
+	agent_dirty = true;
+
+	request_sync();
+}
+
+void NavAgent2D::set_position(const Vector2 &p_position) {
+	position = p_position;
+	if (avoidance_enabled) {
+		rvo_agent.position_ = RVO2D::Vector2(p_position.x, p_position.y);
+	}
+	agent_dirty = true;
+
+	request_sync();
+}
+
+void NavAgent2D::set_target_position(const Vector2 &p_target_position) {
+	target_position = p_target_position;
+}
+
+void NavAgent2D::set_velocity(const Vector2 &p_velocity) {
+	// Sets the "wanted" velocity for an agent as a suggestion
+	// This velocity is not guaranteed, RVO simulation will only try to fulfill it
+	velocity = p_velocity;
+	if (avoidance_enabled) {
+		rvo_agent.prefVelocity_ = RVO2D::Vector2(velocity.x, velocity.y);
+	}
+	agent_dirty = true;
+
+	request_sync();
+}
+
+void NavAgent2D::set_velocity_forced(const Vector2 &p_velocity) {
+	// This function replaces the internal rvo simulation velocity
+	// should only be used after the agent was teleported
+	// as it destroys consistency in movement in cramped situations
+	// use velocity instead to update with a safer "suggestion"
+	velocity_forced = p_velocity;
+	if (avoidance_enabled) {
+		rvo_agent.velocity_ = RVO2D::Vector2(p_velocity.x, p_velocity.y);
+	}
+	agent_dirty = true;
+
+	request_sync();
+}
+
+void NavAgent2D::update() {
+	// Updates this agent with the calculated results from the rvo simulation
+	if (avoidance_enabled) {
+		velocity = Vector2(rvo_agent.velocity_.x(), rvo_agent.velocity_.y());
+	}
+}
+
+void NavAgent2D::set_avoidance_mask(uint32_t p_mask) {
+	avoidance_mask = p_mask;
+	rvo_agent.avoidance_mask_ = avoidance_mask;
+	agent_dirty = true;
+
+	request_sync();
+}
+
+void NavAgent2D::set_avoidance_layers(uint32_t p_layers) {
+	avoidance_layers = p_layers;
+	rvo_agent.avoidance_layers_ = avoidance_layers;
+	agent_dirty = true;
+
+	request_sync();
+}
+
+void NavAgent2D::set_avoidance_priority(real_t p_priority) {
+	ERR_FAIL_COND_MSG(p_priority < 0.0, "Avoidance priority must be between 0.0 and 1.0 inclusive.");
+	ERR_FAIL_COND_MSG(p_priority > 1.0, "Avoidance priority must be between 0.0 and 1.0 inclusive.");
+	avoidance_priority = p_priority;
+	rvo_agent.avoidance_priority_ = avoidance_priority;
+	agent_dirty = true;
+
+	request_sync();
+}
+
+bool NavAgent2D::is_dirty() const {
+	return agent_dirty;
+}
+
+void NavAgent2D::sync() {
+	agent_dirty = false;
+}
+
+const Dictionary NavAgent2D::get_avoidance_data() const {
+	// Returns debug data from RVO simulation internals of this agent.
+	Dictionary _avoidance_data;
+
+	_avoidance_data["max_neighbors"] = int(rvo_agent.maxNeighbors_);
+	_avoidance_data["max_speed"] = float(rvo_agent.maxSpeed_);
+	_avoidance_data["neighbor_distance"] = float(rvo_agent.neighborDist_);
+	_avoidance_data["new_velocity"] = Vector2(rvo_agent.newVelocity_.x(), rvo_agent.newVelocity_.y());
+	_avoidance_data["velocity"] = Vector2(rvo_agent.velocity_.x(), rvo_agent.velocity_.y());
+	_avoidance_data["position"] = Vector2(rvo_agent.position_.x(), rvo_agent.position_.y());
+	_avoidance_data["preferred_velocity"] = Vector2(rvo_agent.prefVelocity_.x(), rvo_agent.prefVelocity_.y());
+	_avoidance_data["radius"] = float(rvo_agent.radius_);
+	_avoidance_data["time_horizon_agents"] = float(rvo_agent.timeHorizon_);
+	_avoidance_data["time_horizon_obstacles"] = float(rvo_agent.timeHorizonObst_);
+	_avoidance_data["avoidance_layers"] = int(rvo_agent.avoidance_layers_);
+	_avoidance_data["avoidance_mask"] = int(rvo_agent.avoidance_mask_);
+	_avoidance_data["avoidance_priority"] = float(rvo_agent.avoidance_priority_);
+	return _avoidance_data;
+}
+
+void NavAgent2D::set_paused(bool p_paused) {
+	if (paused == p_paused) {
+		return;
+	}
+
+	paused = p_paused;
+
+	if (map) {
+		if (paused) {
+			map->remove_agent_as_controlled(this);
+		} else {
+			map->set_agent_as_controlled(this);
+		}
+	}
+}
+
+bool NavAgent2D::get_paused() const {
+	return paused;
+}
+
+void NavAgent2D::request_sync() {
+	if (map && !sync_dirty_request_list_element.in_list()) {
+		map->add_agent_sync_dirty_request(&sync_dirty_request_list_element);
+	}
+}
+
+void NavAgent2D::cancel_sync_request() {
+	if (map && sync_dirty_request_list_element.in_list()) {
+		map->remove_agent_sync_dirty_request(&sync_dirty_request_list_element);
+	}
+}
+
+NavAgent2D::NavAgent2D() :
+		sync_dirty_request_list_element(this) {
+}
+
+NavAgent2D::~NavAgent2D() {
+	cancel_sync_request();
+}

modules/navigation_2d/nav_agent_2d.h

@@ -0,0 +1,148 @@
+/**************************************************************************/
+/*  nav_agent_2d.h                                                        */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "nav_rid_2d.h"
+
+#include "core/object/class_db.h"
+#include "core/templates/self_list.h"
+
+#include <Agent2d.h>
+
+class NavMap2D;
+
+class NavAgent2D : public NavRid2D {
+	Vector2 position;
+	Vector2 target_position;
+	Vector2 velocity;
+	Vector2 velocity_forced;
+	real_t radius = 1.0;
+	real_t max_speed = 1.0;
+	real_t time_horizon_agents = 1.0;
+	real_t time_horizon_obstacles = 0.0;
+	int max_neighbors = 5;
+	real_t neighbor_distance = 5.0;
+	Vector2 safe_velocity;
+	bool clamp_speed = true; // Experimental, clamps velocity to max_speed.
+
+	NavMap2D *map = nullptr;
+
+	RVO2D::Agent2D rvo_agent;
+	bool avoidance_enabled = false;
+
+	uint32_t avoidance_layers = 1;
+	uint32_t avoidance_mask = 1;
+	real_t avoidance_priority = 1.0;
+
+	Callable avoidance_callback;
+
+	bool agent_dirty = true;
+
+	uint32_t last_map_iteration_id = 0;
+	bool paused = false;
+
+	SelfList<NavAgent2D> sync_dirty_request_list_element;
+
+public:
+	NavAgent2D();
+	~NavAgent2D();
+
+	void set_avoidance_enabled(bool p_enabled);
+	bool is_avoidance_enabled() { return avoidance_enabled; }
+
+	void set_map(NavMap2D *p_map);
+	NavMap2D *get_map() { return map; }
+
+	bool is_map_changed();
+
+	RVO2D::Agent2D *get_rvo_agent() { return &rvo_agent; }
+
+	void set_avoidance_callback(Callable p_callback);
+	bool has_avoidance_callback() const;
+
+	void dispatch_avoidance_callback();
+
+	void set_neighbor_distance(real_t p_neighbor_distance);
+	real_t get_neighbor_distance() const { return neighbor_distance; }
+
+	void set_max_neighbors(int p_max_neighbors);
+	int get_max_neighbors() const { return max_neighbors; }
+
+	void set_time_horizon_agents(real_t p_time_horizon);
+	real_t get_time_horizon_agents() const { return time_horizon_agents; }
+
+	void set_time_horizon_obstacles(real_t p_time_horizon);
+	real_t get_time_horizon_obstacles() const { return time_horizon_obstacles; }
+
+	void set_radius(real_t p_radius);
+	real_t get_radius() const { return radius; }
+
+	void set_max_speed(real_t p_max_speed);
+	real_t get_max_speed() const { return max_speed; }
+
+	void set_position(const Vector2 &p_position);
+	Vector2 get_position() const { return position; }
+
+	void set_target_position(const Vector2 &p_target_position);
+	Vector2 get_target_position() const { return target_position; }
+
+	void set_velocity(const Vector2 &p_velocity);
+	Vector2 get_velocity() const { return velocity; }
+
+	void set_velocity_forced(const Vector2 &p_velocity);
+	Vector2 get_velocity_forced() const { return velocity_forced; }
+
+	void set_avoidance_layers(uint32_t p_layers);
+	uint32_t get_avoidance_layers() const { return avoidance_layers; }
+
+	void set_avoidance_mask(uint32_t p_mask);
+	uint32_t get_avoidance_mask() const { return avoidance_mask; }
+
+	void set_avoidance_priority(real_t p_priority);
+	real_t get_avoidance_priority() const { return avoidance_priority; }
+
+	void set_paused(bool p_paused);
+	bool get_paused() const;
+
+	bool is_dirty() const;
+	void sync();
+	void request_sync();
+	void cancel_sync_request();
+
+	// Updates this agent with rvo data after the rvo simulation avoidance step.
+	void update();
+
+	// RVO debug data from the last frame update.
+	const Dictionary get_avoidance_data() const;
+
+private:
+	void _update_rvo_agent_properties();
+};

modules/navigation_2d/nav_base_2d.h

@@ -0,0 +1,67 @@
+/**************************************************************************/
+/*  nav_base_2d.h                                                         */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "nav_rid_2d.h"
+#include "nav_utils_2d.h"
+
+#include "servers/navigation/navigation_utilities.h"
+
+class NavMap2D;
+
+class NavBase2D : public NavRid2D {
+protected:
+	uint32_t navigation_layers = 1;
+	real_t enter_cost = 0.0;
+	real_t travel_cost = 1.0;
+	ObjectID owner_id;
+	NavigationUtilities::PathSegmentType type;
+
+public:
+	NavigationUtilities::PathSegmentType get_type() const { return type; }
+
+	virtual void set_use_edge_connections(bool p_enabled) {}
+	virtual bool get_use_edge_connections() const { return false; }
+
+	virtual void set_navigation_layers(uint32_t p_navigation_layers) {}
+	uint32_t get_navigation_layers() const { return navigation_layers; }
+
+	virtual void set_enter_cost(real_t p_enter_cost) {}
+	real_t get_enter_cost() const { return enter_cost; }
+
+	virtual void set_travel_cost(real_t p_travel_cost) {}
+	real_t get_travel_cost() const { return travel_cost; }
+
+	virtual void set_owner_id(ObjectID p_owner_id) {}
+	ObjectID get_owner_id() const { return owner_id; }
+
+	virtual ~NavBase2D() {}
+};

modules/navigation_2d/nav_link_2d.cpp

@@ -0,0 +1,180 @@
+/**************************************************************************/
+/*  nav_link_2d.cpp                                                       */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "nav_link_2d.h"
+
+#include "nav_map_2d.h"
+
+void NavLink2D::set_map(NavMap2D *p_map) {
+	if (map == p_map) {
+		return;
+	}
+
+	cancel_sync_request();
+
+	if (map) {
+		map->remove_link(this);
+	}
+
+	map = p_map;
+	link_dirty = true;
+
+	if (map) {
+		map->add_link(this);
+		request_sync();
+	}
+}
+
+void NavLink2D::set_enabled(bool p_enabled) {
+	if (enabled == p_enabled) {
+		return;
+	}
+	enabled = p_enabled;
+
+	// TODO: This should not require a full rebuild as the link has not really changed.
+	link_dirty = true;
+
+	request_sync();
+}
+
+void NavLink2D::set_bidirectional(bool p_bidirectional) {
+	if (bidirectional == p_bidirectional) {
+		return;
+	}
+	bidirectional = p_bidirectional;
+	link_dirty = true;
+
+	request_sync();
+}
+
+void NavLink2D::set_start_position(const Vector2 &p_position) {
+	if (start_position == p_position) {
+		return;
+	}
+	start_position = p_position;
+	link_dirty = true;
+
+	request_sync();
+}
+
+void NavLink2D::set_end_position(const Vector2 &p_position) {
+	if (end_position == p_position) {
+		return;
+	}
+	end_position = p_position;
+	link_dirty = true;
+
+	request_sync();
+}
+
+void NavLink2D::set_navigation_layers(uint32_t p_navigation_layers) {
+	if (navigation_layers == p_navigation_layers) {
+		return;
+	}
+	navigation_layers = p_navigation_layers;
+	link_dirty = true;
+
+	request_sync();
+}
+
+void NavLink2D::set_enter_cost(real_t p_enter_cost) {
+	real_t new_enter_cost = MAX(p_enter_cost, 0.0);
+	if (enter_cost == new_enter_cost) {
+		return;
+	}
+	enter_cost = new_enter_cost;
+	link_dirty = true;
+
+	request_sync();
+}
+
+void NavLink2D::set_travel_cost(real_t p_travel_cost) {
+	real_t new_travel_cost = MAX(p_travel_cost, 0.0);
+	if (travel_cost == new_travel_cost) {
+		return;
+	}
+	travel_cost = new_travel_cost;
+	link_dirty = true;
+
+	request_sync();
+}
+
+void NavLink2D::set_owner_id(ObjectID p_owner_id) {
+	if (owner_id == p_owner_id) {
+		return;
+	}
+	owner_id = p_owner_id;
+	link_dirty = true;
+
+	request_sync();
+}
+
+bool NavLink2D::is_dirty() const {
+	return link_dirty;
+}
+
+void NavLink2D::sync() {
+	link_dirty = false;
+}
+
+void NavLink2D::request_sync() {
+	if (map && !sync_dirty_request_list_element.in_list()) {
+		map->add_link_sync_dirty_request(&sync_dirty_request_list_element);
+	}
+}
+
+void NavLink2D::cancel_sync_request() {
+	if (map && sync_dirty_request_list_element.in_list()) {
+		map->remove_link_sync_dirty_request(&sync_dirty_request_list_element);
+	}
+}
+
+NavLink2D::NavLink2D() :
+		sync_dirty_request_list_element(this) {
+	type = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_LINK;
+}
+
+NavLink2D::~NavLink2D() {
+	cancel_sync_request();
+}
+
+void NavLink2D::get_iteration_update(NavLinkIteration2D &r_iteration) {
+	r_iteration.navigation_layers = get_navigation_layers();
+	r_iteration.enter_cost = get_enter_cost();
+	r_iteration.travel_cost = get_travel_cost();
+	r_iteration.owner_object_id = get_owner_id();
+	r_iteration.owner_type = get_type();
+	r_iteration.owner_rid = get_self();
+
+	r_iteration.enabled = get_enabled();
+	r_iteration.start_position = get_start_position();
+	r_iteration.end_position = get_end_position();
+	r_iteration.bidirectional = is_bidirectional();
+}

modules/navigation_2d/nav_link_2d.h

@@ -0,0 +1,99 @@
+/**************************************************************************/
+/*  nav_link_2d.h                                                         */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "2d/nav_base_iteration_2d.h"
+#include "nav_base_2d.h"
+#include "nav_utils_2d.h"
+
+struct NavLinkIteration2D : NavBaseIteration2D {
+	bool bidirectional = true;
+	Vector2 start_position;
+	Vector2 end_position;
+
+	Vector2 get_start_position() const { return start_position; }
+	Vector2 get_end_position() const { return end_position; }
+	bool is_bidirectional() const { return bidirectional; }
+};
+
+#include "core/templates/self_list.h"
+
+class NavLink2D : public NavBase2D {
+	NavMap2D *map = nullptr;
+	bool bidirectional = true;
+	Vector2 start_position;
+	Vector2 end_position;
+	bool enabled = true;
+
+	bool link_dirty = true;
+
+	SelfList<NavLink2D> sync_dirty_request_list_element;
+
+public:
+	NavLink2D();
+	~NavLink2D();
+
+	void set_map(NavMap2D *p_map);
+	NavMap2D *get_map() const {
+		return map;
+	}
+
+	void set_enabled(bool p_enabled);
+	bool get_enabled() const { return enabled; }
+
+	void set_bidirectional(bool p_bidirectional);
+	bool is_bidirectional() const {
+		return bidirectional;
+	}
+
+	void set_start_position(const Vector2 &p_position);
+	Vector2 get_start_position() const {
+		return start_position;
+	}
+
+	void set_end_position(const Vector2 &p_position);
+	Vector2 get_end_position() const {
+		return end_position;
+	}
+
+	// NavBase properties.
+	virtual void set_navigation_layers(uint32_t p_navigation_layers) override;
+	virtual void set_enter_cost(real_t p_enter_cost) override;
+	virtual void set_travel_cost(real_t p_travel_cost) override;
+	virtual void set_owner_id(ObjectID p_owner_id) override;
+
+	bool is_dirty() const;
+	void sync();
+	void request_sync();
+	void cancel_sync_request();
+
+	void get_iteration_update(NavLinkIteration2D &r_iteration);
+};

modules/navigation_2d/nav_map_2d.cpp

@@ -0,0 +1,780 @@
+/**************************************************************************/
+/*  nav_map_2d.cpp                                                        */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "nav_map_2d.h"
+
+#include "2d/nav_map_builder_2d.h"
+#include "2d/nav_mesh_queries_2d.h"
+#include "2d/nav_region_iteration_2d.h"
+#include "nav_agent_2d.h"
+#include "nav_link_2d.h"
+#include "nav_obstacle_2d.h"
+#include "nav_region_2d.h"
+
+#include "core/config/project_settings.h"
+#include "core/object/worker_thread_pool.h"
+
+#include <Obstacle2d.h>
+
+using namespace nav_2d;
+
+#ifdef DEBUG_ENABLED
+#define NAVMAP_ITERATION_ZERO_ERROR_MSG() \
+	ERR_PRINT_ONCE("NavigationServer navigation map query failed because it was made before first map synchronization.\n\
+	NavigationServer 'map_changed' signal can be used to receive update notifications.\n\
+	NavigationServer 'map_get_iteration_id()' can be used to check if a map has finished its newest iteration.");
+#else
+#define NAVMAP_ITERATION_ZERO_ERROR_MSG()
+#endif // DEBUG_ENABLED
+
+#define GET_MAP_ITERATION()                                                   \
+	iteration_slot_rwlock.read_lock();                                        \
+	NavMapIteration2D &map_iteration = iteration_slots[iteration_slot_index]; \
+	NavMapIterationRead2D iteration_read_lock(map_iteration);                 \
+	iteration_slot_rwlock.read_unlock();
+
+#define GET_MAP_ITERATION_CONST()                                                   \
+	iteration_slot_rwlock.read_lock();                                              \
+	const NavMapIteration2D &map_iteration = iteration_slots[iteration_slot_index]; \
+	NavMapIterationRead2D iteration_read_lock(map_iteration);                       \
+	iteration_slot_rwlock.read_unlock();
+
+void NavMap2D::set_cell_size(real_t p_cell_size) {
+	if (cell_size == p_cell_size) {
+		return;
+	}
+	cell_size = MAX(p_cell_size, NavigationDefaults2D::navmesh_cell_size_min);
+	_update_merge_rasterizer_cell_dimensions();
+	map_settings_dirty = true;
+}
+
+void NavMap2D::set_merge_rasterizer_cell_scale(float p_value) {
+	if (merge_rasterizer_cell_scale == p_value) {
+		return;
+	}
+	merge_rasterizer_cell_scale = MAX(p_value, NavigationDefaults2D::navmesh_cell_size_min);
+	_update_merge_rasterizer_cell_dimensions();
+	map_settings_dirty = true;
+}
+
+void NavMap2D::set_use_edge_connections(bool p_enabled) {
+	if (use_edge_connections == p_enabled) {
+		return;
+	}
+	use_edge_connections = p_enabled;
+	iteration_dirty = true;
+}
+
+void NavMap2D::set_edge_connection_margin(real_t p_edge_connection_margin) {
+	if (edge_connection_margin == p_edge_connection_margin) {
+		return;
+	}
+	edge_connection_margin = p_edge_connection_margin;
+	iteration_dirty = true;
+}
+
+void NavMap2D::set_link_connection_radius(real_t p_link_connection_radius) {
+	if (link_connection_radius == p_link_connection_radius) {
+		return;
+	}
+	link_connection_radius = p_link_connection_radius;
+	iteration_dirty = true;
+}
+
+Vector2 NavMap2D::get_merge_rasterizer_cell_size() const {
+	return merge_rasterizer_cell_size;
+}
+
+PointKey NavMap2D::get_point_key(const Vector2 &p_pos) const {
+	const int x = static_cast<int>(Math::floor(p_pos.x / merge_rasterizer_cell_size.x));
+	const int y = static_cast<int>(Math::floor(p_pos.y / merge_rasterizer_cell_size.y));
+
+	PointKey p;
+	p.key = 0;
+	p.x = x;
+	p.y = y;
+	return p;
+}
+
+void NavMap2D::query_path(NavMeshQueries2D::NavMeshPathQueryTask2D &p_query_task) {
+	if (iteration_id == 0) {
+		return;
+	}
+
+	GET_MAP_ITERATION();
+
+	map_iteration.path_query_slots_semaphore.wait();
+
+	map_iteration.path_query_slots_mutex.lock();
+	for (NavMeshQueries2D::PathQuerySlot &p_path_query_slot : map_iteration.path_query_slots) {
+		if (!p_path_query_slot.in_use) {
+			p_path_query_slot.in_use = true;
+			p_query_task.path_query_slot = &p_path_query_slot;
+			break;
+		}
+	}
+	map_iteration.path_query_slots_mutex.unlock();
+
+	if (p_query_task.path_query_slot == nullptr) {
+		map_iteration.path_query_slots_semaphore.post();
+		ERR_FAIL_NULL_MSG(p_query_task.path_query_slot, "No unused NavMap2D path query slot found! This should never happen :(.");
+	}
+
+	NavMeshQueries2D::query_task_map_iteration_get_path(p_query_task, map_iteration);
+
+	map_iteration.path_query_slots_mutex.lock();
+	uint32_t used_slot_index = p_query_task.path_query_slot->slot_index;
+	map_iteration.path_query_slots[used_slot_index].in_use = false;
+	p_query_task.path_query_slot = nullptr;
+	map_iteration.path_query_slots_mutex.unlock();
+
+	map_iteration.path_query_slots_semaphore.post();
+}
+
+Vector2 NavMap2D::get_closest_point(const Vector2 &p_point) const {
+	if (iteration_id == 0) {
+		NAVMAP_ITERATION_ZERO_ERROR_MSG();
+		return Vector2();
+	}
+
+	GET_MAP_ITERATION_CONST();
+
+	return NavMeshQueries2D::map_iteration_get_closest_point(map_iteration, p_point);
+}
+
+RID NavMap2D::get_closest_point_owner(const Vector2 &p_point) const {
+	if (iteration_id == 0) {
+		NAVMAP_ITERATION_ZERO_ERROR_MSG();
+		return RID();
+	}
+
+	GET_MAP_ITERATION_CONST();
+
+	return NavMeshQueries2D::map_iteration_get_closest_point_owner(map_iteration, p_point);
+}
+
+ClosestPointQueryResult NavMap2D::get_closest_point_info(const Vector2 &p_point) const {
+	GET_MAP_ITERATION_CONST();
+
+	return NavMeshQueries2D::map_iteration_get_closest_point_info(map_iteration, p_point);
+}
+
+void NavMap2D::add_region(NavRegion2D *p_region) {
+	regions.push_back(p_region);
+	iteration_dirty = true;
+}
+
+void NavMap2D::remove_region(NavRegion2D *p_region) {
+	int64_t region_index = regions.find(p_region);
+	if (region_index >= 0) {
+		regions.remove_at_unordered(region_index);
+		iteration_dirty = true;
+	}
+}
+
+void NavMap2D::add_link(NavLink2D *p_link) {
+	links.push_back(p_link);
+	iteration_dirty = true;
+}
+
+void NavMap2D::remove_link(NavLink2D *p_link) {
+	int64_t link_index = links.find(p_link);
+	if (link_index >= 0) {
+		links.remove_at_unordered(link_index);
+		iteration_dirty = true;
+	}
+}
+
+bool NavMap2D::has_agent(NavAgent2D *p_agent) const {
+	return agents.has(p_agent);
+}
+
+void NavMap2D::add_agent(NavAgent2D *p_agent) {
+	if (!has_agent(p_agent)) {
+		agents.push_back(p_agent);
+		agents_dirty = true;
+	}
+}
+
+void NavMap2D::remove_agent(NavAgent2D *p_agent) {
+	remove_agent_as_controlled(p_agent);
+	int64_t agent_index = agents.find(p_agent);
+	if (agent_index >= 0) {
+		agents.remove_at_unordered(agent_index);
+		agents_dirty = true;
+	}
+}
+
+bool NavMap2D::has_obstacle(NavObstacle2D *p_obstacle) const {
+	return obstacles.has(p_obstacle);
+}
+
+void NavMap2D::add_obstacle(NavObstacle2D *p_obstacle) {
+	if (p_obstacle->get_paused()) {
+		// No point in adding a paused obstacle, it will add itself when unpaused again.
+		return;
+	}
+
+	if (!has_obstacle(p_obstacle)) {
+		obstacles.push_back(p_obstacle);
+		obstacles_dirty = true;
+	}
+}
+
+void NavMap2D::remove_obstacle(NavObstacle2D *p_obstacle) {
+	int64_t obstacle_index = obstacles.find(p_obstacle);
+	if (obstacle_index >= 0) {
+		obstacles.remove_at_unordered(obstacle_index);
+		obstacles_dirty = true;
+	}
+}
+
+void NavMap2D::set_agent_as_controlled(NavAgent2D *p_agent) {
+	remove_agent_as_controlled(p_agent);
+
+	if (p_agent->get_paused()) {
+		// No point in adding a paused agent, it will add itself when unpaused again.
+		return;
+	}
+
+	int64_t agent_index = active_avoidance_agents.find(p_agent);
+	if (agent_index < 0) {
+		active_avoidance_agents.push_back(p_agent);
+		agents_dirty = true;
+	}
+}
+
+void NavMap2D::remove_agent_as_controlled(NavAgent2D *p_agent) {
+	int64_t agent_index = active_avoidance_agents.find(p_agent);
+	if (agent_index >= 0) {
+		active_avoidance_agents.remove_at_unordered(agent_index);
+		agents_dirty = true;
+	}
+}
+
+Vector2 NavMap2D::get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const {
+	GET_MAP_ITERATION_CONST();
+
+	return NavMeshQueries2D::map_iteration_get_random_point(map_iteration, p_navigation_layers, p_uniformly);
+}
+
+void NavMap2D::_build_iteration() {
+	if (!iteration_dirty || iteration_building || iteration_ready) {
+		return;
+	}
+
+	// Get the next free iteration slot that should be potentially unused.
+	iteration_slot_rwlock.read_lock();
+	NavMapIteration2D &next_map_iteration = iteration_slots[(iteration_slot_index + 1) % 2];
+	// Check if the iteration slot is truly free or still used by an external thread.
+	bool iteration_is_free = next_map_iteration.users.get() == 0;
+	iteration_slot_rwlock.read_unlock();
+
+	if (!iteration_is_free) {
+		// A long running pathfinding thread or something is still reading
+		// from this older iteration and needs to finish first.
+		// Return and wait for the next sync cycle to check again.
+		return;
+	}
+
+	// Iteration slot is free and no longer used by anything, let's build.
+
+	iteration_dirty = false;
+	iteration_building = true;
+	iteration_ready = false;
+
+	// We don't need to hold any lock because at this point nothing else can touch it.
+	// All new queries are already forwarded to the other iteration slot.
+
+	iteration_build.reset();
+
+	iteration_build.merge_rasterizer_cell_size = get_merge_rasterizer_cell_size();
+	iteration_build.use_edge_connections = get_use_edge_connections();
+	iteration_build.edge_connection_margin = get_edge_connection_margin();
+	iteration_build.link_connection_radius = get_link_connection_radius();
+
+	uint32_t enabled_region_count = 0;
+	uint32_t enabled_link_count = 0;
+
+	for (NavRegion2D *region : regions) {
+		if (!region->get_enabled()) {
+			continue;
+		}
+		enabled_region_count++;
+	}
+	for (NavLink2D *link : links) {
+		if (!link->get_enabled()) {
+			continue;
+		}
+		enabled_link_count++;
+	}
+
+	next_map_iteration.region_ptr_to_region_id.clear();
+
+	next_map_iteration.region_iterations.clear();
+	next_map_iteration.link_iterations.clear();
+
+	next_map_iteration.region_iterations.resize(enabled_region_count);
+	next_map_iteration.link_iterations.resize(enabled_link_count);
+
+	uint32_t region_id_count = 0;
+	uint32_t link_id_count = 0;
+
+	for (NavRegion2D *region : regions) {
+		if (!region->get_enabled()) {
+			continue;
+		}
+		NavRegionIteration2D &region_iteration = next_map_iteration.region_iterations[region_id_count];
+		region_iteration.id = region_id_count++;
+		region->get_iteration_update(region_iteration);
+		next_map_iteration.region_ptr_to_region_id[region] = (uint32_t)region_iteration.id;
+	}
+	for (NavLink2D *link : links) {
+		if (!link->get_enabled()) {
+			continue;
+		}
+		NavLinkIteration2D &link_iteration = next_map_iteration.link_iterations[link_id_count];
+		link_iteration.id = link_id_count++;
+		link->get_iteration_update(link_iteration);
+	}
+
+	iteration_build.map_iteration = &next_map_iteration;
+
+	if (use_async_iterations) {
+		iteration_build_thread_task_id = WorkerThreadPool::get_singleton()->add_native_task(&NavMap2D::_build_iteration_threaded, &iteration_build, true, SNAME("NavMapBuilder2D"));
+	} else {
+		NavMapBuilder2D::build_navmap_iteration(iteration_build);
+
+		iteration_building = false;
+		iteration_ready = true;
+	}
+}
+
+void NavMap2D::_build_iteration_threaded(void *p_arg) {
+	NavMapIterationBuild2D *_iteration_build = static_cast<NavMapIterationBuild2D *>(p_arg);
+
+	NavMapBuilder2D::build_navmap_iteration(*_iteration_build);
+}
+
+void NavMap2D::_sync_iteration() {
+	if (iteration_building || !iteration_ready) {
+		return;
+	}
+
+	performance_data.pm_polygon_count = iteration_build.performance_data.pm_polygon_count;
+	performance_data.pm_edge_count = iteration_build.performance_data.pm_edge_count;
+	performance_data.pm_edge_merge_count = iteration_build.performance_data.pm_edge_merge_count;
+	performance_data.pm_edge_connection_count = iteration_build.performance_data.pm_edge_connection_count;
+	performance_data.pm_edge_free_count = iteration_build.performance_data.pm_edge_free_count;
+
+	iteration_id = iteration_id % UINT32_MAX + 1;
+
+	// Finally ping-pong switch the iteration slot.
+	iteration_slot_rwlock.write_lock();
+	uint32_t next_iteration_slot_index = (iteration_slot_index + 1) % 2;
+	iteration_slot_index = next_iteration_slot_index;
+	iteration_slot_rwlock.write_unlock();
+
+	iteration_ready = false;
+}
+
+void NavMap2D::sync() {
+	// Performance Monitor.
+	performance_data.pm_region_count = regions.size();
+	performance_data.pm_agent_count = agents.size();
+	performance_data.pm_link_count = links.size();
+	performance_data.pm_obstacle_count = obstacles.size();
+
+	_sync_dirty_map_update_requests();
+
+	if (iteration_dirty && !iteration_building && !iteration_ready) {
+		_build_iteration();
+	}
+	if (use_async_iterations && iteration_build_thread_task_id != WorkerThreadPool::INVALID_TASK_ID) {
+		if (WorkerThreadPool::get_singleton()->is_task_completed(iteration_build_thread_task_id)) {
+			WorkerThreadPool::get_singleton()->wait_for_task_completion(iteration_build_thread_task_id);
+
+			iteration_build_thread_task_id = WorkerThreadPool::INVALID_TASK_ID;
+			iteration_building = false;
+			iteration_ready = true;
+		}
+	}
+	if (iteration_ready) {
+		_sync_iteration();
+	}
+
+	map_settings_dirty = false;
+
+	_sync_avoidance();
+}
+
+void NavMap2D::_sync_avoidance() {
+	_sync_dirty_avoidance_update_requests();
+
+	if (obstacles_dirty || agents_dirty) {
+		_update_rvo_simulation();
+	}
+
+	obstacles_dirty = false;
+	agents_dirty = false;
+}
+
+void NavMap2D::_update_rvo_obstacles_tree() {
+	int obstacle_vertex_count = 0;
+	for (NavObstacle2D *obstacle : obstacles) {
+		obstacle_vertex_count += obstacle->get_vertices().size();
+	}
+
+	// Cleaning old obstacles.
+	for (size_t i = 0; i < rvo_simulation.obstacles_.size(); ++i) {
+		delete rvo_simulation.obstacles_[i];
+	}
+	rvo_simulation.obstacles_.clear();
+
+	// Cannot use LocalVector here as RVO library expects std::vector to build KdTree
+	std::vector<RVO2D::Obstacle2D *> &raw_obstacles = rvo_simulation.obstacles_;
+	raw_obstacles.reserve(obstacle_vertex_count);
+
+	// The following block is modified copy from RVO2D::AddObstacle()
+	// Obstacles are linked and depend on all other obstacles.
+	for (NavObstacle2D *obstacle : obstacles) {
+		const Vector2 &_obstacle_position = obstacle->get_position();
+		const Vector<Vector2> &_obstacle_vertices = obstacle->get_vertices();
+
+		if (_obstacle_vertices.size() < 2) {
+			continue;
+		}
+
+		std::vector<RVO2D::Vector2> rvo_vertices;
+		rvo_vertices.reserve(_obstacle_vertices.size());
+
+		uint32_t _obstacle_avoidance_layers = obstacle->get_avoidance_layers();
+
+		for (const Vector2 &_obstacle_vertex : _obstacle_vertices) {
+			rvo_vertices.push_back(RVO2D::Vector2(_obstacle_vertex.x + _obstacle_position.x, _obstacle_vertex.y + _obstacle_position.y));
+		}
+
+		const size_t obstacleNo = raw_obstacles.size();
+
+		for (size_t i = 0; i < rvo_vertices.size(); i++) {
+			RVO2D::Obstacle2D *rvo_obstacle = new RVO2D::Obstacle2D();
+			rvo_obstacle->point_ = rvo_vertices[i];
+
+			rvo_obstacle->avoidance_layers_ = _obstacle_avoidance_layers;
+
+			if (i != 0) {
+				rvo_obstacle->prevObstacle_ = raw_obstacles.back();
+				rvo_obstacle->prevObstacle_->nextObstacle_ = rvo_obstacle;
+			}
+
+			if (i == rvo_vertices.size() - 1) {
+				rvo_obstacle->nextObstacle_ = raw_obstacles[obstacleNo];
+				rvo_obstacle->nextObstacle_->prevObstacle_ = rvo_obstacle;
+			}
+
+			rvo_obstacle->unitDir_ = normalize(rvo_vertices[(i == rvo_vertices.size() - 1 ? 0 : i + 1)] - rvo_vertices[i]);
+
+			if (rvo_vertices.size() == 2) {
+				rvo_obstacle->isConvex_ = true;
+			} else {
+				rvo_obstacle->isConvex_ = (leftOf(rvo_vertices[(i == 0 ? rvo_vertices.size() - 1 : i - 1)], rvo_vertices[i], rvo_vertices[(i == rvo_vertices.size() - 1 ? 0 : i + 1)]) >= 0.0f);
+			}
+
+			rvo_obstacle->id_ = raw_obstacles.size();
+
+			raw_obstacles.push_back(rvo_obstacle);
+		}
+	}
+
+	rvo_simulation.kdTree_->buildObstacleTree(raw_obstacles);
+}
+
+void NavMap2D::_update_rvo_agents_tree() {
+	// Cannot use LocalVector here as RVO library expects std::vector to build KdTree.
+	std::vector<RVO2D::Agent2D *> raw_agents;
+	raw_agents.reserve(active_avoidance_agents.size());
+	for (NavAgent2D *agent : active_avoidance_agents) {
+		raw_agents.push_back(agent->get_rvo_agent());
+	}
+	rvo_simulation.kdTree_->buildAgentTree(raw_agents);
+}
+
+void NavMap2D::_update_rvo_simulation() {
+	if (obstacles_dirty) {
+		_update_rvo_obstacles_tree();
+	}
+	if (agents_dirty) {
+		_update_rvo_agents_tree();
+	}
+}
+
+void NavMap2D::compute_single_avoidance_step(uint32_t p_index, NavAgent2D **p_agent) {
+	(*(p_agent + p_index))->get_rvo_agent()->computeNeighbors(&rvo_simulation);
+	(*(p_agent + p_index))->get_rvo_agent()->computeNewVelocity(&rvo_simulation);
+	(*(p_agent + p_index))->get_rvo_agent()->update(&rvo_simulation);
+	(*(p_agent + p_index))->update();
+}
+
+void NavMap2D::step(real_t p_deltatime) {
+	deltatime = p_deltatime;
+
+	rvo_simulation.setTimeStep(float(deltatime));
+
+	if (active_avoidance_agents.size() > 0) {
+		if (use_threads && avoidance_use_multiple_threads) {
+			WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &NavMap2D::compute_single_avoidance_step, active_avoidance_agents.ptr(), active_avoidance_agents.size(), -1, true, SNAME("RVOAvoidanceAgents2D"));
+			WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task);
+		} else {
+			for (NavAgent2D *agent : active_avoidance_agents) {
+				agent->get_rvo_agent()->computeNeighbors(&rvo_simulation);
+				agent->get_rvo_agent()->computeNewVelocity(&rvo_simulation);
+				agent->get_rvo_agent()->update(&rvo_simulation);
+				agent->update();
+			}
+		}
+	}
+}
+
+void NavMap2D::dispatch_callbacks() {
+	for (NavAgent2D *agent : active_avoidance_agents) {
+		agent->dispatch_avoidance_callback();
+	}
+}
+
+void NavMap2D::_update_merge_rasterizer_cell_dimensions() {
+	merge_rasterizer_cell_size.x = cell_size * merge_rasterizer_cell_scale;
+	merge_rasterizer_cell_size.y = cell_size * merge_rasterizer_cell_scale;
+}
+
+int NavMap2D::get_region_connections_count(NavRegion2D *p_region) const {
+	ERR_FAIL_NULL_V(p_region, 0);
+
+	GET_MAP_ITERATION_CONST();
+
+	HashMap<NavRegion2D *, uint32_t>::ConstIterator found_id = map_iteration.region_ptr_to_region_id.find(p_region);
+	if (found_id) {
+		HashMap<uint32_t, LocalVector<Edge::Connection>>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value);
+		if (found_connections) {
+			return found_connections->value.size();
+		}
+	}
+
+	return 0;
+}
+
+Vector2 NavMap2D::get_region_connection_pathway_start(NavRegion2D *p_region, int p_connection_id) const {
+	ERR_FAIL_NULL_V(p_region, Vector2());
+
+	GET_MAP_ITERATION_CONST();
+
+	HashMap<NavRegion2D *, uint32_t>::ConstIterator found_id = map_iteration.region_ptr_to_region_id.find(p_region);
+	if (found_id) {
+		HashMap<uint32_t, LocalVector<Edge::Connection>>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value);
+		if (found_connections) {
+			ERR_FAIL_INDEX_V(p_connection_id, int(found_connections->value.size()), Vector2());
+			return found_connections->value[p_connection_id].pathway_start;
+		}
+	}
+
+	return Vector2();
+}
+
+Vector2 NavMap2D::get_region_connection_pathway_end(NavRegion2D *p_region, int p_connection_id) const {
+	ERR_FAIL_NULL_V(p_region, Vector2());
+
+	GET_MAP_ITERATION_CONST();
+
+	HashMap<NavRegion2D *, uint32_t>::ConstIterator found_id = map_iteration.region_ptr_to_region_id.find(p_region);
+	if (found_id) {
+		HashMap<uint32_t, LocalVector<Edge::Connection>>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value);
+		if (found_connections) {
+			ERR_FAIL_INDEX_V(p_connection_id, int(found_connections->value.size()), Vector2());
+			return found_connections->value[p_connection_id].pathway_end;
+		}
+	}
+
+	return Vector2();
+}
+
+void NavMap2D::add_region_sync_dirty_request(SelfList<NavRegion2D> *p_sync_request) {
+	if (p_sync_request->in_list()) {
+		return;
+	}
+	sync_dirty_requests.regions.add(p_sync_request);
+}
+
+void NavMap2D::add_link_sync_dirty_request(SelfList<NavLink2D> *p_sync_request) {
+	if (p_sync_request->in_list()) {
+		return;
+	}
+	sync_dirty_requests.links.add(p_sync_request);
+}
+
+void NavMap2D::add_agent_sync_dirty_request(SelfList<NavAgent2D> *p_sync_request) {
+	if (p_sync_request->in_list()) {
+		return;
+	}
+	sync_dirty_requests.agents.add(p_sync_request);
+}
+
+void NavMap2D::add_obstacle_sync_dirty_request(SelfList<NavObstacle2D> *p_sync_request) {
+	if (p_sync_request->in_list()) {
+		return;
+	}
+	sync_dirty_requests.obstacles.add(p_sync_request);
+}
+
+void NavMap2D::remove_region_sync_dirty_request(SelfList<NavRegion2D> *p_sync_request) {
+	if (!p_sync_request->in_list()) {
+		return;
+	}
+	sync_dirty_requests.regions.remove(p_sync_request);
+}
+
+void NavMap2D::remove_link_sync_dirty_request(SelfList<NavLink2D> *p_sync_request) {
+	if (!p_sync_request->in_list()) {
+		return;
+	}
+	sync_dirty_requests.links.remove(p_sync_request);
+}
+
+void NavMap2D::remove_agent_sync_dirty_request(SelfList<NavAgent2D> *p_sync_request) {
+	if (!p_sync_request->in_list()) {
+		return;
+	}
+	sync_dirty_requests.agents.remove(p_sync_request);
+}
+
+void NavMap2D::remove_obstacle_sync_dirty_request(SelfList<NavObstacle2D> *p_sync_request) {
+	if (!p_sync_request->in_list()) {
+		return;
+	}
+	sync_dirty_requests.obstacles.remove(p_sync_request);
+}
+
+void NavMap2D::_sync_dirty_map_update_requests() {
+	// If entire map settings changed make all regions dirty.
+	if (map_settings_dirty) {
+		for (NavRegion2D *region : regions) {
+			region->scratch_polygons();
+		}
+		iteration_dirty = true;
+	}
+
+	if (!iteration_dirty) {
+		iteration_dirty = sync_dirty_requests.regions.first() || sync_dirty_requests.links.first();
+	}
+
+	// Sync NavRegions.
+	for (SelfList<NavRegion2D> *element = sync_dirty_requests.regions.first(); element; element = element->next()) {
+		element->self()->sync();
+	}
+	sync_dirty_requests.regions.clear();
+
+	// Sync NavLinks.
+	for (SelfList<NavLink2D> *element = sync_dirty_requests.links.first(); element; element = element->next()) {
+		element->self()->sync();
+	}
+	sync_dirty_requests.links.clear();
+}
+
+void NavMap2D::_sync_dirty_avoidance_update_requests() {
+	// Sync NavAgents.
+	if (!agents_dirty) {
+		agents_dirty = sync_dirty_requests.agents.first();
+	}
+	for (SelfList<NavAgent2D> *element = sync_dirty_requests.agents.first(); element; element = element->next()) {
+		element->self()->sync();
+	}
+	sync_dirty_requests.agents.clear();
+
+	// Sync NavObstacles.
+	if (!obstacles_dirty) {
+		obstacles_dirty = sync_dirty_requests.obstacles.first();
+	}
+	for (SelfList<NavObstacle2D> *element = sync_dirty_requests.obstacles.first(); element; element = element->next()) {
+		element->self()->sync();
+	}
+	sync_dirty_requests.obstacles.clear();
+}
+
+void NavMap2D::set_use_async_iterations(bool p_enabled) {
+	if (use_async_iterations == p_enabled) {
+		return;
+	}
+#ifdef THREADS_ENABLED
+	use_async_iterations = p_enabled;
+#endif
+}
+
+bool NavMap2D::get_use_async_iterations() const {
+	return use_async_iterations;
+}
+
+NavMap2D::NavMap2D() {
+	avoidance_use_multiple_threads = GLOBAL_GET("navigation/avoidance/thread_model/avoidance_use_multiple_threads");
+	avoidance_use_high_priority_threads = GLOBAL_GET("navigation/avoidance/thread_model/avoidance_use_high_priority_threads");
+
+	path_query_slots_max = GLOBAL_GET("navigation/pathfinding/max_threads");
+
+	int processor_count = OS::get_singleton()->get_processor_count();
+	if (path_query_slots_max < 0) {
+		path_query_slots_max = processor_count;
+	}
+	if (processor_count < path_query_slots_max) {
+		path_query_slots_max = processor_count;
+	}
+	if (path_query_slots_max < 1) {
+		path_query_slots_max = 1;
+	}
+
+	iteration_slots.resize(2);
+
+	for (NavMapIteration2D &iteration_slot : iteration_slots) {
+		iteration_slot.path_query_slots.resize(path_query_slots_max);
+		for (uint32_t i = 0; i < iteration_slot.path_query_slots.size(); i++) {
+			iteration_slot.path_query_slots[i].slot_index = i;
+		}
+		iteration_slot.path_query_slots_semaphore.post(path_query_slots_max);
+	}
+
+#ifdef THREADS_ENABLED
+	use_async_iterations = GLOBAL_GET("navigation/world/map_use_async_iterations");
+#else
+	use_async_iterations = false;
+#endif
+}
+
+NavMap2D::~NavMap2D() {
+	if (iteration_build_thread_task_id != WorkerThreadPool::INVALID_TASK_ID) {
+		WorkerThreadPool::get_singleton()->wait_for_task_completion(iteration_build_thread_task_id);
+		iteration_build_thread_task_id = WorkerThreadPool::INVALID_TASK_ID;
+	}
+}

modules/navigation_2d/nav_map_2d.h

@@ -0,0 +1,251 @@
+/**************************************************************************/
+/*  nav_map_2d.h                                                          */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "2d/nav_map_iteration_2d.h"
+#include "2d/nav_mesh_queries_2d.h"
+#include "nav_rid_2d.h"
+#include "nav_utils_2d.h"
+
+#include "core/math/math_defs.h"
+#include "core/object/worker_thread_pool.h"
+#include "servers/navigation/navigation_globals.h"
+
+#include <KdTree2d.h>
+#include <RVOSimulator2d.h>
+
+class NavLink2D;
+class NavRegion2D;
+class NavAgent2D;
+class NavObstacle2D;
+
+class NavMap2D : public NavRid2D {
+	/// To find the polygons edges the vertices are displaced in a grid where
+	/// each cell has the following cell_size.
+	real_t cell_size = NavigationDefaults2D::navmesh_cell_size;
+
+	// For the inter-region merging to work, internal rasterization is performed.
+	Vector2 merge_rasterizer_cell_size = Vector2(cell_size, cell_size);
+
+	// This value is used to control sensitivity of internal rasterizer.
+	float merge_rasterizer_cell_scale = 1.0;
+
+	bool use_edge_connections = true;
+	/// This value is used to detect the near edges to connect.
+	real_t edge_connection_margin = NavigationDefaults2D::edge_connection_margin;
+
+	/// This value is used to limit how far links search to find polygons to connect to.
+	real_t link_connection_radius = NavigationDefaults2D::link_connection_radius;
+
+	bool map_settings_dirty = true;
+
+	/// Map regions.
+	LocalVector<NavRegion2D *> regions;
+
+	/// Map links.
+	LocalVector<NavLink2D *> links;
+
+	/// RVO avoidance world.
+	RVO2D::RVOSimulator2D rvo_simulation;
+
+	/// Avoidance controlled agents.
+	LocalVector<NavAgent2D *> active_avoidance_agents;
+
+	/// dirty flag when one of the agent's arrays are modified.
+	bool agents_dirty = true;
+
+	/// All the Agents (even the controlled one).
+	LocalVector<NavAgent2D *> agents;
+
+	/// All the avoidance obstacles (both static and dynamic).
+	LocalVector<NavObstacle2D *> obstacles;
+
+	/// Are rvo obstacles modified?
+	bool obstacles_dirty = true;
+
+	/// Physics delta time.
+	real_t deltatime = 0.0;
+
+	/// Change the id each time the map is updated.
+	uint32_t iteration_id = 0;
+
+	bool use_threads = true;
+	bool avoidance_use_multiple_threads = true;
+	bool avoidance_use_high_priority_threads = true;
+
+	// Performance Monitor
+	nav_2d::PerformanceData performance_data;
+
+	struct {
+		SelfList<NavRegion2D>::List regions;
+		SelfList<NavLink2D>::List links;
+		SelfList<NavAgent2D>::List agents;
+		SelfList<NavObstacle2D>::List obstacles;
+	} sync_dirty_requests;
+
+	int path_query_slots_max = 4;
+
+	bool use_async_iterations = true;
+
+	uint32_t iteration_slot_index = 0;
+	LocalVector<NavMapIteration2D> iteration_slots;
+	mutable RWLock iteration_slot_rwlock;
+
+	NavMapIterationBuild2D iteration_build;
+	bool iteration_build_use_threads = false;
+	WorkerThreadPool::TaskID iteration_build_thread_task_id = WorkerThreadPool::INVALID_TASK_ID;
+	static void _build_iteration_threaded(void *p_arg);
+
+	bool iteration_dirty = true;
+	bool iteration_building = false;
+	bool iteration_ready = false;
+
+	void _build_iteration();
+	void _sync_iteration();
+
+public:
+	NavMap2D();
+	~NavMap2D();
+
+	uint32_t get_iteration_id() const { return iteration_id; }
+
+	void set_cell_size(real_t p_cell_size);
+	real_t get_cell_size() const {
+		return cell_size;
+	}
+
+	void set_merge_rasterizer_cell_scale(float p_value);
+	float get_merge_rasterizer_cell_scale() const {
+		return merge_rasterizer_cell_scale;
+	}
+
+	void set_use_edge_connections(bool p_enabled);
+	bool get_use_edge_connections() const {
+		return use_edge_connections;
+	}
+
+	void set_edge_connection_margin(real_t p_edge_connection_margin);
+	real_t get_edge_connection_margin() const {
+		return edge_connection_margin;
+	}
+
+	void set_link_connection_radius(real_t p_link_connection_radius);
+	real_t get_link_connection_radius() const {
+		return link_connection_radius;
+	}
+
+	nav_2d::PointKey get_point_key(const Vector2 &p_pos) const;
+	Vector2 get_merge_rasterizer_cell_size() const;
+
+	void query_path(NavMeshQueries2D::NavMeshPathQueryTask2D &p_query_task);
+
+	Vector2 get_closest_point(const Vector2 &p_point) const;
+	nav_2d::ClosestPointQueryResult get_closest_point_info(const Vector2 &p_point) const;
+	RID get_closest_point_owner(const Vector2 &p_point) const;
+
+	void add_region(NavRegion2D *p_region);
+	void remove_region(NavRegion2D *p_region);
+	const LocalVector<NavRegion2D *> &get_regions() const {
+		return regions;
+	}
+
+	void add_link(NavLink2D *p_link);
+	void remove_link(NavLink2D *p_link);
+	const LocalVector<NavLink2D *> &get_links() const {
+		return links;
+	}
+
+	bool has_agent(NavAgent2D *p_agent) const;
+	void add_agent(NavAgent2D *p_agent);
+	void remove_agent(NavAgent2D *p_agent);
+	const LocalVector<NavAgent2D *> &get_agents() const {
+		return agents;
+	}
+
+	void set_agent_as_controlled(NavAgent2D *p_agent);
+	void remove_agent_as_controlled(NavAgent2D *p_agent);
+
+	bool has_obstacle(NavObstacle2D *p_obstacle) const;
+	void add_obstacle(NavObstacle2D *p_obstacle);
+	void remove_obstacle(NavObstacle2D *p_obstacle);
+	const LocalVector<NavObstacle2D *> &get_obstacles() const {
+		return obstacles;
+	}
+
+	Vector2 get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const;
+
+	void sync();
+	void step(real_t p_deltatime);
+	void dispatch_callbacks();
+
+	// Performance Monitor
+	int get_pm_region_count() const { return performance_data.pm_region_count; }
+	int get_pm_agent_count() const { return performance_data.pm_agent_count; }
+	int get_pm_link_count() const { return performance_data.pm_link_count; }
+	int get_pm_polygon_count() const { return performance_data.pm_polygon_count; }
+	int get_pm_edge_count() const { return performance_data.pm_edge_count; }
+	int get_pm_edge_merge_count() const { return performance_data.pm_edge_merge_count; }
+	int get_pm_edge_connection_count() const { return performance_data.pm_edge_connection_count; }
+	int get_pm_edge_free_count() const { return performance_data.pm_edge_free_count; }
+	int get_pm_obstacle_count() const { return performance_data.pm_obstacle_count; }
+
+	int get_region_connections_count(NavRegion2D *p_region) const;
+	Vector2 get_region_connection_pathway_start(NavRegion2D *p_region, int p_connection_id) const;
+	Vector2 get_region_connection_pathway_end(NavRegion2D *p_region, int p_connection_id) const;
+
+	void add_region_sync_dirty_request(SelfList<NavRegion2D> *p_sync_request);
+	void add_link_sync_dirty_request(SelfList<NavLink2D> *p_sync_request);
+	void add_agent_sync_dirty_request(SelfList<NavAgent2D> *p_sync_request);
+	void add_obstacle_sync_dirty_request(SelfList<NavObstacle2D> *p_sync_request);
+
+	void remove_region_sync_dirty_request(SelfList<NavRegion2D> *p_sync_request);
+	void remove_link_sync_dirty_request(SelfList<NavLink2D> *p_sync_request);
+	void remove_agent_sync_dirty_request(SelfList<NavAgent2D> *p_sync_request);
+	void remove_obstacle_sync_dirty_request(SelfList<NavObstacle2D> *p_sync_request);
+
+	void set_use_async_iterations(bool p_enabled);
+	bool get_use_async_iterations() const;
+
+private:
+	void _sync_dirty_map_update_requests();
+	void _sync_dirty_avoidance_update_requests();
+
+	void compute_single_step(uint32_t p_index, NavAgent2D **p_agent);
+
+	void compute_single_avoidance_step(uint32_t p_index, NavAgent2D **p_agent);
+
+	void _sync_avoidance();
+	void _update_rvo_simulation();
+	void _update_rvo_obstacles_tree();
+	void _update_rvo_agents_tree();
+
+	void _update_merge_rasterizer_cell_dimensions();
+};

modules/navigation_2d/nav_obstacle_2d.cpp

@@ -0,0 +1,222 @@
+/**************************************************************************/
+/*  nav_obstacle_2d.cpp                                                   */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "nav_obstacle_2d.h"
+
+#include "nav_agent_2d.h"
+#include "nav_map_2d.h"
+
+void NavObstacle2D::set_agent(NavAgent2D *p_agent) {
+	if (agent == p_agent) {
+		return;
+	}
+
+	agent = p_agent;
+
+	internal_update_agent();
+
+	request_sync();
+}
+
+void NavObstacle2D::set_avoidance_enabled(bool p_enabled) {
+	if (avoidance_enabled == p_enabled) {
+		return;
+	}
+
+	avoidance_enabled = p_enabled;
+	obstacle_dirty = true;
+
+	internal_update_agent();
+
+	request_sync();
+}
+
+void NavObstacle2D::set_map(NavMap2D *p_map) {
+	if (map == p_map) {
+		return;
+	}
+
+	cancel_sync_request();
+
+	if (map) {
+		map->remove_obstacle(this);
+		if (agent) {
+			agent->set_map(nullptr);
+		}
+	}
+
+	map = p_map;
+	obstacle_dirty = true;
+
+	if (map) {
+		map->add_obstacle(this);
+		internal_update_agent();
+
+		request_sync();
+	}
+}
+
+void NavObstacle2D::set_position(const Vector2 &p_position) {
+	if (position == p_position) {
+		return;
+	}
+
+	position = p_position;
+	obstacle_dirty = true;
+
+	if (agent) {
+		agent->set_position(position);
+	}
+
+	request_sync();
+}
+
+void NavObstacle2D::set_radius(real_t p_radius) {
+	if (radius == p_radius) {
+		return;
+	}
+
+	radius = p_radius;
+
+	if (agent) {
+		agent->set_radius(radius);
+	}
+}
+
+void NavObstacle2D::set_velocity(const Vector2 &p_velocity) {
+	velocity = p_velocity;
+
+	if (agent) {
+		agent->set_velocity(velocity);
+	}
+}
+
+void NavObstacle2D::set_vertices(const Vector<Vector2> &p_vertices) {
+	if (vertices == p_vertices) {
+		return;
+	}
+
+	vertices = p_vertices;
+	obstacle_dirty = true;
+
+	request_sync();
+}
+
+bool NavObstacle2D::is_map_changed() {
+	if (map) {
+		bool is_changed = map->get_iteration_id() != last_map_iteration_id;
+		last_map_iteration_id = map->get_iteration_id();
+		return is_changed;
+	} else {
+		return false;
+	}
+}
+
+void NavObstacle2D::set_avoidance_layers(uint32_t p_layers) {
+	if (avoidance_layers == p_layers) {
+		return;
+	}
+
+	avoidance_layers = p_layers;
+	obstacle_dirty = true;
+
+	if (agent) {
+		agent->set_avoidance_layers(avoidance_layers);
+	}
+
+	request_sync();
+}
+
+bool NavObstacle2D::is_dirty() const {
+	return obstacle_dirty;
+}
+
+void NavObstacle2D::sync() {
+	obstacle_dirty = false;
+}
+
+void NavObstacle2D::internal_update_agent() {
+	if (agent) {
+		agent->set_neighbor_distance(0.0);
+		agent->set_max_neighbors(0.0);
+		agent->set_time_horizon_agents(0.0);
+		agent->set_time_horizon_obstacles(0.0);
+		agent->set_avoidance_mask(0.0);
+		agent->set_neighbor_distance(0.0);
+		agent->set_avoidance_priority(1.0);
+		agent->set_map(map);
+		agent->set_paused(paused);
+		agent->set_radius(radius);
+		agent->set_position(position);
+		agent->set_avoidance_layers(avoidance_layers);
+		agent->set_avoidance_enabled(avoidance_enabled);
+	}
+}
+
+void NavObstacle2D::set_paused(bool p_paused) {
+	if (paused == p_paused) {
+		return;
+	}
+
+	paused = p_paused;
+
+	if (map) {
+		if (paused) {
+			map->remove_obstacle(this);
+		} else {
+			map->add_obstacle(this);
+		}
+	}
+	internal_update_agent();
+}
+
+bool NavObstacle2D::get_paused() const {
+	return paused;
+}
+
+void NavObstacle2D::request_sync() {
+	if (map && !sync_dirty_request_list_element.in_list()) {
+		map->add_obstacle_sync_dirty_request(&sync_dirty_request_list_element);
+	}
+}
+
+void NavObstacle2D::cancel_sync_request() {
+	if (map && sync_dirty_request_list_element.in_list()) {
+		map->remove_obstacle_sync_dirty_request(&sync_dirty_request_list_element);
+	}
+}
+
+NavObstacle2D::NavObstacle2D() :
+		sync_dirty_request_list_element(this) {
+}
+
+NavObstacle2D::~NavObstacle2D() {
+	cancel_sync_request();
+}

modules/navigation_2d/nav_obstacle_2d.h

@@ -0,0 +1,100 @@
+/**************************************************************************/
+/*  nav_obstacle_2d.h                                                     */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "nav_rid_2d.h"
+
+#include "core/object/class_db.h"
+#include "core/templates/self_list.h"
+
+class NavAgent2D;
+class NavMap2D;
+
+class NavObstacle2D : public NavRid2D {
+	NavAgent2D *agent = nullptr;
+	NavMap2D *map = nullptr;
+	Vector2 velocity;
+	Vector2 position;
+	Vector<Vector2> vertices;
+
+	real_t radius = 0.0;
+
+	bool avoidance_enabled = false;
+	uint32_t avoidance_layers = 1;
+
+	bool obstacle_dirty = true;
+
+	uint32_t last_map_iteration_id = 0;
+	bool paused = false;
+
+	SelfList<NavObstacle2D> sync_dirty_request_list_element;
+
+public:
+	NavObstacle2D();
+	~NavObstacle2D();
+
+	void set_avoidance_enabled(bool p_enabled);
+	bool is_avoidance_enabled() { return avoidance_enabled; }
+
+	void set_map(NavMap2D *p_map);
+	NavMap2D *get_map() { return map; }
+
+	void set_agent(NavAgent2D *p_agent);
+	NavAgent2D *get_agent() { return agent; }
+
+	void set_position(const Vector2 &p_position);
+	Vector2 get_position() const { return position; }
+
+	void set_radius(real_t p_radius);
+	real_t get_radius() const { return radius; }
+
+	void set_velocity(const Vector2 &p_velocity);
+	Vector2 get_velocity() const { return velocity; }
+
+	void set_vertices(const Vector<Vector2> &p_vertices);
+	const Vector<Vector2> &get_vertices() const { return vertices; }
+
+	bool is_map_changed();
+
+	void set_avoidance_layers(uint32_t p_layers);
+	uint32_t get_avoidance_layers() const { return avoidance_layers; }
+
+	void set_paused(bool p_paused);
+	bool get_paused() const;
+
+	bool is_dirty() const;
+	void sync();
+	void request_sync();
+	void cancel_sync_request();
+
+private:
+	void internal_update_agent();
+};

modules/navigation_2d/nav_region_2d.cpp

@@ -0,0 +1,319 @@
+/**************************************************************************/
+/*  nav_region_2d.cpp                                                     */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "nav_region_2d.h"
+
+#include "nav_map_2d.h"
+#include "triangle2.h"
+
+#include "2d/nav_map_builder_2d.h"
+#include "2d/nav_mesh_queries_2d.h"
+#include "2d/nav_region_iteration_2d.h"
+
+using namespace nav_2d;
+
+void NavRegion2D::set_map(NavMap2D *p_map) {
+	if (map == p_map) {
+		return;
+	}
+
+	cancel_sync_request();
+
+	if (map) {
+		map->remove_region(this);
+	}
+
+	map = p_map;
+	polygons_dirty = true;
+
+	if (map) {
+		map->add_region(this);
+		request_sync();
+	}
+}
+
+void NavRegion2D::set_enabled(bool p_enabled) {
+	if (enabled == p_enabled) {
+		return;
+	}
+	enabled = p_enabled;
+
+	// TODO: This should not require a full rebuild as the region has not really changed.
+	polygons_dirty = true;
+
+	request_sync();
+}
+
+void NavRegion2D::set_use_edge_connections(bool p_enabled) {
+	if (use_edge_connections != p_enabled) {
+		use_edge_connections = p_enabled;
+		polygons_dirty = true;
+	}
+
+	request_sync();
+}
+
+void NavRegion2D::set_transform(const Transform2D &p_transform) {
+	if (transform == p_transform) {
+		return;
+	}
+	transform = p_transform;
+	polygons_dirty = true;
+
+	request_sync();
+}
+
+void NavRegion2D::set_navigation_polygon(Ref<NavigationPolygon> p_navigation_polygon) {
+#ifdef DEBUG_ENABLED
+	if (map && p_navigation_polygon.is_valid() && !Math::is_equal_approx(double(map->get_cell_size()), double(p_navigation_polygon->get_cell_size()))) {
+		ERR_PRINT_ONCE(vformat("Attempted to update a navigation region with a navigation mesh that uses a `cell_size` of %s while assigned to a navigation map set to a `cell_size` of %s. The cell size for navigation maps can be changed by using the NavigationServer map_set_cell_size() function. The cell size for default navigation maps can also be changed in the ProjectSettings.", double(p_navigation_polygon->get_cell_size()), double(map->get_cell_size())));
+	}
+#endif // DEBUG_ENABLED
+
+	RWLockWrite write_lock(navmesh_rwlock);
+
+	pending_navmesh_vertices.clear();
+	pending_navmesh_polygons.clear();
+
+	if (p_navigation_polygon.is_valid()) {
+		p_navigation_polygon->get_data(pending_navmesh_vertices, pending_navmesh_polygons);
+	}
+
+	polygons_dirty = true;
+
+	request_sync();
+}
+
+ClosestPointQueryResult NavRegion2D::get_closest_point_info(const Vector2 &p_point) const {
+	RWLockRead read_lock(region_rwlock);
+
+	return NavMeshQueries2D::polygons_get_closest_point_info(get_polygons(), p_point);
+}
+
+Vector2 NavRegion2D::get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const {
+	RWLockRead read_lock(region_rwlock);
+
+	if (!get_enabled()) {
+		return Vector2();
+	}
+
+	return NavMeshQueries2D::polygons_get_random_point(get_polygons(), p_navigation_layers, p_uniformly);
+}
+
+void NavRegion2D::set_navigation_layers(uint32_t p_navigation_layers) {
+	if (navigation_layers == p_navigation_layers) {
+		return;
+	}
+	navigation_layers = p_navigation_layers;
+	region_dirty = true;
+
+	request_sync();
+}
+
+void NavRegion2D::set_enter_cost(real_t p_enter_cost) {
+	real_t new_enter_cost = MAX(p_enter_cost, 0.0);
+	if (enter_cost == new_enter_cost) {
+		return;
+	}
+	enter_cost = new_enter_cost;
+	region_dirty = true;
+
+	request_sync();
+}
+
+void NavRegion2D::set_travel_cost(real_t p_travel_cost) {
+	real_t new_travel_cost = MAX(p_travel_cost, 0.0);
+	if (travel_cost == new_travel_cost) {
+		return;
+	}
+	travel_cost = new_travel_cost;
+	region_dirty = true;
+
+	request_sync();
+}
+
+void NavRegion2D::set_owner_id(ObjectID p_owner_id) {
+	if (owner_id == p_owner_id) {
+		return;
+	}
+	owner_id = p_owner_id;
+	region_dirty = true;
+
+	request_sync();
+}
+
+bool NavRegion2D::sync() {
+	RWLockWrite write_lock(region_rwlock);
+
+	bool something_changed = region_dirty || polygons_dirty;
+
+	region_dirty = false;
+
+	update_polygons();
+
+	return something_changed;
+}
+
+void NavRegion2D::update_polygons() {
+	if (!polygons_dirty) {
+		return;
+	}
+	navmesh_polygons.clear();
+	surface_area = 0.0;
+	bounds = Rect2();
+	polygons_dirty = false;
+
+	if (map == nullptr) {
+		return;
+	}
+
+	RWLockRead read_lock(navmesh_rwlock);
+
+	if (pending_navmesh_vertices.is_empty() || pending_navmesh_polygons.is_empty()) {
+		return;
+	}
+
+	int len = pending_navmesh_vertices.size();
+	if (len == 0) {
+		return;
+	}
+
+	const Vector2 *vertices_r = pending_navmesh_vertices.ptr();
+
+	navmesh_polygons.resize(pending_navmesh_polygons.size());
+
+	real_t _new_region_surface_area = 0.0;
+	Rect2 _new_bounds;
+
+	bool first_vertex = true;
+	int navigation_mesh_polygon_index = 0;
+
+	for (Polygon &polygon : navmesh_polygons) {
+		polygon.surface_area = 0.0;
+
+		Vector<int> navigation_mesh_polygon = pending_navmesh_polygons[navigation_mesh_polygon_index];
+		navigation_mesh_polygon_index += 1;
+
+		int navigation_mesh_polygon_size = navigation_mesh_polygon.size();
+		if (navigation_mesh_polygon_size < 3) {
+			continue;
+		}
+
+		const int *indices = navigation_mesh_polygon.ptr();
+		bool valid(true);
+
+		polygon.points.resize(navigation_mesh_polygon_size);
+		polygon.edges.resize(navigation_mesh_polygon_size);
+
+		real_t _new_polygon_surface_area = 0.0;
+
+		for (int j(2); j < navigation_mesh_polygon_size; j++) {
+			const Triangle2 triangle = Triangle2(
+					transform.xform(vertices_r[indices[0]]),
+					transform.xform(vertices_r[indices[j - 1]]),
+					transform.xform(vertices_r[indices[j]]));
+
+			_new_polygon_surface_area += triangle.get_area();
+		}
+
+		polygon.surface_area = _new_polygon_surface_area;
+		_new_region_surface_area += _new_polygon_surface_area;
+
+		for (int j(0); j < navigation_mesh_polygon_size; j++) {
+			int idx = indices[j];
+			if (idx < 0 || idx >= len) {
+				valid = false;
+				break;
+			}
+
+			Vector2 point_position = transform.xform(vertices_r[idx]);
+			polygon.points[j].pos = point_position;
+			polygon.points[j].key = NavMapBuilder2D::get_point_key(point_position, map->get_merge_rasterizer_cell_size());
+
+			if (first_vertex) {
+				first_vertex = false;
+				_new_bounds.position = point_position;
+			} else {
+				_new_bounds.expand_to(point_position);
+			}
+		}
+
+		if (!valid) {
+			ERR_BREAK_MSG(!valid, "The navigation polygon set in this region is not valid!");
+		}
+	}
+
+	surface_area = _new_region_surface_area;
+	bounds = _new_bounds;
+}
+
+void NavRegion2D::get_iteration_update(NavRegionIteration2D &r_iteration) {
+	r_iteration.navigation_layers = get_navigation_layers();
+	r_iteration.enter_cost = get_enter_cost();
+	r_iteration.travel_cost = get_travel_cost();
+	r_iteration.owner_object_id = get_owner_id();
+	r_iteration.owner_type = get_type();
+	r_iteration.owner_rid = get_self();
+
+	r_iteration.enabled = get_enabled();
+	r_iteration.transform = get_transform();
+	r_iteration.owner_use_edge_connections = get_use_edge_connections();
+	r_iteration.bounds = get_bounds();
+	r_iteration.surface_area = get_surface_area();
+
+	r_iteration.navmesh_polygons.clear();
+	r_iteration.navmesh_polygons.resize(navmesh_polygons.size());
+	for (uint32_t i = 0; i < navmesh_polygons.size(); i++) {
+		Polygon &navmesh_polygon = navmesh_polygons[i];
+		navmesh_polygon.owner = &r_iteration;
+		r_iteration.navmesh_polygons[i] = navmesh_polygon;
+	}
+}
+
+void NavRegion2D::request_sync() {
+	if (map && !sync_dirty_request_list_element.in_list()) {
+		map->add_region_sync_dirty_request(&sync_dirty_request_list_element);
+	}
+}
+
+void NavRegion2D::cancel_sync_request() {
+	if (map && sync_dirty_request_list_element.in_list()) {
+		map->remove_region_sync_dirty_request(&sync_dirty_request_list_element);
+	}
+}
+
+NavRegion2D::NavRegion2D() :
+		sync_dirty_request_list_element(this) {
+	type = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_REGION;
+}
+
+NavRegion2D::~NavRegion2D() {
+	cancel_sync_request();
+}

modules/navigation_2d/nav_region_2d.h

@@ -0,0 +1,114 @@
+/**************************************************************************/
+/*  nav_region_2d.h                                                       */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "nav_base_2d.h"
+#include "nav_utils_2d.h"
+
+#include "core/os/rw_lock.h"
+#include "scene/resources/2d/navigation_polygon.h"
+
+struct NavRegionIteration2D;
+
+class NavRegion2D : public NavBase2D {
+	RWLock region_rwlock;
+
+	NavMap2D *map = nullptr;
+	Transform2D transform;
+	bool enabled = true;
+
+	bool use_edge_connections = true;
+
+	bool region_dirty = true;
+	bool polygons_dirty = true;
+
+	LocalVector<nav_2d::Polygon> navmesh_polygons;
+
+	real_t surface_area = 0.0;
+	Rect2 bounds;
+
+	RWLock navmesh_rwlock;
+	Vector<Vector2> pending_navmesh_vertices;
+	Vector<Vector<int>> pending_navmesh_polygons;
+
+	SelfList<NavRegion2D> sync_dirty_request_list_element;
+
+public:
+	NavRegion2D();
+	~NavRegion2D();
+
+	void scratch_polygons() {
+		polygons_dirty = true;
+	}
+
+	void set_enabled(bool p_enabled);
+	bool get_enabled() const { return enabled; }
+
+	void set_map(NavMap2D *p_map);
+	NavMap2D *get_map() const {
+		return map;
+	}
+
+	virtual void set_use_edge_connections(bool p_enabled) override;
+	virtual bool get_use_edge_connections() const override { return use_edge_connections; }
+
+	void set_transform(const Transform2D &p_transform);
+	const Transform2D &get_transform() const {
+		return transform;
+	}
+
+	void set_navigation_polygon(Ref<NavigationPolygon> p_navigation_polygon);
+
+	LocalVector<nav_2d::Polygon> const &get_polygons() const {
+		return navmesh_polygons;
+	}
+
+	nav_2d::ClosestPointQueryResult get_closest_point_info(const Vector2 &p_point) const;
+	Vector2 get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const;
+
+	real_t get_surface_area() const { return surface_area; }
+	Rect2 get_bounds() const { return bounds; }
+
+	// NavBase properties.
+	virtual void set_navigation_layers(uint32_t p_navigation_layers) override;
+	virtual void set_enter_cost(real_t p_enter_cost) override;
+	virtual void set_travel_cost(real_t p_travel_cost) override;
+	virtual void set_owner_id(ObjectID p_owner_id) override;
+
+	bool sync();
+	void request_sync();
+	void cancel_sync_request();
+
+	void get_iteration_update(NavRegionIteration2D &r_iteration);
+
+private:
+	void update_polygons();
+};

modules/navigation_2d/nav_rid_2d.h

@@ -0,0 +1,41 @@
+/**************************************************************************/
+/*  nav_rid_2d.h                                                          */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/templates/rid.h"
+
+class NavRid2D {
+	RID self;
+
+public:
+	_FORCE_INLINE_ void set_self(const RID &p_self) { self = p_self; }
+	_FORCE_INLINE_ RID get_self() const { return self; }
+};

modules/navigation_2d/nav_utils_2d.h

@@ -0,0 +1,214 @@
+/**************************************************************************/
+/*  nav_utils_2d.h                                                        */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/math/vector3.h"
+#include "core/templates/hash_map.h"
+#include "core/templates/hashfuncs.h"
+#include "servers/navigation/nav_heap.h"
+#include "servers/navigation/navigation_utilities.h"
+
+struct NavBaseIteration2D;
+
+namespace nav_2d {
+struct Polygon;
+
+union PointKey {
+	struct {
+		int64_t x : 32;
+		int64_t y : 32;
+	};
+
+	uint64_t key = 0;
+};
+
+struct EdgeKey {
+	PointKey a;
+	PointKey b;
+
+	static uint32_t hash(const EdgeKey &p_val) {
+		return hash_one_uint64(p_val.a.key) ^ hash_one_uint64(p_val.b.key);
+	}
+
+	bool operator==(const EdgeKey &p_key) const {
+		return (a.key == p_key.a.key) && (b.key == p_key.b.key);
+	}
+
+	EdgeKey(const PointKey &p_a = PointKey(), const PointKey &p_b = PointKey()) :
+			a(p_a),
+			b(p_b) {
+		if (a.key > b.key) {
+			SWAP(a, b);
+		}
+	}
+};
+
+struct Point {
+	Vector2 pos;
+	PointKey key;
+};
+
+struct Edge {
+	/// The gateway in the edge, as, in some case, the whole edge might not be navigable.
+	struct Connection {
+		/// Polygon that this connection leads to.
+		Polygon *polygon = nullptr;
+
+		/// Edge of the source polygon where this connection starts from.
+		int edge = -1;
+
+		/// Point on the edge where the gateway leading to the poly starts.
+		Vector2 pathway_start;
+
+		/// Point on the edge where the gateway leading to the poly ends.
+		Vector2 pathway_end;
+	};
+
+	/// Connections from this edge to other polygons.
+	LocalVector<Connection> connections;
+};
+
+struct Polygon {
+	/// Id of the polygon in the map.
+	uint32_t id = UINT32_MAX;
+
+	/// Navigation region or link that contains this polygon.
+	const NavBaseIteration2D *owner = nullptr;
+
+	/// The points of this `Polygon`
+	LocalVector<Point> points;
+
+	/// The edges of this `Polygon`
+	LocalVector<Edge> edges;
+
+	real_t surface_area = 0.0;
+};
+
+struct NavigationPoly {
+	/// This poly.
+	const Polygon *poly = nullptr;
+
+	/// Index in the heap of traversable polygons.
+	uint32_t traversable_poly_index = UINT32_MAX;
+
+	/// Those 4 variables are used to travel the path backwards.
+	int back_navigation_poly_id = -1;
+	int back_navigation_edge = -1;
+	Vector2 back_navigation_edge_pathway_start;
+	Vector2 back_navigation_edge_pathway_end;
+
+	/// The entry position of this poly.
+	Vector2 entry;
+	/// The distance traveled until now (g cost).
+	real_t traveled_distance = 0.0;
+	/// The distance to the destination (h cost).
+	real_t distance_to_destination = 0.0;
+
+	/// The total travel cost (f cost).
+	real_t total_travel_cost() const {
+		return traveled_distance + distance_to_destination;
+	}
+
+	bool operator==(const NavigationPoly &p_other) const {
+		return poly == p_other.poly;
+	}
+
+	bool operator!=(const NavigationPoly &p_other) const {
+		return !(*this == p_other);
+	}
+
+	void reset() {
+		poly = nullptr;
+		traversable_poly_index = UINT32_MAX;
+		back_navigation_poly_id = -1;
+		back_navigation_edge = -1;
+		traveled_distance = FLT_MAX;
+		distance_to_destination = 0.0;
+	}
+};
+
+struct NavPolyTravelCostGreaterThan {
+	// Returns `true` if the travel cost of `a` is higher than that of `b`.
+	bool operator()(const NavigationPoly *p_poly_a, const NavigationPoly *p_poly_b) const {
+		real_t f_cost_a = p_poly_a->total_travel_cost();
+		real_t h_cost_a = p_poly_a->distance_to_destination;
+		real_t f_cost_b = p_poly_b->total_travel_cost();
+		real_t h_cost_b = p_poly_b->distance_to_destination;
+
+		if (f_cost_a != f_cost_b) {
+			return f_cost_a > f_cost_b;
+		} else {
+			return h_cost_a > h_cost_b;
+		}
+	}
+};
+
+struct NavPolyHeapIndexer {
+	void operator()(NavigationPoly *p_poly, uint32_t p_heap_index) const {
+		p_poly->traversable_poly_index = p_heap_index;
+	}
+};
+
+struct ClosestPointQueryResult {
+	Vector2 point;
+	RID owner;
+};
+
+struct EdgeConnectionPair {
+	Edge::Connection connections[2];
+	int size = 0;
+};
+
+struct PerformanceData {
+	int pm_region_count = 0;
+	int pm_agent_count = 0;
+	int pm_link_count = 0;
+	int pm_polygon_count = 0;
+	int pm_edge_count = 0;
+	int pm_edge_merge_count = 0;
+	int pm_edge_connection_count = 0;
+	int pm_edge_free_count = 0;
+	int pm_obstacle_count = 0;
+
+	void reset() {
+		pm_region_count = 0;
+		pm_agent_count = 0;
+		pm_link_count = 0;
+		pm_polygon_count = 0;
+		pm_edge_count = 0;
+		pm_edge_merge_count = 0;
+		pm_edge_connection_count = 0;
+		pm_edge_free_count = 0;
+		pm_obstacle_count = 0;
+	}
+};
+
+} // namespace nav_2d

modules/navigation_2d/register_types.cpp

@@ -0,0 +1,52 @@
+/**************************************************************************/
+/*  register_types.cpp                                                    */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "register_types.h"
+
+#include "2d/godot_navigation_server_2d.h"
+
+#include "core/config/engine.h"
+#include "servers/navigation_server_2d.h"
+
+NavigationServer2D *new_navigation_server_2d() {
+	return memnew(GodotNavigationServer2D);
+}
+
+void initialize_navigation_2d_module(ModuleInitializationLevel p_level) {
+	if (p_level == MODULE_INITIALIZATION_LEVEL_SERVERS) {
+		NavigationServer2DManager::set_default_server(new_navigation_server_2d);
+	}
+}
+
+void uninitialize_navigation_2d_module(ModuleInitializationLevel p_level) {
+	if (p_level != MODULE_INITIALIZATION_LEVEL_SERVERS) {
+		return;
+	}
+}

modules/navigation/register_types.h → modules/navigation_2d/register_types.h

@@ -32,5 +32,5 @@
 
 #include "modules/register_module_types.h"
 
-void initialize_navigation_module(ModuleInitializationLevel p_level);
-void uninitialize_navigation_module(ModuleInitializationLevel p_level);
+void initialize_navigation_2d_module(ModuleInitializationLevel p_level);
+void uninitialize_navigation_2d_module(ModuleInitializationLevel p_level);

modules/navigation_2d/triangle2.cpp

@@ -0,0 +1,112 @@
+/**************************************************************************/
+/*  triangle2.cpp                                                         */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "triangle2.h"
+
+Vector2 Triangle2::get_random_point_inside() const {
+	real_t a = Math::random(0.0, 1.0);
+	real_t b = Math::random(0.0, 1.0);
+	if (a > b) {
+		SWAP(a, b);
+	}
+
+	return vertex[0] * a + vertex[1] * (b - a) + vertex[2] * (1.0f - b);
+}
+
+Vector2 Triangle2::get_closest_point_to(const Vector2 &p_point) const {
+	Vector2 edge0 = vertex[1] - vertex[0];
+	Vector2 edge1 = vertex[2] - vertex[0];
+	Vector2 v0 = vertex[0] - p_point;
+
+	real_t a = edge0.dot(edge0);
+	real_t b = edge0.dot(edge1);
+	real_t c = edge1.dot(edge1);
+	real_t d = edge0.dot(v0);
+	real_t e = edge1.dot(v0);
+
+	real_t det = a * c - b * b;
+	real_t s = b * e - c * d;
+	real_t t = b * d - a * e;
+
+	if (s + t < det) {
+		if (s < 0.f) {
+			if (t < 0.f) {
+				if (d < 0.f) {
+					s = CLAMP(-d / a, 0.f, 1.f);
+					t = 0.f;
+				} else {
+					s = 0.f;
+					t = CLAMP(-e / c, 0.f, 1.f);
+				}
+			} else {
+				s = 0.f;
+				t = CLAMP(-e / c, 0.f, 1.f);
+			}
+		} else if (t < 0.f) {
+			s = CLAMP(-d / a, 0.f, 1.f);
+			t = 0.f;
+		} else {
+			real_t inv_det = 1.f / det;
+			s *= inv_det;
+			t *= inv_det;
+		}
+	} else {
+		if (s < 0.f) {
+			real_t tmp0 = b + d;
+			real_t tmp1 = c + e;
+			if (tmp1 > tmp0) {
+				real_t numer = tmp1 - tmp0;
+				real_t denom = a - 2 * b + c;
+				s = CLAMP(numer / denom, 0.f, 1.f);
+				t = 1 - s;
+			} else {
+				t = CLAMP(-e / c, 0.f, 1.f);
+				s = 0.f;
+			}
+		} else if (t < 0.f) {
+			if (a + d > b + e) {
+				real_t numer = c + e - b - d;
+				real_t denom = a - 2 * b + c;
+				s = CLAMP(numer / denom, 0.f, 1.f);
+				t = 1 - s;
+			} else {
+				s = CLAMP(-d / a, 0.f, 1.f);
+				t = 0.f;
+			}
+		} else {
+			real_t numer = c + e - b - d;
+			real_t denom = a - 2 * b + c;
+			s = CLAMP(numer / denom, 0.f, 1.f);
+			t = 1.f - s;
+		}
+	}
+
+	return vertex[0] + s * edge0 + t * edge1;
+}

modules/navigation_2d/triangle2.h

@@ -0,0 +1,52 @@
+/**************************************************************************/
+/*  triangle2.h                                                           */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "core/math/vector2.h"
+
+struct Triangle2 {
+	Vector2 vertex[3];
+
+	real_t get_area() const {
+		return Math::sqrt((vertex[0] - vertex[1]).cross(vertex[0] - vertex[2])) * 0.5f;
+	}
+
+	Vector2 get_random_point_inside() const;
+
+	Vector2 get_closest_point_to(const Vector2 &p_point) const;
+
+	Triangle2() {}
+	Triangle2(const Vector2 &p_v1, const Vector2 &p_v2, const Vector2 &p_v3) {
+		vertex[0] = p_v1;
+		vertex[1] = p_v2;
+		vertex[2] = p_v3;
+	}
+};

modules/navigation/3d/godot_navigation_server_3d.cpp → modules/navigation_3d/3d/godot_navigation_server_3d.cpp

@@ -33,9 +33,7 @@
 #include "core/os/mutex.h"
 #include "scene/main/node.h"
 
-#ifndef _3D_DISABLED
 #include "nav_mesh_generator_3d.h"
-#endif // _3D_DISABLED
 
 using namespace NavigationUtilities;
 
@@ -527,13 +525,11 @@ void GodotNavigationServer3D::region_bake_navigation_mesh(Ref<NavigationMesh> p_
 
 	WARN_PRINT_ONCE("NavigationServer3D::region_bake_navigation_mesh() is deprecated due to core threading changes. To upgrade existing code, first create a NavigationMeshSourceGeometryData3D resource. Use this resource with method parse_source_geometry_data() to parse the SceneTree for nodes that should contribute to the navigation mesh baking. The SceneTree parsing needs to happen on the main thread. After the parsing is finished use the resource with method bake_from_source_geometry_data() to bake a navigation mesh..");
 
-#ifndef _3D_DISABLED
 	p_navigation_mesh->clear();
 	Ref<NavigationMeshSourceGeometryData3D> source_geometry_data;
 	source_geometry_data.instantiate();
 	parse_source_geometry_data(p_navigation_mesh, source_geometry_data, p_root_node);
 	bake_from_source_geometry_data(p_navigation_mesh, source_geometry_data);
-#endif
 }
 #endif // DISABLE_DEPRECATED
 
@@ -1168,7 +1164,6 @@ uint32_t GodotNavigationServer3D::obstacle_get_avoidance_layers(RID p_obstacle)
 }
 
 void GodotNavigationServer3D::parse_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data, Node *p_root_node, const Callable &p_callback) {
-#ifndef _3D_DISABLED
 	ERR_FAIL_COND_MSG(!Thread::is_main_thread(), "The SceneTree can only be parsed on the main thread. Call this function from the main thread or use call_deferred().");
 	ERR_FAIL_COND_MSG(p_navigation_mesh.is_null(), "Invalid navigation mesh.");
 	ERR_FAIL_NULL_MSG(p_root_node, "No parsing root node specified.");
@@ -1176,35 +1171,26 @@ void GodotNavigationServer3D::parse_source_geometry_data(const Ref<NavigationMes
 
 	ERR_FAIL_NULL(NavMeshGenerator3D::get_singleton());
 	NavMeshGenerator3D::get_singleton()->parse_source_geometry_data(p_navigation_mesh, p_source_geometry_data, p_root_node, p_callback);
-#endif // _3D_DISABLED
 }
 
 void GodotNavigationServer3D::bake_from_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data, const Callable &p_callback) {
-#ifndef _3D_DISABLED
 	ERR_FAIL_COND_MSG(p_navigation_mesh.is_null(), "Invalid navigation mesh.");
 	ERR_FAIL_COND_MSG(p_source_geometry_data.is_null(), "Invalid NavigationMeshSourceGeometryData3D.");
 
 	ERR_FAIL_NULL(NavMeshGenerator3D::get_singleton());
 	NavMeshGenerator3D::get_singleton()->bake_from_source_geometry_data(p_navigation_mesh, p_source_geometry_data, p_callback);
-#endif // _3D_DISABLED
 }
 
 void GodotNavigationServer3D::bake_from_source_geometry_data_async(const Ref<NavigationMesh> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data, const Callable &p_callback) {
-#ifndef _3D_DISABLED
 	ERR_FAIL_COND_MSG(p_navigation_mesh.is_null(), "Invalid navigation mesh.");
 	ERR_FAIL_COND_MSG(p_source_geometry_data.is_null(), "Invalid NavigationMeshSourceGeometryData3D.");
 
 	ERR_FAIL_NULL(NavMeshGenerator3D::get_singleton());
 	NavMeshGenerator3D::get_singleton()->bake_from_source_geometry_data_async(p_navigation_mesh, p_source_geometry_data, p_callback);
-#endif // _3D_DISABLED
 }
 
 bool GodotNavigationServer3D::is_baking_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh) const {
-#ifdef _3D_DISABLED
-	return false;
-#else
 	return NavMeshGenerator3D::get_singleton()->is_baking(p_navigation_mesh);
-#endif // _3D_DISABLED
 }
 
 COMMAND_1(free, RID, p_object) {
@@ -1277,9 +1263,7 @@ COMMAND_1(free, RID, p_object) {
 		ERR_FAIL_NULL(parser);
 
 		generator_parsers.erase(parser);
-#ifndef _3D_DISABLED
 		NavMeshGenerator3D::get_singleton()->set_generator_parsers(generator_parsers);
-#endif
 		geometry_parser_owner.free(parser->self);
 
 	} else {
@@ -1351,11 +1335,9 @@ uint32_t GodotNavigationServer3D::map_get_iteration_id(RID p_map) const {
 }
 
 void GodotNavigationServer3D::sync() {
-#ifndef _3D_DISABLED
 	if (navmesh_generator_3d) {
 		navmesh_generator_3d->sync();
 	}
-#endif // _3D_DISABLED
 }
 
 void GodotNavigationServer3D::process(real_t p_delta_time) {
@@ -1413,22 +1395,18 @@ void GodotNavigationServer3D::process(real_t p_delta_time) {
 }
 
 void GodotNavigationServer3D::init() {
-#ifndef _3D_DISABLED
 	navmesh_generator_3d = memnew(NavMeshGenerator3D);
 	RWLockRead read_lock(geometry_parser_rwlock);
 	navmesh_generator_3d->set_generator_parsers(generator_parsers);
-#endif // _3D_DISABLED
 }
 
 void GodotNavigationServer3D::finish() {
 	flush_queries();
-#ifndef _3D_DISABLED
 	if (navmesh_generator_3d) {
 		navmesh_generator_3d->finish();
 		memdelete(navmesh_generator_3d);
 		navmesh_generator_3d = nullptr;
 	}
-#endif // _3D_DISABLED
 }
 
 void GodotNavigationServer3D::query_path(const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result, const Callable &p_callback) {
@@ -1450,9 +1428,7 @@ RID GodotNavigationServer3D::source_geometry_parser_create() {
 	parser->self = rid;
 
 	generator_parsers.push_back(parser);
-#ifndef _3D_DISABLED
 	NavMeshGenerator3D::get_singleton()->set_generator_parsers(generator_parsers);
-#endif
 	return rid;
 }
 

modules/navigation/3d/godot_navigation_server_3d.h → modules/navigation_3d/3d/godot_navigation_server_3d.h

@@ -57,9 +57,7 @@
 	void MERGE(_cmd_, F_NAME)(T_0 D_0, T_1 D_1)
 
 class GodotNavigationServer3D;
-#ifndef _3D_DISABLED
 class NavMeshGenerator3D;
-#endif // _3D_DISABLED
 
 struct SetCommand3D {
 	virtual ~SetCommand3D() {}
@@ -83,9 +81,7 @@ class GodotNavigationServer3D : public NavigationServer3D {
 	LocalVector<NavMap3D *> active_maps;
 	LocalVector<uint32_t> active_maps_iteration_id;
 
-#ifndef _3D_DISABLED
 	NavMeshGenerator3D *navmesh_generator_3d = nullptr;
-#endif // _3D_DISABLED
 
 	// Performance Monitor
 	int pm_region_count = 0;

modules/navigation/3d/nav_map_builder_3d.cpp → modules/navigation_3d/3d/nav_map_builder_3d.cpp

@@ -28,8 +28,6 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
 /**************************************************************************/
 
-#ifndef _3D_DISABLED
-
 #include "nav_map_builder_3d.h"
 
 #include "../nav_link_3d.h"
@@ -406,5 +404,3 @@ void NavMapBuilder3D::_build_update_map_iteration(NavMapIterationBuild3D &r_buil
 	}
 	map_iteration->path_query_slots_mutex.unlock();
 }
-
-#endif // _3D_DISABLED

modules/navigation/3d/nav_mesh_generator_3d.cpp → modules/navigation_3d/3d/nav_mesh_generator_3d.cpp

@@ -28,8 +28,6 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
 /**************************************************************************/
 
-#ifndef _3D_DISABLED
-
 #include "nav_mesh_generator_3d.h"
 
 #include "core/config/project_settings.h"
@@ -560,5 +558,3 @@ bool NavMeshGenerator3D::generator_emit_callback(const Callable &p_callback) {
 
 	return ce.error == Callable::CallError::CALL_OK;
 }
-
-#endif // _3D_DISABLED

modules/navigation/3d/nav_mesh_generator_3d.h → modules/navigation_3d/3d/nav_mesh_generator_3d.h

@@ -30,8 +30,6 @@
 
 #pragma once
 
-#ifndef _3D_DISABLED
-
 #include "core/object/class_db.h"
 #include "core/object/worker_thread_pool.h"
 #include "core/templates/rid_owner.h"
@@ -99,5 +97,3 @@ public:
 	NavMeshGenerator3D();
 	~NavMeshGenerator3D();
 };
-
-#endif // _3D_DISABLED

modules/navigation/3d/nav_mesh_queries_3d.cpp → modules/navigation_3d/3d/nav_mesh_queries_3d.cpp

@@ -28,8 +28,6 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
 /**************************************************************************/
 
-#ifndef _3D_DISABLED
-
 #include "nav_mesh_queries_3d.h"
 
 #include "../nav_base_3d.h"
@@ -1203,5 +1201,3 @@ void NavMeshQueries3D::simplify_path_segment(int p_start_inx, int p_end_inx, con
 		simplify_path_segment(point_max_index, p_end_inx, p_points, p_epsilon, r_simplified_path_indices);
 	}
 }
-
-#endif // _3D_DISABLED

modules/navigation/3d/nav_mesh_queries_3d.h → modules/navigation_3d/3d/nav_mesh_queries_3d.h

@@ -30,8 +30,6 @@
 
 #pragma once
 
-#ifndef _3D_DISABLED
-
 #include "../nav_utils_3d.h"
 
 #include "servers/navigation/navigation_path_query_parameters_3d.h"
@@ -146,5 +144,3 @@ public:
 	static void simplify_path_segment(int p_start_inx, int p_end_inx, const LocalVector<Vector3> &p_points, real_t p_epsilon, LocalVector<uint32_t> &r_simplified_path_indices);
 	static LocalVector<uint32_t> get_simplified_path_indices(const LocalVector<Vector3> &p_path, real_t p_epsilon);
 };
-
-#endif // _3D_DISABLED

modules/navigation/3d/navigation_mesh_generator.cpp → modules/navigation_3d/3d/navigation_mesh_generator.cpp

@@ -28,8 +28,6 @@
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
 /**************************************************************************/
 
-#ifndef _3D_DISABLED
-
 #include "navigation_mesh_generator.h"
 
 #include "scene/resources/3d/navigation_mesh_source_geometry_data_3d.h"
@@ -74,5 +72,3 @@ void NavigationMeshGenerator::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("parse_source_geometry_data", "navigation_mesh", "source_geometry_data", "root_node", "callback"), &NavigationMeshGenerator::parse_source_geometry_data, DEFVAL(Callable()));
 	ClassDB::bind_method(D_METHOD("bake_from_source_geometry_data", "navigation_mesh", "source_geometry_data", "callback"), &NavigationMeshGenerator::bake_from_source_geometry_data, DEFVAL(Callable()));
 }
-
-#endif

modules/navigation/3d/navigation_mesh_generator.h → modules/navigation_3d/3d/navigation_mesh_generator.h

@@ -30,8 +30,6 @@
 
 #pragma once
 
-#ifndef _3D_DISABLED
-
 #include "scene/3d/navigation_region_3d.h"
 #include "scene/resources/navigation_mesh.h"
 
@@ -57,5 +55,3 @@ public:
 	void parse_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_root_node, const Callable &p_callback = Callable());
 	void bake_from_source_geometry_data(Ref<NavigationMesh> p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data, const Callable &p_callback = Callable());
 };
-
-#endif

modules/navigation/SCsub → modules/navigation_3d/SCsub

@@ -4,11 +4,12 @@ from misc.utility.scons_hints import *
 Import("env")
 Import("env_modules")
 
-env_navigation = env_modules.Clone()
+env_navigation_3d = env_modules.Clone()
 
 # Thirdparty source files
 
 thirdparty_obj = []
+navigation_2d_enabled = "navigation_2d" in env.module_list
 
 # Recast Thirdparty source files
 if env["builtin_recastnavigation"]:
@@ -28,9 +29,9 @@ if env["builtin_recastnavigation"]:
     ]
     thirdparty_sources = [thirdparty_dir + file for file in thirdparty_sources]
 
-    env_navigation.Prepend(CPPPATH=[thirdparty_dir + "Include"])
+    env_navigation_3d.Prepend(CPPPATH=[thirdparty_dir + "Include"])
 
-    env_thirdparty = env_navigation.Clone()
+    env_thirdparty = env_navigation_3d.Clone()
     env_thirdparty.disable_warnings()
     env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources)
 
@@ -45,11 +46,13 @@ if env["builtin_rvo2_2d"]:
     ]
     thirdparty_sources = [thirdparty_dir + file for file in thirdparty_sources]
 
-    env_navigation.Prepend(CPPPATH=[thirdparty_dir])
+    env_navigation_3d.Prepend(CPPPATH=[thirdparty_dir])
 
-    env_thirdparty = env_navigation.Clone()
-    env_thirdparty.disable_warnings()
-    env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources)
+    # Don't build rvo_2d if 2D navigation is enabled.
+    if not navigation_2d_enabled:
+        env_thirdparty = env_navigation_3d.Clone()
+        env_thirdparty.disable_warnings()
+        env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources)
 
 # RVO 3D Thirdparty source files
 if env["builtin_rvo2_3d"]:
@@ -61,9 +64,9 @@ if env["builtin_rvo2_3d"]:
     ]
     thirdparty_sources = [thirdparty_dir + file for file in thirdparty_sources]
 
-    env_navigation.Prepend(CPPPATH=[thirdparty_dir])
+    env_navigation_3d.Prepend(CPPPATH=[thirdparty_dir])
 
-    env_thirdparty = env_navigation.Clone()
+    env_thirdparty = env_navigation_3d.Clone()
     env_thirdparty.disable_warnings()
     env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources)
 
@@ -74,12 +77,10 @@ env.modules_sources += thirdparty_obj
 
 module_obj = []
 
-env_navigation.add_source_files(module_obj, "*.cpp")
-env_navigation.add_source_files(module_obj, "2d/*.cpp")
-if not env["disable_3d"]:
-    env_navigation.add_source_files(module_obj, "3d/*.cpp")
+env_navigation_3d.add_source_files(module_obj, "*.cpp")
+env_navigation_3d.add_source_files(module_obj, "3d/*.cpp")
 if env.editor_build:
-    env_navigation.add_source_files(module_obj, "editor/*.cpp")
+    env_navigation_3d.add_source_files(module_obj, "editor/*.cpp")
 env.modules_sources += module_obj
 
 # Needed to force rebuilding the module files when the thirdparty library is updated.

modules/navigation/register_types.cpp → modules/navigation_3d/register_types.cpp

@@ -30,13 +30,10 @@
 
 #include "register_types.h"
 
-#include "2d/godot_navigation_server_2d.h"
 #include "3d/godot_navigation_server_3d.h"
 
 #ifndef DISABLE_DEPRECATED
-#ifndef _3D_DISABLED
 #include "3d/navigation_mesh_generator.h"
-#endif
 #endif // DISABLE_DEPRECATED
 
 #ifdef TOOLS_ENABLED
@@ -44,34 +41,24 @@
 #endif
 
 #include "core/config/engine.h"
-#include "servers/navigation_server_2d.h"
 #include "servers/navigation_server_3d.h"
 
 #ifndef DISABLE_DEPRECATED
-#ifndef _3D_DISABLED
 NavigationMeshGenerator *_nav_mesh_generator = nullptr;
-#endif
 #endif // DISABLE_DEPRECATED
 
 NavigationServer3D *new_navigation_server_3d() {
 	return memnew(GodotNavigationServer3D);
 }
 
-NavigationServer2D *new_navigation_server_2d() {
-	return memnew(GodotNavigationServer2D);
-}
-
-void initialize_navigation_module(ModuleInitializationLevel p_level) {
+void initialize_navigation_3d_module(ModuleInitializationLevel p_level) {
 	if (p_level == MODULE_INITIALIZATION_LEVEL_SERVERS) {
 		NavigationServer3DManager::set_default_server(new_navigation_server_3d);
-		NavigationServer2DManager::set_default_server(new_navigation_server_2d);
 
 #ifndef DISABLE_DEPRECATED
-#ifndef _3D_DISABLED
 		_nav_mesh_generator = memnew(NavigationMeshGenerator);
 		GDREGISTER_CLASS(NavigationMeshGenerator);
 		Engine::get_singleton()->add_singleton(Engine::Singleton("NavigationMeshGenerator", NavigationMeshGenerator::get_singleton()));
-#endif
 #endif // DISABLE_DEPRECATED
 	}
 
@@ -82,16 +69,14 @@ void initialize_navigation_module(ModuleInitializationLevel p_level) {
 #endif
 }
 
-void uninitialize_navigation_module(ModuleInitializationLevel p_level) {
+void uninitialize_navigation_3d_module(ModuleInitializationLevel p_level) {
 	if (p_level != MODULE_INITIALIZATION_LEVEL_SERVERS) {
 		return;
 	}
 
 #ifndef DISABLE_DEPRECATED
-#ifndef _3D_DISABLED
 	if (_nav_mesh_generator) {
 		memdelete(_nav_mesh_generator);
 	}
-#endif
 #endif // DISABLE_DEPRECATED
 }

modules/navigation_3d/register_types.h

@@ -0,0 +1,36 @@
+/**************************************************************************/
+/*  register_types.h                                                      */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#pragma once
+
+#include "modules/register_module_types.h"
+
+void initialize_navigation_3d_module(ModuleInitializationLevel p_level);
+void uninitialize_navigation_3d_module(ModuleInitializationLevel p_level);

scu_builders.py

@@ -315,9 +315,10 @@ def generate_scu_files(max_includes_per_scu):
     process_folder(["modules/gltf/editor"])
     process_folder(["modules/gltf/extensions"])
     process_folder(["modules/gltf/extensions/physics"])
-    process_folder(["modules/navigation"])
-    process_folder(["modules/navigation/2d"])
-    process_folder(["modules/navigation/3d"])
+    process_folder(["modules/navigation_3d"])
+    process_folder(["modules/navigation_3d/3d"])
+    process_folder(["modules/navigation_2d"])
+    process_folder(["modules/navigation_2d/2d"])
     process_folder(["modules/webrtc"])
     process_folder(["modules/websocket"])
     process_folder(["modules/gridmap"])

servers/navigation/navigation_globals.h

@@ -54,6 +54,7 @@ namespace NavigationDefaults2D {
 
 // Same as in 3D but larger since 1px is treated as 1m.
 constexpr float navmesh_cell_size{ 1.0f }; // Must match ProjectSettings default 2D cell_size.
+constexpr float navmesh_cell_size_min{ 0.01f };
 constexpr auto navmesh_cell_size_hint{ "0.001,100,0.001,or_greater" };
 
 // Map.

servers/navigation/navigation_path_query_result_2d.cpp

@@ -69,6 +69,47 @@ void NavigationPathQueryResult2D::reset() {
 	path_owner_ids.clear();
 }
 
+void NavigationPathQueryResult2D::set_data(const LocalVector<Vector2> &p_path, const LocalVector<int32_t> &p_path_types, const LocalVector<RID> &p_path_rids, const LocalVector<int64_t> &p_path_owner_ids) {
+	path.clear();
+	path_types.clear();
+	path_rids.clear();
+	path_owner_ids.clear();
+
+	{
+		path.resize(p_path.size());
+		Vector2 *w = path.ptrw();
+		const Vector2 *r = p_path.ptr();
+		for (uint32_t i = 0; i < p_path.size(); i++) {
+			w[i] = r[i];
+		}
+	}
+
+	{
+		path_types.resize(p_path_types.size());
+		int32_t *w = path_types.ptrw();
+		const int32_t *r = p_path_types.ptr();
+		for (uint32_t i = 0; i < p_path_types.size(); i++) {
+			w[i] = r[i];
+		}
+	}
+
+	{
+		path_rids.resize(p_path_rids.size());
+		for (uint32_t i = 0; i < p_path_rids.size(); i++) {
+			path_rids[i] = p_path_rids[i];
+		}
+	}
+
+	{
+		path_owner_ids.resize(p_path_owner_ids.size());
+		int64_t *w = path_owner_ids.ptrw();
+		const int64_t *r = p_path_owner_ids.ptr();
+		for (uint32_t i = 0; i < p_path_owner_ids.size(); i++) {
+			w[i] = r[i];
+		}
+	}
+}
+
 void NavigationPathQueryResult2D::_bind_methods() {
 	ClassDB::bind_method(D_METHOD("set_path", "path"), &NavigationPathQueryResult2D::set_path);
 	ClassDB::bind_method(D_METHOD("get_path"), &NavigationPathQueryResult2D::get_path);

servers/navigation/navigation_path_query_result_2d.h

@@ -63,6 +63,8 @@ public:
 	const Vector<int64_t> &get_path_owner_ids() const;
 
 	void reset();
+
+	void set_data(const LocalVector<Vector2> &p_path, const LocalVector<int32_t> &p_path_types, const LocalVector<RID> &p_path_rids, const LocalVector<int64_t> &p_path_owner_ids);
 };
 
 VARIANT_ENUM_CAST(NavigationPathQueryResult2D::PathSegmentType);

servers/navigation_server_2d.cpp

@@ -31,8 +31,10 @@
 #include "navigation_server_2d.h"
 #include "navigation_server_2d.compat.inc"
 
+#include "core/config/project_settings.h"
+#include "scene/main/node.h"
+#include "servers/navigation/navigation_globals.h"
 #include "servers/navigation_server_2d_dummy.h"
-#include "servers/navigation_server_3d.h"
 
 NavigationServer2D *NavigationServer2D::singleton = nullptr;
 
@@ -182,12 +184,28 @@ void NavigationServer2D::_bind_methods() {
 
 	ClassDB::bind_method(D_METHOD("free_rid", "rid"), &NavigationServer2D::free);
 
+	ClassDB::bind_method(D_METHOD("set_active", "active"), &NavigationServer2D::set_active);
+
 	ClassDB::bind_method(D_METHOD("set_debug_enabled", "enabled"), &NavigationServer2D::set_debug_enabled);
 	ClassDB::bind_method(D_METHOD("get_debug_enabled"), &NavigationServer2D::get_debug_enabled);
 
 	ADD_SIGNAL(MethodInfo("map_changed", PropertyInfo(Variant::RID, "map")));
 
 	ADD_SIGNAL(MethodInfo("navigation_debug_changed"));
+	ADD_SIGNAL(MethodInfo("avoidance_debug_changed"));
+
+	ClassDB::bind_method(D_METHOD("get_process_info", "process_info"), &NavigationServer2D::get_process_info);
+
+	BIND_ENUM_CONSTANT(INFO_ACTIVE_MAPS);
+	BIND_ENUM_CONSTANT(INFO_REGION_COUNT);
+	BIND_ENUM_CONSTANT(INFO_AGENT_COUNT);
+	BIND_ENUM_CONSTANT(INFO_LINK_COUNT);
+	BIND_ENUM_CONSTANT(INFO_POLYGON_COUNT);
+	BIND_ENUM_CONSTANT(INFO_EDGE_COUNT);
+	BIND_ENUM_CONSTANT(INFO_EDGE_MERGE_COUNT);
+	BIND_ENUM_CONSTANT(INFO_EDGE_CONNECTION_COUNT);
+	BIND_ENUM_CONSTANT(INFO_EDGE_FREE_COUNT);
+	BIND_ENUM_CONSTANT(INFO_OBSTACLE_COUNT);
 }
 
 NavigationServer2D *NavigationServer2D::get_singleton() {
@@ -197,19 +215,49 @@ NavigationServer2D *NavigationServer2D::get_singleton() {
 NavigationServer2D::NavigationServer2D() {
 	ERR_FAIL_COND(singleton != nullptr);
 	singleton = this;
-	ERR_FAIL_NULL_MSG(NavigationServer3D::get_singleton(), "The Navigation3D singleton should be initialized before the 2D one.");
-	NavigationServer3D::get_singleton()->connect("map_changed", callable_mp(this, &NavigationServer2D::_emit_map_changed));
 
-#ifdef DEBUG_ENABLED
-	NavigationServer3D::get_singleton()->connect(SNAME("navigation_debug_changed"), callable_mp(this, &NavigationServer2D::_emit_navigation_debug_changed_signal));
-#endif // DEBUG_ENABLED
-}
+	GLOBAL_DEF_BASIC(PropertyInfo(Variant::FLOAT, "navigation/2d/default_cell_size", PROPERTY_HINT_RANGE, NavigationDefaults2D::navmesh_cell_size_hint), NavigationDefaults2D::navmesh_cell_size);
+	GLOBAL_DEF("navigation/2d/use_edge_connections", true);
+	GLOBAL_DEF_BASIC(PropertyInfo(Variant::FLOAT, "navigation/2d/default_edge_connection_margin", PROPERTY_HINT_RANGE, "0.01,10,0.001,or_greater"), NavigationDefaults2D::edge_connection_margin);
+	GLOBAL_DEF_BASIC(PropertyInfo(Variant::FLOAT, "navigation/2d/default_link_connection_radius", PROPERTY_HINT_RANGE, "0.01,10,0.001,or_greater"), NavigationDefaults2D::link_connection_radius);
 
 #ifdef DEBUG_ENABLED
-void NavigationServer2D::_emit_navigation_debug_changed_signal() {
-	emit_signal(SNAME("navigation_debug_changed"));
-}
+	debug_navigation_edge_connection_color = GLOBAL_DEF("debug/shapes/navigation/2d/edge_connection_color", Color(1.0, 0.0, 1.0, 1.0));
+	debug_navigation_geometry_edge_color = GLOBAL_DEF("debug/shapes/navigation/2d/geometry_edge_color", Color(0.5, 1.0, 1.0, 1.0));
+	debug_navigation_geometry_face_color = GLOBAL_DEF("debug/shapes/navigation/2d/geometry_face_color", Color(0.5, 1.0, 1.0, 0.4));
+	debug_navigation_geometry_edge_disabled_color = GLOBAL_DEF("debug/shapes/navigation/2d/geometry_edge_disabled_color", Color(0.5, 0.5, 0.5, 1.0));
+	debug_navigation_geometry_face_disabled_color = GLOBAL_DEF("debug/shapes/navigation/2d/geometry_face_disabled_color", Color(0.5, 0.5, 0.5, 0.4));
+	debug_navigation_link_connection_color = GLOBAL_DEF("debug/shapes/navigation/2d/link_connection_color", Color(1.0, 0.5, 1.0, 1.0));
+	debug_navigation_link_connection_disabled_color = GLOBAL_DEF("debug/shapes/navigation/2d/link_connection_disabled_color", Color(0.5, 0.5, 0.5, 1.0));
+	debug_navigation_agent_path_color = GLOBAL_DEF("debug/shapes/navigation/2d/agent_path_color", Color(1.0, 0.0, 0.0, 1.0));
+
+	debug_navigation_enable_edge_connections = GLOBAL_DEF("debug/shapes/navigation/2d/enable_edge_connections", true);
+	debug_navigation_enable_edge_lines = GLOBAL_DEF("debug/shapes/navigation/2d/enable_edge_lines", true);
+	debug_navigation_enable_geometry_face_random_color = GLOBAL_DEF("debug/shapes/navigation/2d/enable_geometry_face_random_color", true);
+	debug_navigation_enable_link_connections = GLOBAL_DEF("debug/shapes/navigation/2d/enable_link_connections", true);
+
+	debug_navigation_enable_agent_paths = GLOBAL_DEF("debug/shapes/navigation/2d/enable_agent_paths", true);
+	debug_navigation_agent_path_point_size = GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "debug/shapes/navigation/2d/agent_path_point_size", PROPERTY_HINT_RANGE, "0.01,10,0.001,or_greater"), 4.0);
+
+	debug_navigation_avoidance_agents_radius_color = GLOBAL_DEF("debug/shapes/avoidance/2d/agents_radius_color", Color(1.0, 1.0, 0.0, 0.25));
+	debug_navigation_avoidance_obstacles_radius_color = GLOBAL_DEF("debug/shapes/avoidance/2d/obstacles_radius_color", Color(1.0, 0.5, 0.0, 0.25));
+	debug_navigation_avoidance_static_obstacle_pushin_face_color = GLOBAL_DEF("debug/shapes/avoidance/2d/obstacles_static_face_pushin_color", Color(1.0, 0.0, 0.0, 0.0));
+	debug_navigation_avoidance_static_obstacle_pushin_edge_color = GLOBAL_DEF("debug/shapes/avoidance/2d/obstacles_static_edge_pushin_color", Color(1.0, 0.0, 0.0, 1.0));
+	debug_navigation_avoidance_static_obstacle_pushout_face_color = GLOBAL_DEF("debug/shapes/avoidance/2d/obstacles_static_face_pushout_color", Color(1.0, 1.0, 0.0, 0.5));
+	debug_navigation_avoidance_static_obstacle_pushout_edge_color = GLOBAL_DEF("debug/shapes/avoidance/2d/obstacles_static_edge_pushout_color", Color(1.0, 1.0, 0.0, 1.0));
+	debug_navigation_avoidance_enable_agents_radius = GLOBAL_DEF("debug/shapes/avoidance/2d/enable_agents_radius", true);
+	debug_navigation_avoidance_enable_obstacles_radius = GLOBAL_DEF("debug/shapes/avoidance/2d/enable_obstacles_radius", true);
+	debug_navigation_avoidance_enable_obstacles_static = GLOBAL_DEF("debug/shapes/avoidance/2d/enable_obstacles_static", true);
+
+	if (Engine::get_singleton()->is_editor_hint()) {
+		// enable NavigationServer3D when in Editor or else navigation mesh edge connections are invisible
+		// on runtime tests SceneTree has "Visible Navigation" set and main iteration takes care of this.
+		set_debug_enabled(true);
+		set_debug_navigation_enabled(true);
+		set_debug_avoidance_enabled(true);
+	}
 #endif // DEBUG_ENABLED
+}
 
 NavigationServer2D::~NavigationServer2D() {
 	singleton = nullptr;
@@ -256,209 +304,239 @@ void NavigationServer2D::source_geometry_parser_set_callback(RID p_parser, const
 	parser->callback = p_callback;
 }
 
-void NavigationServer2D::_emit_map_changed(RID p_map) {
-	emit_signal(SNAME("map_changed"), p_map);
-}
-
 void NavigationServer2D::set_debug_enabled(bool p_enabled) {
-	NavigationServer3D::get_singleton()->set_debug_enabled(p_enabled);
+#ifdef DEBUG_ENABLED
+	if (debug_enabled != p_enabled) {
+		debug_dirty = true;
+	}
+
+	debug_enabled = p_enabled;
+
+	if (debug_dirty) {
+		navigation_debug_dirty = true;
+		callable_mp(this, &NavigationServer2D::_emit_navigation_debug_changed_signal).call_deferred();
+
+		avoidance_debug_dirty = true;
+		callable_mp(this, &NavigationServer2D::_emit_avoidance_debug_changed_signal).call_deferred();
+	}
+#endif // DEBUG_ENABLED
 }
 
 bool NavigationServer2D::get_debug_enabled() const {
-	return NavigationServer3D::get_singleton()->get_debug_enabled();
+	return debug_enabled;
+}
+
+#ifdef DEBUG_ENABLED
+void NavigationServer2D::_emit_navigation_debug_changed_signal() {
+	if (navigation_debug_dirty) {
+		navigation_debug_dirty = false;
+		emit_signal(SNAME("navigation_debug_changed"));
+	}
 }
 
+void NavigationServer2D::_emit_avoidance_debug_changed_signal() {
+	if (avoidance_debug_dirty) {
+		avoidance_debug_dirty = false;
+		emit_signal(SNAME("avoidance_debug_changed"));
+	}
+}
+#endif // DEBUG_ENABLED
+
 #ifdef DEBUG_ENABLED
 void NavigationServer2D::set_debug_navigation_enabled(bool p_enabled) {
-	NavigationServer3D::get_singleton()->set_debug_navigation_enabled(p_enabled);
+	debug_navigation_enabled = p_enabled;
+	navigation_debug_dirty = true;
+	callable_mp(this, &NavigationServer2D::_emit_navigation_debug_changed_signal).call_deferred();
 }
 
 bool NavigationServer2D::get_debug_navigation_enabled() const {
-	return NavigationServer3D::get_singleton()->get_debug_navigation_enabled();
+	return debug_navigation_enabled;
 }
 
 void NavigationServer2D::set_debug_avoidance_enabled(bool p_enabled) {
-	NavigationServer3D::get_singleton()->set_debug_avoidance_enabled(p_enabled);
+	debug_avoidance_enabled = p_enabled;
+	avoidance_debug_dirty = true;
+	callable_mp(this, &NavigationServer2D::_emit_avoidance_debug_changed_signal).call_deferred();
 }
 
 bool NavigationServer2D::get_debug_avoidance_enabled() const {
-	return NavigationServer3D::get_singleton()->get_debug_avoidance_enabled();
+	return debug_avoidance_enabled;
 }
 
 void NavigationServer2D::set_debug_navigation_edge_connection_color(const Color &p_color) {
-	NavigationServer3D::get_singleton()->set_debug_navigation_edge_connection_color(p_color);
+	debug_navigation_edge_connection_color = p_color;
 }
 
 Color NavigationServer2D::get_debug_navigation_edge_connection_color() const {
-	return NavigationServer3D::get_singleton()->get_debug_navigation_edge_connection_color();
+	return debug_navigation_edge_connection_color;
 }
 
 void NavigationServer2D::set_debug_navigation_geometry_face_color(const Color &p_color) {
-	NavigationServer3D::get_singleton()->set_debug_navigation_geometry_face_color(p_color);
+	debug_navigation_geometry_face_color = p_color;
 }
 
 Color NavigationServer2D::get_debug_navigation_geometry_face_color() const {
-	return NavigationServer3D::get_singleton()->get_debug_navigation_geometry_face_color();
+	return debug_navigation_geometry_face_color;
 }
 
 void NavigationServer2D::set_debug_navigation_geometry_face_disabled_color(const Color &p_color) {
-	NavigationServer3D::get_singleton()->set_debug_navigation_geometry_face_disabled_color(p_color);
+	debug_navigation_geometry_face_disabled_color = p_color;
 }
 
 Color NavigationServer2D::get_debug_navigation_geometry_face_disabled_color() const {
-	return NavigationServer3D::get_singleton()->get_debug_navigation_geometry_face_disabled_color();
+	return debug_navigation_geometry_face_disabled_color;
 }
 
 void NavigationServer2D::set_debug_navigation_link_connection_color(const Color &p_color) {
-	NavigationServer3D::get_singleton()->set_debug_navigation_link_connection_color(p_color);
+	debug_navigation_link_connection_color = p_color;
 }
 
 Color NavigationServer2D::get_debug_navigation_link_connection_color() const {
-	return NavigationServer3D::get_singleton()->get_debug_navigation_link_connection_color();
+	return debug_navigation_link_connection_color;
 }
 
 void NavigationServer2D::set_debug_navigation_link_connection_disabled_color(const Color &p_color) {
-	NavigationServer3D::get_singleton()->set_debug_navigation_link_connection_disabled_color(p_color);
+	debug_navigation_link_connection_disabled_color = p_color;
 }
 
 Color NavigationServer2D::get_debug_navigation_link_connection_disabled_color() const {
-	return NavigationServer3D::get_singleton()->get_debug_navigation_link_connection_disabled_color();
+	return debug_navigation_link_connection_disabled_color;
 }
 
 void NavigationServer2D::set_debug_navigation_geometry_edge_color(const Color &p_color) {
-	NavigationServer3D::get_singleton()->set_debug_navigation_geometry_edge_color(p_color);
+	debug_navigation_geometry_edge_color = p_color;
 }
 
 Color NavigationServer2D::get_debug_navigation_geometry_edge_color() const {
-	return NavigationServer3D::get_singleton()->get_debug_navigation_geometry_edge_color();
+	return debug_navigation_geometry_edge_color;
 }
 
 void NavigationServer2D::set_debug_navigation_geometry_edge_disabled_color(const Color &p_color) {
-	NavigationServer3D::get_singleton()->set_debug_navigation_geometry_edge_disabled_color(p_color);
+	debug_navigation_geometry_edge_disabled_color = p_color;
 }
 
 Color NavigationServer2D::get_debug_navigation_geometry_edge_disabled_color() const {
-	return NavigationServer3D::get_singleton()->get_debug_navigation_geometry_edge_disabled_color();
+	return debug_navigation_geometry_edge_disabled_color;
 }
 
 void NavigationServer2D::set_debug_navigation_enable_edge_connections(const bool p_value) {
-	NavigationServer3D::get_singleton()->set_debug_navigation_enable_edge_connections(p_value);
+	debug_navigation_enable_edge_connections = p_value;
 }
 
 bool NavigationServer2D::get_debug_navigation_enable_edge_connections() const {
-	return NavigationServer3D::get_singleton()->get_debug_navigation_enable_edge_connections();
+	return debug_navigation_enable_edge_connections;
 }
 
 void NavigationServer2D::set_debug_navigation_enable_geometry_face_random_color(const bool p_value) {
-	NavigationServer3D::get_singleton()->set_debug_navigation_enable_geometry_face_random_color(p_value);
+	debug_navigation_enable_geometry_face_random_color = p_value;
 }
 
 bool NavigationServer2D::get_debug_navigation_enable_geometry_face_random_color() const {
-	return NavigationServer3D::get_singleton()->get_debug_navigation_enable_geometry_face_random_color();
+	return debug_navigation_enable_geometry_face_random_color;
 }
 
 void NavigationServer2D::set_debug_navigation_enable_edge_lines(const bool p_value) {
-	NavigationServer3D::get_singleton()->set_debug_navigation_enable_edge_lines(p_value);
+	debug_navigation_enable_edge_lines = p_value;
 }
 
 bool NavigationServer2D::get_debug_navigation_enable_edge_lines() const {
-	return NavigationServer3D::get_singleton()->get_debug_navigation_enable_edge_lines();
+	return debug_navigation_enable_edge_lines;
 }
 
 void NavigationServer2D::set_debug_navigation_agent_path_color(const Color &p_color) {
-	NavigationServer3D::get_singleton()->set_debug_navigation_agent_path_color(p_color);
+	debug_navigation_agent_path_color = p_color;
 }
 
 Color NavigationServer2D::get_debug_navigation_agent_path_color() const {
-	return NavigationServer3D::get_singleton()->get_debug_navigation_agent_path_color();
+	return debug_navigation_agent_path_color;
 }
 
 void NavigationServer2D::set_debug_navigation_enable_agent_paths(const bool p_value) {
-	NavigationServer3D::get_singleton()->set_debug_navigation_enable_agent_paths(p_value);
+	debug_navigation_enable_agent_paths = p_value;
 }
 
 bool NavigationServer2D::get_debug_navigation_enable_agent_paths() const {
-	return NavigationServer3D::get_singleton()->get_debug_navigation_enable_agent_paths();
+	return debug_navigation_enable_agent_paths;
 }
 
 void NavigationServer2D::set_debug_navigation_agent_path_point_size(real_t p_point_size) {
-	NavigationServer3D::get_singleton()->set_debug_navigation_agent_path_point_size(p_point_size);
+	debug_navigation_agent_path_point_size = p_point_size;
 }
 
 real_t NavigationServer2D::get_debug_navigation_agent_path_point_size() const {
-	return NavigationServer3D::get_singleton()->get_debug_navigation_agent_path_point_size();
+	return debug_navigation_agent_path_point_size;
 }
 
 void NavigationServer2D::set_debug_navigation_avoidance_enable_agents_radius(const bool p_value) {
-	NavigationServer3D::get_singleton()->set_debug_navigation_avoidance_enable_agents_radius(p_value);
+	debug_navigation_avoidance_enable_agents_radius = p_value;
 }
 
 bool NavigationServer2D::get_debug_navigation_avoidance_enable_agents_radius() const {
-	return NavigationServer3D::get_singleton()->get_debug_navigation_avoidance_enable_agents_radius();
+	return debug_navigation_avoidance_enable_agents_radius;
 }
 
 void NavigationServer2D::set_debug_navigation_avoidance_enable_obstacles_radius(const bool p_value) {
-	NavigationServer3D::get_singleton()->set_debug_navigation_avoidance_enable_obstacles_radius(p_value);
+	debug_navigation_avoidance_enable_obstacles_radius = p_value;
 }
 
 bool NavigationServer2D::get_debug_navigation_avoidance_enable_obstacles_radius() const {
-	return NavigationServer3D::get_singleton()->get_debug_navigation_avoidance_enable_obstacles_radius();
+	return debug_navigation_avoidance_enable_obstacles_radius;
 }
 
 void NavigationServer2D::set_debug_navigation_avoidance_agents_radius_color(const Color &p_color) {
-	NavigationServer3D::get_singleton()->set_debug_navigation_avoidance_agents_radius_color(p_color);
+	debug_navigation_avoidance_agents_radius_color = p_color;
 }
 
 Color NavigationServer2D::get_debug_navigation_avoidance_agents_radius_color() const {
-	return NavigationServer3D::get_singleton()->get_debug_navigation_avoidance_agents_radius_color();
+	return debug_navigation_avoidance_agents_radius_color;
 }
 
 void NavigationServer2D::set_debug_navigation_avoidance_obstacles_radius_color(const Color &p_color) {
-	NavigationServer3D::get_singleton()->set_debug_navigation_avoidance_obstacles_radius_color(p_color);
+	debug_navigation_avoidance_obstacles_radius_color = p_color;
 }
 
 Color NavigationServer2D::get_debug_navigation_avoidance_obstacles_radius_color() const {
-	return NavigationServer3D::get_singleton()->get_debug_navigation_avoidance_obstacles_radius_color();
+	return debug_navigation_avoidance_obstacles_radius_color;
 }
 
 void NavigationServer2D::set_debug_navigation_avoidance_static_obstacle_pushin_face_color(const Color &p_color) {
-	NavigationServer3D::get_singleton()->set_debug_navigation_avoidance_static_obstacle_pushin_face_color(p_color);
+	debug_navigation_avoidance_static_obstacle_pushin_face_color = p_color;
 }
 
 Color NavigationServer2D::get_debug_navigation_avoidance_static_obstacle_pushin_face_color() const {
-	return NavigationServer3D::get_singleton()->get_debug_navigation_avoidance_static_obstacle_pushin_face_color();
+	return debug_navigation_avoidance_static_obstacle_pushin_face_color;
 }
 
 void NavigationServer2D::set_debug_navigation_avoidance_static_obstacle_pushout_face_color(const Color &p_color) {
-	NavigationServer3D::get_singleton()->set_debug_navigation_avoidance_static_obstacle_pushout_face_color(p_color);
+	debug_navigation_avoidance_static_obstacle_pushout_face_color = p_color;
 }
 
 Color NavigationServer2D::get_debug_navigation_avoidance_static_obstacle_pushout_face_color() const {
-	return NavigationServer3D::get_singleton()->get_debug_navigation_avoidance_static_obstacle_pushout_face_color();
+	return debug_navigation_avoidance_static_obstacle_pushout_face_color;
 }
 
 void NavigationServer2D::set_debug_navigation_avoidance_static_obstacle_pushin_edge_color(const Color &p_color) {
-	NavigationServer3D::get_singleton()->set_debug_navigation_avoidance_static_obstacle_pushin_edge_color(p_color);
+	debug_navigation_avoidance_static_obstacle_pushin_edge_color = p_color;
 }
 
 Color NavigationServer2D::get_debug_navigation_avoidance_static_obstacle_pushin_edge_color() const {
-	return NavigationServer3D::get_singleton()->get_debug_navigation_avoidance_static_obstacle_pushin_edge_color();
+	return debug_navigation_avoidance_static_obstacle_pushin_edge_color;
 }
 
 void NavigationServer2D::set_debug_navigation_avoidance_static_obstacle_pushout_edge_color(const Color &p_color) {
-	NavigationServer3D::get_singleton()->set_debug_navigation_avoidance_static_obstacle_pushout_edge_color(p_color);
+	debug_navigation_avoidance_static_obstacle_pushout_edge_color = p_color;
 }
 
 Color NavigationServer2D::get_debug_navigation_avoidance_static_obstacle_pushout_edge_color() const {
-	return NavigationServer3D::get_singleton()->get_debug_navigation_avoidance_static_obstacle_pushout_edge_color();
+	return debug_navigation_avoidance_static_obstacle_pushout_edge_color;
 }
 
 void NavigationServer2D::set_debug_navigation_avoidance_enable_obstacles_static(const bool p_value) {
-	NavigationServer3D::get_singleton()->set_debug_navigation_avoidance_enable_obstacles_static(p_value);
+	debug_navigation_avoidance_enable_obstacles_static = p_value;
 }
 
 bool NavigationServer2D::get_debug_navigation_avoidance_enable_obstacles_static() const {
-	return NavigationServer3D::get_singleton()->get_debug_navigation_avoidance_enable_obstacles_static();
+	return debug_navigation_avoidance_enable_obstacles_static;
 }
 #endif // DEBUG_ENABLED
 
@@ -481,8 +559,6 @@ NavigationServer2D *NavigationServer2DManager::new_default_server() {
 }
 
 void NavigationServer2DManager::initialize_server() {
-	// NavigationServer3D must be initialized before NavigationServer2D.
-	ERR_FAIL_NULL(NavigationServer3D::get_singleton());
 	ERR_FAIL_COND(navigation_server_2d != nullptr);
 
 	// Init 2D Navigation Server

servers/navigation_server_2d.h

@@ -53,8 +53,6 @@ class NavigationServer2D : public Object {
 
 	static NavigationServer2D *singleton;
 
-	void _emit_map_changed(RID p_map);
-
 protected:
 	static void _bind_methods();
 
@@ -303,6 +301,14 @@ public:
 	/// Returns a customized navigation path using a query parameters object
 	virtual void query_path(const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result, const Callable &p_callback = Callable()) = 0;
 
+	/// Control activation of this server.
+	virtual void set_active(bool p_active) = 0;
+
+	/// Process the collision avoidance agents.
+	/// The result of this process is needed by the physics server,
+	/// so this must be called in the main thread.
+	/// Note: This function is not thread safe.
+	virtual void process(real_t delta_time) = 0;
 	virtual void init() = 0;
 	virtual void sync() = 0;
 	virtual void finish() = 0;
@@ -329,6 +335,21 @@ public:
 	NavigationServer2D();
 	~NavigationServer2D() override;
 
+	enum ProcessInfo {
+		INFO_ACTIVE_MAPS,
+		INFO_REGION_COUNT,
+		INFO_AGENT_COUNT,
+		INFO_LINK_COUNT,
+		INFO_POLYGON_COUNT,
+		INFO_EDGE_COUNT,
+		INFO_EDGE_MERGE_COUNT,
+		INFO_EDGE_CONNECTION_COUNT,
+		INFO_EDGE_FREE_COUNT,
+		INFO_OBSTACLE_COUNT,
+	};
+
+	virtual int get_process_info(ProcessInfo p_info) const = 0;
+
 	void set_debug_enabled(bool p_enabled);
 	bool get_debug_enabled() const;
 
@@ -339,8 +360,50 @@ protected:
 	static void _bind_compatibility_methods();
 #endif
 
-public:
+private:
+	bool debug_enabled = false;
+
 #ifdef DEBUG_ENABLED
+	bool debug_dirty = true;
+
+	bool debug_navigation_enabled = false;
+	bool navigation_debug_dirty = true;
+	void _emit_navigation_debug_changed_signal();
+
+	bool debug_avoidance_enabled = false;
+	bool avoidance_debug_dirty = true;
+	void _emit_avoidance_debug_changed_signal();
+
+	Color debug_navigation_edge_connection_color = Color(1.0, 0.0, 1.0, 1.0);
+	Color debug_navigation_geometry_edge_color = Color(0.5, 1.0, 1.0, 1.0);
+	Color debug_navigation_geometry_face_color = Color(0.5, 1.0, 1.0, 0.4);
+	Color debug_navigation_geometry_edge_disabled_color = Color(0.5, 0.5, 0.5, 1.0);
+	Color debug_navigation_geometry_face_disabled_color = Color(0.5, 0.5, 0.5, 0.4);
+	Color debug_navigation_link_connection_color = Color(1.0, 0.5, 1.0, 1.0);
+	Color debug_navigation_link_connection_disabled_color = Color(0.5, 0.5, 0.5, 1.0);
+	Color debug_navigation_agent_path_color = Color(1.0, 0.0, 0.0, 1.0);
+
+	real_t debug_navigation_agent_path_point_size = 4.0;
+
+	Color debug_navigation_avoidance_agents_radius_color = Color(1.0, 1.0, 0.0, 0.25);
+	Color debug_navigation_avoidance_obstacles_radius_color = Color(1.0, 0.5, 0.0, 0.25);
+
+	Color debug_navigation_avoidance_static_obstacle_pushin_face_color = Color(1.0, 0.0, 0.0, 0.0);
+	Color debug_navigation_avoidance_static_obstacle_pushout_face_color = Color(1.0, 1.0, 0.0, 0.5);
+	Color debug_navigation_avoidance_static_obstacle_pushin_edge_color = Color(1.0, 0.0, 0.0, 1.0);
+	Color debug_navigation_avoidance_static_obstacle_pushout_edge_color = Color(1.0, 1.0, 0.0, 1.0);
+
+	bool debug_navigation_enable_edge_connections = true;
+	bool debug_navigation_enable_edge_lines = true;
+	bool debug_navigation_enable_geometry_face_random_color = true;
+	bool debug_navigation_enable_link_connections = true;
+	bool debug_navigation_enable_agent_paths = true;
+
+	bool debug_navigation_avoidance_enable_agents_radius = true;
+	bool debug_navigation_avoidance_enable_obstacles_radius = true;
+	bool debug_navigation_avoidance_enable_obstacles_static = true;
+
+public:
 	void set_debug_navigation_enabled(bool p_enabled);
 	bool get_debug_navigation_enabled() const;
 
@@ -413,11 +476,6 @@ public:
 	void set_debug_navigation_avoidance_enable_obstacles_static(const bool p_value);
 	bool get_debug_navigation_avoidance_enable_obstacles_static() const;
 #endif // DEBUG_ENABLED
-
-#ifdef DEBUG_ENABLED
-private:
-	void _emit_navigation_debug_changed_signal();
-#endif // DEBUG_ENABLED
 };
 
 typedef NavigationServer2D *(*NavigationServer2DCallback)();
@@ -433,3 +491,5 @@ public:
 	static void initialize_server();
 	static void finalize_server();
 };
+
+VARIANT_ENUM_CAST(NavigationServer2D::ProcessInfo);

servers/navigation_server_2d_dummy.h

@@ -162,10 +162,15 @@ public:
 
 	void query_path(const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result, const Callable &p_callback = Callable()) override {}
 
+	void set_active(bool p_active) override {}
+
+	void process(real_t delta_time) override {}
 	void init() override {}
 	void sync() override {}
 	void finish() override {}
 
+	int get_process_info(ProcessInfo p_info) const override { return 0; }
+
 	void free(RID p_object) override {}
 
 	void parse_source_geometry_data(const Ref<NavigationPolygon> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData2D> &p_source_geometry_data, Node *p_root_node, const Callable &p_callback = Callable()) override {}

servers/navigation_server_3d.cpp

@@ -240,11 +240,6 @@ NavigationServer3D::NavigationServer3D() {
 	ERR_FAIL_COND(singleton != nullptr);
 	singleton = this;
 
-	GLOBAL_DEF_BASIC(PropertyInfo(Variant::FLOAT, "navigation/2d/default_cell_size", PROPERTY_HINT_RANGE, NavigationDefaults2D::navmesh_cell_size_hint), NavigationDefaults2D::navmesh_cell_size);
-	GLOBAL_DEF("navigation/2d/use_edge_connections", true);
-	GLOBAL_DEF_BASIC(PropertyInfo(Variant::FLOAT, "navigation/2d/default_edge_connection_margin", PROPERTY_HINT_RANGE, "0.01,10,0.001,or_greater"), NavigationDefaults2D::edge_connection_margin);
-	GLOBAL_DEF_BASIC(PropertyInfo(Variant::FLOAT, "navigation/2d/default_link_connection_radius", PROPERTY_HINT_RANGE, "0.01,10,0.001,or_greater"), NavigationDefaults2D::link_connection_radius);
-
 	GLOBAL_DEF_BASIC(PropertyInfo(Variant::FLOAT, "navigation/3d/default_cell_size", PROPERTY_HINT_RANGE, NavigationDefaults3D::navmesh_cell_size_hint), NavigationDefaults3D::navmesh_cell_size);
 	GLOBAL_DEF_BASIC(PropertyInfo(Variant::FLOAT, "navigation/3d/default_cell_height", PROPERTY_HINT_RANGE, "0.001,100,0.001,or_greater"), NavigationDefaults3D::navmesh_cell_height);
 	GLOBAL_DEF("navigation/3d/default_up", Vector3(0, 1, 0));
@@ -253,48 +248,90 @@ NavigationServer3D::NavigationServer3D() {
 	GLOBAL_DEF_BASIC(PropertyInfo(Variant::FLOAT, "navigation/3d/default_edge_connection_margin", PROPERTY_HINT_RANGE, "0.01,10,0.001,or_greater"), NavigationDefaults3D::edge_connection_margin);
 	GLOBAL_DEF_BASIC(PropertyInfo(Variant::FLOAT, "navigation/3d/default_link_connection_radius", PROPERTY_HINT_RANGE, "0.01,10,0.001,or_greater"), NavigationDefaults3D::link_connection_radius);
 
-	GLOBAL_DEF("navigation/world/map_use_async_iterations", true);
-
-	GLOBAL_DEF("navigation/avoidance/thread_model/avoidance_use_multiple_threads", true);
-	GLOBAL_DEF("navigation/avoidance/thread_model/avoidance_use_high_priority_threads", true);
-
-	GLOBAL_DEF("navigation/pathfinding/max_threads", 4);
-
-	GLOBAL_DEF("navigation/baking/use_crash_prevention_checks", true);
-	GLOBAL_DEF("navigation/baking/thread_model/baking_use_multiple_threads", true);
-	GLOBAL_DEF("navigation/baking/thread_model/baking_use_high_priority_threads", true);
-
 #ifdef DEBUG_ENABLED
-	debug_navigation_edge_connection_color = GLOBAL_DEF("debug/shapes/navigation/edge_connection_color", Color(1.0, 0.0, 1.0, 1.0));
-	debug_navigation_geometry_edge_color = GLOBAL_DEF("debug/shapes/navigation/geometry_edge_color", Color(0.5, 1.0, 1.0, 1.0));
-	debug_navigation_geometry_face_color = GLOBAL_DEF("debug/shapes/navigation/geometry_face_color", Color(0.5, 1.0, 1.0, 0.4));
-	debug_navigation_geometry_edge_disabled_color = GLOBAL_DEF("debug/shapes/navigation/geometry_edge_disabled_color", Color(0.5, 0.5, 0.5, 1.0));
-	debug_navigation_geometry_face_disabled_color = GLOBAL_DEF("debug/shapes/navigation/geometry_face_disabled_color", Color(0.5, 0.5, 0.5, 0.4));
-	debug_navigation_link_connection_color = GLOBAL_DEF("debug/shapes/navigation/link_connection_color", Color(1.0, 0.5, 1.0, 1.0));
-	debug_navigation_link_connection_disabled_color = GLOBAL_DEF("debug/shapes/navigation/link_connection_disabled_color", Color(0.5, 0.5, 0.5, 1.0));
-	debug_navigation_agent_path_color = GLOBAL_DEF("debug/shapes/navigation/agent_path_color", Color(1.0, 0.0, 0.0, 1.0));
-
-	debug_navigation_enable_edge_connections = GLOBAL_DEF("debug/shapes/navigation/enable_edge_connections", true);
-	debug_navigation_enable_edge_connections_xray = GLOBAL_DEF("debug/shapes/navigation/enable_edge_connections_xray", true);
-	debug_navigation_enable_edge_lines = GLOBAL_DEF("debug/shapes/navigation/enable_edge_lines", true);
-	debug_navigation_enable_edge_lines_xray = GLOBAL_DEF("debug/shapes/navigation/enable_edge_lines_xray", true);
-	debug_navigation_enable_geometry_face_random_color = GLOBAL_DEF("debug/shapes/navigation/enable_geometry_face_random_color", true);
-	debug_navigation_enable_link_connections = GLOBAL_DEF("debug/shapes/navigation/enable_link_connections", true);
-	debug_navigation_enable_link_connections_xray = GLOBAL_DEF("debug/shapes/navigation/enable_link_connections_xray", true);
-
-	debug_navigation_enable_agent_paths = GLOBAL_DEF("debug/shapes/navigation/enable_agent_paths", true);
-	debug_navigation_enable_agent_paths_xray = GLOBAL_DEF("debug/shapes/navigation/enable_agent_paths_xray", true);
-	debug_navigation_agent_path_point_size = GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "debug/shapes/navigation/agent_path_point_size", PROPERTY_HINT_RANGE, "0.01,10,0.001,or_greater"), 4.0);
-
-	debug_navigation_avoidance_agents_radius_color = GLOBAL_DEF("debug/shapes/avoidance/agents_radius_color", Color(1.0, 1.0, 0.0, 0.25));
-	debug_navigation_avoidance_obstacles_radius_color = GLOBAL_DEF("debug/shapes/avoidance/obstacles_radius_color", Color(1.0, 0.5, 0.0, 0.25));
-	debug_navigation_avoidance_static_obstacle_pushin_face_color = GLOBAL_DEF("debug/shapes/avoidance/obstacles_static_face_pushin_color", Color(1.0, 0.0, 0.0, 0.0));
-	debug_navigation_avoidance_static_obstacle_pushin_edge_color = GLOBAL_DEF("debug/shapes/avoidance/obstacles_static_edge_pushin_color", Color(1.0, 0.0, 0.0, 1.0));
-	debug_navigation_avoidance_static_obstacle_pushout_face_color = GLOBAL_DEF("debug/shapes/avoidance/obstacles_static_face_pushout_color", Color(1.0, 1.0, 0.0, 0.5));
-	debug_navigation_avoidance_static_obstacle_pushout_edge_color = GLOBAL_DEF("debug/shapes/avoidance/obstacles_static_edge_pushout_color", Color(1.0, 1.0, 0.0, 1.0));
-	debug_navigation_avoidance_enable_agents_radius = GLOBAL_DEF("debug/shapes/avoidance/enable_agents_radius", true);
-	debug_navigation_avoidance_enable_obstacles_radius = GLOBAL_DEF("debug/shapes/avoidance/enable_obstacles_radius", true);
-	debug_navigation_avoidance_enable_obstacles_static = GLOBAL_DEF("debug/shapes/avoidance/enable_obstacles_static", true);
+#ifndef DISABLE_DEPRECATED
+#define MOVE_PROJECT_SETTING_1(m_old_setting, m_new_setting)                                                                             \
+	if (!ProjectSettings::get_singleton()->has_setting(m_new_setting) && ProjectSettings::get_singleton()->has_setting(m_old_setting)) { \
+		Variant value = GLOBAL_GET(m_old_setting);                                                                                       \
+		ProjectSettings::get_singleton()->set_setting(m_new_setting, value);                                                             \
+		ProjectSettings::get_singleton()->clear(m_old_setting);                                                                          \
+	}
+#define MOVE_PROJECT_SETTING_2(m_old_setting, m_new_setting_1, m_new_setting_2)                                                                 \
+	if ((!ProjectSettings::get_singleton()->has_setting(m_new_setting_1) || !ProjectSettings::get_singleton()->has_setting(m_new_setting_2)) && \
+			ProjectSettings::get_singleton()->has_setting(m_old_setting)) {                                                                     \
+		Variant value = GLOBAL_GET(m_old_setting);                                                                                              \
+		if (!ProjectSettings::get_singleton()->has_setting(m_new_setting_1)) {                                                                  \
+			ProjectSettings::get_singleton()->set_setting(m_new_setting_1, value);                                                              \
+		}                                                                                                                                       \
+		if (!ProjectSettings::get_singleton()->has_setting(m_new_setting_2)) {                                                                  \
+			ProjectSettings::get_singleton()->set_setting(m_new_setting_2, value);                                                              \
+		}                                                                                                                                       \
+		ProjectSettings::get_singleton()->clear(m_old_setting);                                                                                 \
+	}
+	MOVE_PROJECT_SETTING_2("debug/shapes/navigation/edge_connection_color", "debug/shapes/navigation/2d/edge_connection_color", "debug/shapes/navigation/3d/edge_connection_color");
+	MOVE_PROJECT_SETTING_2("debug/shapes/navigation/geometry_edge_color", "debug/shapes/navigation/2d/geometry_edge_color", "debug/shapes/navigation/3d/geometry_edge_color");
+	MOVE_PROJECT_SETTING_2("debug/shapes/navigation/geometry_face_color", "debug/shapes/navigation/2d/geometry_face_color", "debug/shapes/navigation/3d/geometry_face_color");
+	MOVE_PROJECT_SETTING_2("debug/shapes/navigation/geometry_edge_disabled_color", "debug/shapes/navigation/2d/geometry_edge_disabled_color", "debug/shapes/navigation/3d/geometry_edge_disabled_color");
+	MOVE_PROJECT_SETTING_2("debug/shapes/navigation/geometry_face_disabled_color", "debug/shapes/navigation/2d/geometry_face_disabled_color", "debug/shapes/navigation/3d/geometry_face_disabled_color");
+	MOVE_PROJECT_SETTING_2("debug/shapes/navigation/link_connection_color", "debug/shapes/navigation/2d/link_connection_color", "debug/shapes/navigation/3d/link_connection_color");
+	MOVE_PROJECT_SETTING_2("debug/shapes/navigation/link_connection_disabled_color", "debug/shapes/navigation/2d/link_connection_disabled_color", "debug/shapes/navigation/3d/link_connection_disabled_color");
+	MOVE_PROJECT_SETTING_2("debug/shapes/navigation/agent_path_color", "debug/shapes/navigation/2d/agent_path_color", "debug/shapes/navigation/3d/agent_path_color");
+
+	MOVE_PROJECT_SETTING_2("debug/shapes/navigation/enable_edge_connections", "debug/shapes/navigation/2d/enable_edge_connections", "debug/shapes/navigation/3d/enable_edge_connections");
+	MOVE_PROJECT_SETTING_1("debug/shapes/navigation/enable_edge_connections_xray", "debug/shapes/navigation/3d/enable_edge_connections_xray");
+	MOVE_PROJECT_SETTING_2("debug/shapes/navigation/enable_edge_lines", "debug/shapes/navigation/2d/enable_edge_lines", "debug/shapes/navigation/3d/enable_edge_lines");
+	MOVE_PROJECT_SETTING_1("debug/shapes/navigation/enable_edge_lines_xray", "debug/shapes/navigation/3d/enable_edge_lines_xray");
+	MOVE_PROJECT_SETTING_2("debug/shapes/navigation/enable_geometry_face_random_color", "debug/shapes/navigation/2d/enable_geometry_face_random_color", "debug/shapes/navigation/3d/enable_geometry_face_random_color");
+	MOVE_PROJECT_SETTING_2("debug/shapes/navigation/enable_link_connections", "debug/shapes/navigation/2d/enable_link_connections", "debug/shapes/navigation/3d/enable_link_connections");
+	MOVE_PROJECT_SETTING_1("debug/shapes/navigation/enable_link_connections_xray", "debug/shapes/navigation/3d/enable_link_connections_xray");
+
+	MOVE_PROJECT_SETTING_2("debug/shapes/navigation/enable_agent_paths", "debug/shapes/navigation/2d/enable_agent_paths", "debug/shapes/navigation/3d/enable_agent_paths");
+	MOVE_PROJECT_SETTING_1("debug/shapes/navigation/enable_agent_paths_xray", "debug/shapes/navigation/3d/enable_agent_paths_xray");
+	MOVE_PROJECT_SETTING_2("debug/shapes/navigation/agent_path_point_size", "debug/shapes/navigation/2d/agent_path_point_size", "debug/shapes/navigation/3d/agent_path_point_size");
+
+	MOVE_PROJECT_SETTING_2("debug/shapes/avoidance/agents_radius_color", "debug/shapes/avoidance/2d/agents_radius_color", "debug/shapes/avoidance/3d/agents_radius_color");
+	MOVE_PROJECT_SETTING_2("debug/shapes/avoidance/obstacles_radius_color", "debug/shapes/avoidance/2d/obstacles_radius_color", "debug/shapes/avoidance/3d/obstacles_radius_color");
+	MOVE_PROJECT_SETTING_2("debug/shapes/avoidance/obstacles_static_face_pushin_color", "debug/shapes/avoidance/2d/obstacles_static_face_pushin_color", "debug/shapes/avoidance/3d/obstacles_static_face_pushin_color");
+	MOVE_PROJECT_SETTING_2("debug/shapes/avoidance/obstacles_static_edge_pushin_color", "debug/shapes/avoidance/2d/obstacles_static_edge_pushin_color", "debug/shapes/avoidance/3d/obstacles_static_edge_pushin_color");
+	MOVE_PROJECT_SETTING_2("debug/shapes/avoidance/obstacles_static_face_pushout_color", "debug/shapes/avoidance/2d/obstacles_static_face_pushout_color", "debug/shapes/avoidance/3d/obstacles_static_face_pushout_color");
+	MOVE_PROJECT_SETTING_2("debug/shapes/avoidance/obstacles_static_edge_pushout_color", "debug/shapes/avoidance/2d/obstacles_static_edge_pushout_color", "debug/shapes/avoidance/3d/obstacles_static_edge_pushout_color");
+	MOVE_PROJECT_SETTING_2("debug/shapes/avoidance/enable_agents_radius", "debug/shapes/avoidance/2d/enable_agents_radius", "debug/shapes/avoidance/3d/enable_agents_radius");
+	MOVE_PROJECT_SETTING_2("debug/shapes/avoidance/enable_obstacles_radius", "debug/shapes/avoidance/2d/enable_obstacles_radius", "debug/shapes/avoidance/2d/enable_obstacles_static");
+	MOVE_PROJECT_SETTING_2("debug/shapes/avoidance/enable_obstacles_radius", "debug/shapes/avoidance/3d/enable_obstacles_radius", "debug/shapes/avoidance/3d/enable_obstacles_static");
+#undef MOVE_PROJECT_SETTING_1
+#undef MOVE_PROJECT_SETTING_2
+#endif // DISABLE_DEPRECATED
+
+	debug_navigation_edge_connection_color = GLOBAL_DEF("debug/shapes/navigation/3d/edge_connection_color", Color(1.0, 0.0, 1.0, 1.0));
+	debug_navigation_geometry_edge_color = GLOBAL_DEF("debug/shapes/navigation/3d/geometry_edge_color", Color(0.5, 1.0, 1.0, 1.0));
+	debug_navigation_geometry_face_color = GLOBAL_DEF("debug/shapes/navigation/3d/geometry_face_color", Color(0.5, 1.0, 1.0, 0.4));
+	debug_navigation_geometry_edge_disabled_color = GLOBAL_DEF("debug/shapes/navigation/3d/geometry_edge_disabled_color", Color(0.5, 0.5, 0.5, 1.0));
+	debug_navigation_geometry_face_disabled_color = GLOBAL_DEF("debug/shapes/navigation/3d/geometry_face_disabled_color", Color(0.5, 0.5, 0.5, 0.4));
+	debug_navigation_link_connection_color = GLOBAL_DEF("debug/shapes/navigation/3d/link_connection_color", Color(1.0, 0.5, 1.0, 1.0));
+	debug_navigation_link_connection_disabled_color = GLOBAL_DEF("debug/shapes/navigation/3d/link_connection_disabled_color", Color(0.5, 0.5, 0.5, 1.0));
+	debug_navigation_agent_path_color = GLOBAL_DEF("debug/shapes/navigation/3d/agent_path_color", Color(1.0, 0.0, 0.0, 1.0));
+
+	debug_navigation_enable_edge_connections = GLOBAL_DEF("debug/shapes/navigation/3d/enable_edge_connections", true);
+	debug_navigation_enable_edge_connections_xray = GLOBAL_DEF("debug/shapes/navigation/3d/enable_edge_connections_xray", true);
+	debug_navigation_enable_edge_lines = GLOBAL_DEF("debug/shapes/navigation/3d/enable_edge_lines", true);
+	debug_navigation_enable_edge_lines_xray = GLOBAL_DEF("debug/shapes/navigation/3d/enable_edge_lines_xray", true);
+	debug_navigation_enable_geometry_face_random_color = GLOBAL_DEF("debug/shapes/navigation/3d/enable_geometry_face_random_color", true);
+	debug_navigation_enable_link_connections = GLOBAL_DEF("debug/shapes/navigation/3d/enable_link_connections", true);
+	debug_navigation_enable_link_connections_xray = GLOBAL_DEF("debug/shapes/navigation/3d/enable_link_connections_xray", true);
+
+	debug_navigation_enable_agent_paths = GLOBAL_DEF("debug/shapes/navigation/3d/enable_agent_paths", true);
+	debug_navigation_enable_agent_paths_xray = GLOBAL_DEF("debug/shapes/navigation/3d/enable_agent_paths_xray", true);
+	debug_navigation_agent_path_point_size = GLOBAL_DEF(PropertyInfo(Variant::FLOAT, "debug/shapes/navigation/3d/agent_path_point_size", PROPERTY_HINT_RANGE, "0.01,10,0.001,or_greater"), 4.0);
+
+	debug_navigation_avoidance_agents_radius_color = GLOBAL_DEF("debug/shapes/avoidance/3d/agents_radius_color", Color(1.0, 1.0, 0.0, 0.25));
+	debug_navigation_avoidance_obstacles_radius_color = GLOBAL_DEF("debug/shapes/avoidance/3d/obstacles_radius_color", Color(1.0, 0.5, 0.0, 0.25));
+	debug_navigation_avoidance_static_obstacle_pushin_face_color = GLOBAL_DEF("debug/shapes/avoidance/3d/obstacles_static_face_pushin_color", Color(1.0, 0.0, 0.0, 0.0));
+	debug_navigation_avoidance_static_obstacle_pushin_edge_color = GLOBAL_DEF("debug/shapes/avoidance/3d/obstacles_static_edge_pushin_color", Color(1.0, 0.0, 0.0, 1.0));
+	debug_navigation_avoidance_static_obstacle_pushout_face_color = GLOBAL_DEF("debug/shapes/avoidance/3d/obstacles_static_face_pushout_color", Color(1.0, 1.0, 0.0, 0.5));
+	debug_navigation_avoidance_static_obstacle_pushout_edge_color = GLOBAL_DEF("debug/shapes/avoidance/3d/obstacles_static_edge_pushout_color", Color(1.0, 1.0, 0.0, 1.0));
+	debug_navigation_avoidance_enable_agents_radius = GLOBAL_DEF("debug/shapes/avoidance/3d/enable_agents_radius", true);
+	debug_navigation_avoidance_enable_obstacles_radius = GLOBAL_DEF("debug/shapes/avoidance/3d/enable_obstacles_radius", true);
+	debug_navigation_avoidance_enable_obstacles_static = GLOBAL_DEF("debug/shapes/avoidance/3d/enable_obstacles_static", true);
 
 	if (Engine::get_singleton()->is_editor_hint()) {
 		// enable NavigationServer3D when in Editor or else navigation mesh edge connections are invisible

tests/scene/test_navigation_agent_2d.h

@@ -39,7 +39,7 @@
 
 namespace TestNavigationAgent2D {
 
-TEST_SUITE("[Navigation]") {
+TEST_SUITE("[Navigation2D]") {
 	TEST_CASE("[SceneTree][NavigationAgent2D] New agent should have valid RID") {
 		NavigationAgent2D *agent_node = memnew(NavigationAgent2D);
 		CHECK(agent_node->get_rid().is_valid());

tests/scene/test_navigation_agent_3d.h

@@ -38,7 +38,7 @@
 
 namespace TestNavigationAgent3D {
 
-TEST_SUITE("[Navigation]") {
+TEST_SUITE("[Navigation3D]") {
 	TEST_CASE("[SceneTree][NavigationAgent3D] New agent should have valid RID") {
 		NavigationAgent3D *agent_node = memnew(NavigationAgent3D);
 		CHECK(agent_node->get_rid().is_valid());

tests/scene/test_navigation_obstacle_2d.h

@@ -37,7 +37,7 @@
 
 namespace TestNavigationObstacle2D {
 
-TEST_SUITE("[Navigation]") {
+TEST_SUITE("[Navigation2D]") {
 	TEST_CASE("[SceneTree][NavigationObstacle2D] New obstacle should have valid RID") {
 		NavigationObstacle2D *obstacle_node = memnew(NavigationObstacle2D);
 		CHECK(obstacle_node->get_rid().is_valid());

tests/scene/test_navigation_obstacle_3d.h

@@ -37,7 +37,7 @@
 
 namespace TestNavigationObstacle3D {
 
-TEST_SUITE("[Navigation]") {
+TEST_SUITE("[Navigation3D]") {
 	TEST_CASE("[SceneTree][NavigationObstacle3D] New obstacle should have valid RID") {
 		NavigationObstacle3D *obstacle_node = memnew(NavigationObstacle3D);
 		CHECK(obstacle_node->get_rid().is_valid());

tests/scene/test_navigation_region_2d.h

@@ -37,7 +37,7 @@
 
 namespace TestNavigationRegion2D {
 
-TEST_SUITE("[Navigation]") {
+TEST_SUITE("[Navigation2D]") {
 	TEST_CASE("[SceneTree][NavigationRegion2D] New region should have valid RID") {
 		NavigationRegion2D *region_node = memnew(NavigationRegion2D);
 		CHECK(region_node->get_rid().is_valid());

tests/scene/test_navigation_region_3d.h

@@ -39,7 +39,7 @@
 
 namespace TestNavigationRegion3D {
 
-TEST_SUITE("[Navigation]") {
+TEST_SUITE("[Navigation3D]") {
 	TEST_CASE("[SceneTree][NavigationRegion3D] New region should have valid RID") {
 		NavigationRegion3D *region_node = memnew(NavigationRegion3D);
 		CHECK(region_node->get_rid().is_valid());

tests/servers/test_navigation_server_2d.h

@@ -30,15 +30,738 @@
 
 #pragma once
 
+#include "modules/navigation_2d/nav_utils_2d.h"
 #include "servers/navigation_server_2d.h"
 
+#include "scene/2d/polygon_2d.h"
+
 #include "tests/test_macros.h"
 
 namespace TestNavigationServer2D {
-TEST_SUITE("[Navigation]") {
+
+// TODO: Find a more generic way to create `Callable` mocks.
+class CallableMock : public Object {
+	GDCLASS(CallableMock, Object);
+
+public:
+	void function1(Variant arg0) {
+		function1_calls++;
+		function1_latest_arg0 = arg0;
+	}
+
+	unsigned function1_calls{ 0 };
+	Variant function1_latest_arg0;
+};
+
+static inline Array build_array() {
+	return Array();
+}
+template <typename... Targs>
+static inline Array build_array(Variant item, Targs... Fargs) {
+	Array a = build_array(Fargs...);
+	a.push_front(item);
+	return a;
+}
+
+struct GreaterThan {
+	bool operator()(int p_a, int p_b) const { return p_a > p_b; }
+};
+
+struct CompareArrayValues {
+	const int *array;
+
+	CompareArrayValues(const int *p_array) :
+			array(p_array) {}
+
+	bool operator()(uint32_t p_index_a, uint32_t p_index_b) const {
+		return array[p_index_a] < array[p_index_b];
+	}
+};
+
+struct RegisterHeapIndexes {
+	uint32_t *indexes;
+
+	RegisterHeapIndexes(uint32_t *p_indexes) :
+			indexes(p_indexes) {}
+
+	void operator()(uint32_t p_vector_index, uint32_t p_heap_index) {
+		indexes[p_vector_index] = p_heap_index;
+	}
+};
+
+TEST_SUITE("[Navigation2D]") {
 	TEST_CASE("[NavigationServer2D] Server should be empty when initialized") {
 		NavigationServer2D *navigation_server = NavigationServer2D::get_singleton();
 		CHECK_EQ(navigation_server->get_maps().size(), 0);
+
+		SUBCASE("'ProcessInfo' should report all counters empty as well") {
+			CHECK_EQ(navigation_server->get_process_info(NavigationServer2D::INFO_ACTIVE_MAPS), 0);
+			CHECK_EQ(navigation_server->get_process_info(NavigationServer2D::INFO_REGION_COUNT), 0);
+			CHECK_EQ(navigation_server->get_process_info(NavigationServer2D::INFO_AGENT_COUNT), 0);
+			CHECK_EQ(navigation_server->get_process_info(NavigationServer2D::INFO_LINK_COUNT), 0);
+			CHECK_EQ(navigation_server->get_process_info(NavigationServer2D::INFO_POLYGON_COUNT), 0);
+			CHECK_EQ(navigation_server->get_process_info(NavigationServer2D::INFO_EDGE_COUNT), 0);
+			CHECK_EQ(navigation_server->get_process_info(NavigationServer2D::INFO_EDGE_MERGE_COUNT), 0);
+			CHECK_EQ(navigation_server->get_process_info(NavigationServer2D::INFO_EDGE_CONNECTION_COUNT), 0);
+			CHECK_EQ(navigation_server->get_process_info(NavigationServer2D::INFO_EDGE_FREE_COUNT), 0);
+		}
+	}
+
+	TEST_CASE("[NavigationServer2D] Server should manage agent properly") {
+		NavigationServer2D *navigation_server = NavigationServer2D::get_singleton();
+
+		RID agent = navigation_server->agent_create();
+		CHECK(agent.is_valid());
+
+		SUBCASE("'ProcessInfo' should not report dangling agent") {
+			CHECK_EQ(navigation_server->get_process_info(NavigationServer2D::INFO_AGENT_COUNT), 0);
+		}
+
+		SUBCASE("Setters/getters should work") {
+			bool initial_avoidance_enabled = navigation_server->agent_get_avoidance_enabled(agent);
+			navigation_server->agent_set_avoidance_enabled(agent, !initial_avoidance_enabled);
+			navigation_server->process(0.0); // Give server some cycles to commit.
+
+			CHECK_EQ(navigation_server->agent_get_avoidance_enabled(agent), !initial_avoidance_enabled);
+			// TODO: Add remaining setters/getters once the missing getters are added.
+		}
+
+		SUBCASE("'ProcessInfo' should report agent with active map") {
+			RID map = navigation_server->map_create();
+			CHECK(map.is_valid());
+			navigation_server->map_set_active(map, true);
+			navigation_server->agent_set_map(agent, map);
+			navigation_server->process(0.0); // Give server some cycles to commit.
+			CHECK_EQ(navigation_server->get_process_info(NavigationServer2D::INFO_AGENT_COUNT), 1);
+			navigation_server->agent_set_map(agent, RID());
+			navigation_server->free(map);
+			navigation_server->process(0.0); // Give server some cycles to commit.
+			CHECK_EQ(navigation_server->get_process_info(NavigationServer2D::INFO_AGENT_COUNT), 0);
+		}
+
+		navigation_server->free(agent);
+	}
+
+	TEST_CASE("[NavigationServer2D] Server should manage map properly") {
+		NavigationServer2D *navigation_server = NavigationServer2D::get_singleton();
+
+		RID map;
+		CHECK_FALSE(map.is_valid());
+
+		SUBCASE("Queries against invalid map should return empty or invalid values") {
+			ERR_PRINT_OFF;
+			CHECK_EQ(navigation_server->map_get_closest_point(map, Vector2(7, 7)), Vector2());
+			CHECK_FALSE(navigation_server->map_get_closest_point_owner(map, Vector2(7, 7)).is_valid());
+			CHECK_EQ(navigation_server->map_get_path(map, Vector2(7, 7), Vector2(8, 8), true).size(), 0);
+			CHECK_EQ(navigation_server->map_get_path(map, Vector2(7, 7), Vector2(8, 8), false).size(), 0);
+
+			Ref<NavigationPathQueryParameters2D> query_parameters;
+			query_parameters.instantiate();
+			query_parameters->set_map(map);
+			query_parameters->set_start_position(Vector2(7, 7));
+			query_parameters->set_target_position(Vector2(8, 8));
+			Ref<NavigationPathQueryResult2D> query_result;
+			query_result.instantiate();
+			navigation_server->query_path(query_parameters, query_result);
+			CHECK_EQ(query_result->get_path().size(), 0);
+			CHECK_EQ(query_result->get_path_types().size(), 0);
+			CHECK_EQ(query_result->get_path_rids().size(), 0);
+			CHECK_EQ(query_result->get_path_owner_ids().size(), 0);
+			ERR_PRINT_ON;
+		}
+
+		map = navigation_server->map_create();
+		CHECK(map.is_valid());
+		CHECK_EQ(navigation_server->get_maps().size(), 1);
+
+		SUBCASE("'ProcessInfo' should not report inactive map") {
+			CHECK_EQ(navigation_server->get_process_info(NavigationServer2D::INFO_ACTIVE_MAPS), 0);
+		}
+
+		SUBCASE("Setters/getters should work") {
+			navigation_server->map_set_cell_size(map, 0.55);
+			navigation_server->map_set_edge_connection_margin(map, 0.66);
+			navigation_server->map_set_link_connection_radius(map, 0.77);
+			bool initial_use_edge_connections = navigation_server->map_get_use_edge_connections(map);
+			navigation_server->map_set_use_edge_connections(map, !initial_use_edge_connections);
+			navigation_server->process(0.0); // Give server some cycles to commit.
+
+			CHECK_EQ(navigation_server->map_get_cell_size(map), doctest::Approx(0.55));
+			CHECK_EQ(navigation_server->map_get_edge_connection_margin(map), doctest::Approx(0.66));
+			CHECK_EQ(navigation_server->map_get_link_connection_radius(map), doctest::Approx(0.77));
+			CHECK_EQ(navigation_server->map_get_use_edge_connections(map), !initial_use_edge_connections);
+		}
+
+		SUBCASE("'ProcessInfo' should report map iff active") {
+			navigation_server->map_set_active(map, true);
+			navigation_server->process(0.0); // Give server some cycles to commit.
+			CHECK(navigation_server->map_is_active(map));
+			CHECK_EQ(navigation_server->get_process_info(NavigationServer2D::INFO_ACTIVE_MAPS), 1);
+			navigation_server->map_set_active(map, false);
+			navigation_server->process(0.0); // Give server some cycles to commit.
+			CHECK_EQ(navigation_server->get_process_info(NavigationServer2D::INFO_ACTIVE_MAPS), 0);
+		}
+
+		SUBCASE("Number of agents should be reported properly") {
+			RID agent = navigation_server->agent_create();
+			CHECK(agent.is_valid());
+			navigation_server->agent_set_map(agent, map);
+			navigation_server->process(0.0); // Give server some cycles to commit.
+			CHECK_EQ(navigation_server->map_get_agents(map).size(), 1);
+			navigation_server->free(agent);
+			navigation_server->process(0.0); // Give server some cycles to commit.
+			CHECK_EQ(navigation_server->map_get_agents(map).size(), 0);
+		}
+
+		SUBCASE("Number of links should be reported properly") {
+			RID link = navigation_server->link_create();
+			CHECK(link.is_valid());
+			navigation_server->link_set_map(link, map);
+			navigation_server->process(0.0); // Give server some cycles to commit.
+			CHECK_EQ(navigation_server->map_get_links(map).size(), 1);
+			navigation_server->free(link);
+			navigation_server->process(0.0); // Give server some cycles to commit.
+			CHECK_EQ(navigation_server->map_get_links(map).size(), 0);
+		}
+
+		SUBCASE("Number of obstacles should be reported properly") {
+			RID obstacle = navigation_server->obstacle_create();
+			CHECK(obstacle.is_valid());
+			navigation_server->obstacle_set_map(obstacle, map);
+			navigation_server->process(0.0); // Give server some cycles to commit.
+			CHECK_EQ(navigation_server->map_get_obstacles(map).size(), 1);
+			navigation_server->free(obstacle);
+			navigation_server->process(0.0); // Give server some cycles to commit.
+			CHECK_EQ(navigation_server->map_get_obstacles(map).size(), 0);
+		}
+
+		SUBCASE("Number of regions should be reported properly") {
+			RID region = navigation_server->region_create();
+			CHECK(region.is_valid());
+			navigation_server->region_set_map(region, map);
+			navigation_server->process(0.0); // Give server some cycles to commit.
+			CHECK_EQ(navigation_server->map_get_regions(map).size(), 1);
+			navigation_server->free(region);
+			navigation_server->process(0.0); // Give server some cycles to commit.
+			CHECK_EQ(navigation_server->map_get_regions(map).size(), 0);
+		}
+
+		SUBCASE("Queries against empty map should return empty or invalid values") {
+			navigation_server->map_set_active(map, true);
+			navigation_server->process(0.0); // Give server some cycles to commit.
+
+			ERR_PRINT_OFF;
+			CHECK_EQ(navigation_server->map_get_closest_point(map, Vector2(7, 7)), Vector2());
+			CHECK_FALSE(navigation_server->map_get_closest_point_owner(map, Vector2(7, 7)).is_valid());
+			CHECK_EQ(navigation_server->map_get_path(map, Vector2(7, 7), Vector2(8, 8), true).size(), 0);
+			CHECK_EQ(navigation_server->map_get_path(map, Vector2(7, 7), Vector2(8, 8), false).size(), 0);
+
+			Ref<NavigationPathQueryParameters2D> query_parameters;
+			query_parameters.instantiate();
+			query_parameters->set_map(map);
+			query_parameters->set_start_position(Vector2(7, 7));
+			query_parameters->set_target_position(Vector2(8, 8));
+			Ref<NavigationPathQueryResult2D> query_result;
+			query_result.instantiate();
+			navigation_server->query_path(query_parameters, query_result);
+			CHECK_EQ(query_result->get_path().size(), 0);
+			CHECK_EQ(query_result->get_path_types().size(), 0);
+			CHECK_EQ(query_result->get_path_rids().size(), 0);
+			CHECK_EQ(query_result->get_path_owner_ids().size(), 0);
+			ERR_PRINT_ON;
+
+			navigation_server->map_set_active(map, false);
+			navigation_server->process(0.0); // Give server some cycles to commit.
+		}
+
+		navigation_server->free(map);
+		navigation_server->process(0.0); // Give server some cycles to actually remove map.
+		CHECK_EQ(navigation_server->get_maps().size(), 0);
+	}
+
+	TEST_CASE("[NavigationServer2D] Server should manage link properly") {
+		NavigationServer2D *navigation_server = NavigationServer2D::get_singleton();
+
+		RID link = navigation_server->link_create();
+		CHECK(link.is_valid());
+
+		SUBCASE("'ProcessInfo' should not report dangling link") {
+			CHECK_EQ(navigation_server->get_process_info(NavigationServer2D::INFO_LINK_COUNT), 0);
+		}
+
+		SUBCASE("Setters/getters should work") {
+			bool initial_bidirectional = navigation_server->link_is_bidirectional(link);
+			navigation_server->link_set_bidirectional(link, !initial_bidirectional);
+			navigation_server->link_set_end_position(link, Vector2(7, 7));
+			navigation_server->link_set_enter_cost(link, 0.55);
+			navigation_server->link_set_navigation_layers(link, 6);
+			navigation_server->link_set_owner_id(link, ObjectID((int64_t)7));
+			navigation_server->link_set_start_position(link, Vector2(8, 8));
+			navigation_server->link_set_travel_cost(link, 0.66);
+			navigation_server->process(0.0); // Give server some cycles to commit.
+
+			CHECK_EQ(navigation_server->link_is_bidirectional(link), !initial_bidirectional);
+			CHECK_EQ(navigation_server->link_get_end_position(link), Vector2(7, 7));
+			CHECK_EQ(navigation_server->link_get_enter_cost(link), doctest::Approx(0.55));
+			CHECK_EQ(navigation_server->link_get_navigation_layers(link), 6);
+			CHECK_EQ(navigation_server->link_get_owner_id(link), ObjectID((int64_t)7));
+			CHECK_EQ(navigation_server->link_get_start_position(link), Vector2(8, 8));
+			CHECK_EQ(navigation_server->link_get_travel_cost(link), doctest::Approx(0.66));
+		}
+
+		SUBCASE("'ProcessInfo' should report link with active map") {
+			RID map = navigation_server->map_create();
+			CHECK(map.is_valid());
+			navigation_server->map_set_active(map, true);
+			navigation_server->link_set_map(link, map);
+			navigation_server->process(0.0); // Give server some cycles to commit.
+			CHECK_EQ(navigation_server->get_process_info(NavigationServer2D::INFO_LINK_COUNT), 1);
+			navigation_server->link_set_map(link, RID());
+			navigation_server->free(map);
+			navigation_server->process(0.0); // Give server some cycles to commit.
+			CHECK_EQ(navigation_server->get_process_info(NavigationServer2D::INFO_LINK_COUNT), 0);
+		}
+
+		navigation_server->free(link);
+	}
+
+	TEST_CASE("[NavigationServer2D] Server should manage obstacles properly") {
+		NavigationServer2D *navigation_server = NavigationServer2D::get_singleton();
+
+		RID obstacle = navigation_server->obstacle_create();
+		CHECK(obstacle.is_valid());
+
+		// TODO: Add tests for setters/getters once getters are added.
+
+		navigation_server->free(obstacle);
+	}
+
+	TEST_CASE("[NavigationServer2D] Server should manage regions properly") {
+		NavigationServer2D *navigation_server = NavigationServer2D::get_singleton();
+
+		RID region = navigation_server->region_create();
+		CHECK(region.is_valid());
+
+		SUBCASE("'ProcessInfo' should not report dangling region") {
+			CHECK_EQ(navigation_server->get_process_info(NavigationServer2D::INFO_REGION_COUNT), 0);
+		}
+
+		SUBCASE("Setters/getters should work") {
+			bool initial_use_edge_connections = navigation_server->region_get_use_edge_connections(region);
+			navigation_server->region_set_enter_cost(region, 0.55);
+			navigation_server->region_set_navigation_layers(region, 5);
+			navigation_server->region_set_owner_id(region, ObjectID((int64_t)7));
+			navigation_server->region_set_travel_cost(region, 0.66);
+			navigation_server->region_set_use_edge_connections(region, !initial_use_edge_connections);
+			navigation_server->process(0.0); // Give server some cycles to commit.
+
+			CHECK_EQ(navigation_server->region_get_enter_cost(region), doctest::Approx(0.55));
+			CHECK_EQ(navigation_server->region_get_navigation_layers(region), 5);
+			CHECK_EQ(navigation_server->region_get_owner_id(region), ObjectID((int64_t)7));
+			CHECK_EQ(navigation_server->region_get_travel_cost(region), doctest::Approx(0.66));
+			CHECK_EQ(navigation_server->region_get_use_edge_connections(region), !initial_use_edge_connections);
+		}
+
+		SUBCASE("'ProcessInfo' should report region with active map") {
+			RID map = navigation_server->map_create();
+			CHECK(map.is_valid());
+			navigation_server->map_set_active(map, true);
+			navigation_server->region_set_map(region, map);
+			navigation_server->process(0.0); // Give server some cycles to commit.
+			CHECK_EQ(navigation_server->get_process_info(NavigationServer2D::INFO_REGION_COUNT), 1);
+			navigation_server->region_set_map(region, RID());
+			navigation_server->free(map);
+			navigation_server->process(0.0); // Give server some cycles to commit.
+			CHECK_EQ(navigation_server->get_process_info(NavigationServer2D::INFO_REGION_COUNT), 0);
+		}
+
+		SUBCASE("Queries against empty region should return empty or invalid values") {
+			ERR_PRINT_OFF;
+			CHECK_EQ(navigation_server->region_get_connections_count(region), 0);
+			CHECK_EQ(navigation_server->region_get_connection_pathway_end(region, 55), Vector2());
+			CHECK_EQ(navigation_server->region_get_connection_pathway_start(region, 55), Vector2());
+			ERR_PRINT_ON;
+		}
+
+		navigation_server->free(region);
+	}
+
+	// This test case does not check precise values on purpose - to not be too sensitivte.
+	TEST_CASE("[NavigationServer2D] Server should move agent properly") {
+		NavigationServer2D *navigation_server = NavigationServer2D::get_singleton();
+
+		RID map = navigation_server->map_create();
+		RID agent = navigation_server->agent_create();
+
+		navigation_server->map_set_active(map, true);
+		navigation_server->agent_set_map(agent, map);
+		navigation_server->agent_set_avoidance_enabled(agent, true);
+		navigation_server->agent_set_velocity(agent, Vector2(1, 1));
+		CallableMock agent_avoidance_callback_mock;
+		navigation_server->agent_set_avoidance_callback(agent, callable_mp(&agent_avoidance_callback_mock, &CallableMock::function1));
+		CHECK_EQ(agent_avoidance_callback_mock.function1_calls, 0);
+		navigation_server->process(0.0); // Give server some cycles to commit.
+		CHECK_EQ(agent_avoidance_callback_mock.function1_calls, 1);
+		CHECK_NE(agent_avoidance_callback_mock.function1_latest_arg0, Vector3(0, 0, 0));
+
+		navigation_server->free(agent);
+		navigation_server->free(map);
+	}
+
+	// This test case does not check precise values on purpose - to not be too sensitivte.
+	TEST_CASE("[NavigationServer2D] Server should make agents avoid each other when avoidance enabled") {
+		NavigationServer2D *navigation_server = NavigationServer2D::get_singleton();
+
+		RID map = navigation_server->map_create();
+		RID agent_1 = navigation_server->agent_create();
+		RID agent_2 = navigation_server->agent_create();
+
+		navigation_server->map_set_active(map, true);
+
+		navigation_server->agent_set_map(agent_1, map);
+		navigation_server->agent_set_avoidance_enabled(agent_1, true);
+		navigation_server->agent_set_position(agent_1, Vector2(0, 0));
+		navigation_server->agent_set_radius(agent_1, 1);
+		navigation_server->agent_set_velocity(agent_1, Vector2(1, 0));
+		CallableMock agent_1_avoidance_callback_mock;
+		navigation_server->agent_set_avoidance_callback(agent_1, callable_mp(&agent_1_avoidance_callback_mock, &CallableMock::function1));
+
+		navigation_server->agent_set_map(agent_2, map);
+		navigation_server->agent_set_avoidance_enabled(agent_2, true);
+		navigation_server->agent_set_position(agent_2, Vector2(2.5, 0.5));
+		navigation_server->agent_set_radius(agent_2, 1);
+		navigation_server->agent_set_velocity(agent_2, Vector2(-1, 0));
+		CallableMock agent_2_avoidance_callback_mock;
+		navigation_server->agent_set_avoidance_callback(agent_2, callable_mp(&agent_2_avoidance_callback_mock, &CallableMock::function1));
+
+		CHECK_EQ(agent_1_avoidance_callback_mock.function1_calls, 0);
+		CHECK_EQ(agent_2_avoidance_callback_mock.function1_calls, 0);
+		navigation_server->process(0.0); // Give server some cycles to commit.
+		CHECK_EQ(agent_1_avoidance_callback_mock.function1_calls, 1);
+		CHECK_EQ(agent_2_avoidance_callback_mock.function1_calls, 1);
+		Vector3 agent_1_safe_velocity = agent_1_avoidance_callback_mock.function1_latest_arg0;
+		Vector3 agent_2_safe_velocity = agent_2_avoidance_callback_mock.function1_latest_arg0;
+		CHECK_MESSAGE(agent_1_safe_velocity.x > 0, "agent 1 should move a bit along desired velocity (+X)");
+		CHECK_MESSAGE(agent_2_safe_velocity.x < 0, "agent 2 should move a bit along desired velocity (-X)");
+		CHECK_MESSAGE(agent_1_safe_velocity.z < 0, "agent 1 should move a bit to the side so that it avoids agent 2");
+		CHECK_MESSAGE(agent_2_safe_velocity.z > 0, "agent 2 should move a bit to the side so that it avoids agent 1");
+
+		navigation_server->free(agent_2);
+		navigation_server->free(agent_1);
+		navigation_server->free(map);
+	}
+
+	TEST_CASE("[NavigationServer2D] Server should make agents avoid dynamic obstacles when avoidance enabled") {
+		NavigationServer2D *navigation_server = NavigationServer2D::get_singleton();
+
+		RID map = navigation_server->map_create();
+		RID agent_1 = navigation_server->agent_create();
+		RID obstacle_1 = navigation_server->obstacle_create();
+
+		navigation_server->map_set_active(map, true);
+
+		navigation_server->agent_set_map(agent_1, map);
+		navigation_server->agent_set_avoidance_enabled(agent_1, true);
+		navigation_server->agent_set_position(agent_1, Vector2(0, 0));
+		navigation_server->agent_set_radius(agent_1, 1);
+		navigation_server->agent_set_velocity(agent_1, Vector2(1, 0));
+		CallableMock agent_1_avoidance_callback_mock;
+		navigation_server->agent_set_avoidance_callback(agent_1, callable_mp(&agent_1_avoidance_callback_mock, &CallableMock::function1));
+
+		navigation_server->obstacle_set_map(obstacle_1, map);
+		navigation_server->obstacle_set_avoidance_enabled(obstacle_1, true);
+		navigation_server->obstacle_set_position(obstacle_1, Vector2(2.5, 0.5));
+		navigation_server->obstacle_set_radius(obstacle_1, 1);
+
+		CHECK_EQ(agent_1_avoidance_callback_mock.function1_calls, 0);
+		navigation_server->process(0.0); // Give server some cycles to commit.
+		CHECK_EQ(agent_1_avoidance_callback_mock.function1_calls, 1);
+		Vector3 agent_1_safe_velocity = agent_1_avoidance_callback_mock.function1_latest_arg0;
+		CHECK_MESSAGE(agent_1_safe_velocity.x > 0, "Agent 1 should move a bit along desired velocity (+X).");
+		CHECK_MESSAGE(agent_1_safe_velocity.z < 0, "Agent 1 should move a bit to the side so that it avoids obstacle.");
+
+		navigation_server->free(obstacle_1);
+		navigation_server->free(agent_1);
+		navigation_server->free(map);
+		navigation_server->process(0.0); // Give server some cycles to commit.
+	}
+
+	TEST_CASE("[NavigationServer2D] Server should make agents avoid static obstacles when avoidance enabled") {
+		NavigationServer2D *navigation_server = NavigationServer2D::get_singleton();
+
+		RID map = navigation_server->map_create();
+		RID agent_1 = navigation_server->agent_create();
+		RID agent_2 = navigation_server->agent_create();
+		RID obstacle_1 = navigation_server->obstacle_create();
+
+		navigation_server->map_set_active(map, true);
+
+		navigation_server->agent_set_map(agent_1, map);
+		navigation_server->agent_set_avoidance_enabled(agent_1, true);
+		navigation_server->agent_set_radius(agent_1, 1.6); // Have hit the obstacle already.
+		navigation_server->agent_set_velocity(agent_1, Vector2(1, 0));
+		CallableMock agent_1_avoidance_callback_mock;
+		navigation_server->agent_set_avoidance_callback(agent_1, callable_mp(&agent_1_avoidance_callback_mock, &CallableMock::function1));
+
+		navigation_server->agent_set_map(agent_2, map);
+		navigation_server->agent_set_avoidance_enabled(agent_2, true);
+		navigation_server->agent_set_radius(agent_2, 1.4); // Haven't hit the obstacle yet.
+		navigation_server->agent_set_velocity(agent_2, Vector2(1, 0));
+		CallableMock agent_2_avoidance_callback_mock;
+		navigation_server->agent_set_avoidance_callback(agent_2, callable_mp(&agent_2_avoidance_callback_mock, &CallableMock::function1));
+
+		navigation_server->obstacle_set_map(obstacle_1, map);
+		navigation_server->obstacle_set_avoidance_enabled(obstacle_1, true);
+		PackedVector2Array obstacle_1_vertices;
+
+		SUBCASE("Static obstacles should work on ground level") {
+			navigation_server->agent_set_position(agent_1, Vector2(0, 0));
+			navigation_server->agent_set_position(agent_2, Vector2(0, 5));
+			obstacle_1_vertices.push_back(Vector2(1.5, 0.5));
+			obstacle_1_vertices.push_back(Vector2(1.5, 4.5));
+		}
+
+		navigation_server->obstacle_set_vertices(obstacle_1, obstacle_1_vertices);
+
+		CHECK_EQ(agent_1_avoidance_callback_mock.function1_calls, 0);
+		CHECK_EQ(agent_2_avoidance_callback_mock.function1_calls, 0);
+		navigation_server->process(0.0); // Give server some cycles to commit.
+		CHECK_EQ(agent_1_avoidance_callback_mock.function1_calls, 1);
+		CHECK_EQ(agent_2_avoidance_callback_mock.function1_calls, 1);
+		Vector3 agent_1_safe_velocity = agent_1_avoidance_callback_mock.function1_latest_arg0;
+		Vector3 agent_2_safe_velocity = agent_2_avoidance_callback_mock.function1_latest_arg0;
+		CHECK_MESSAGE(agent_1_safe_velocity.x > 0, "Agent 1 should move a bit along desired velocity (+X).");
+		CHECK_MESSAGE(agent_1_safe_velocity.z < 0, "Agent 1 should move a bit to the side so that it avoids obstacle.");
+		CHECK_MESSAGE(agent_2_safe_velocity.x > 0, "Agent 2 should move a bit along desired velocity (+X).");
+		CHECK_MESSAGE(agent_2_safe_velocity.z == 0, "Agent 2 should not move to the side.");
+
+		navigation_server->free(obstacle_1);
+		navigation_server->free(agent_2);
+		navigation_server->free(agent_1);
+		navigation_server->free(map);
+		navigation_server->process(0.0); // Give server some cycles to commit.
+	}
+
+	TEST_CASE("[NavigationServer2D][SceneTree] Server should be able to parse geometry") {
+		NavigationServer2D *navigation_server = NavigationServer2D::get_singleton();
+
+		// Prepare scene tree with simple mesh to serve as an input geometry.
+		Node2D *node_2d = memnew(Node2D);
+		SceneTree::get_singleton()->get_root()->add_child(node_2d);
+		Polygon2D *polygon = memnew(Polygon2D);
+		polygon->set_polygon(PackedVector2Array({ Vector2(200.0, 200.0), Vector2(400.0, 200.0), Vector2(400.0, 400.0), Vector2(200.0, 400.0) }));
+		node_2d->add_child(polygon);
+
+		// TODO: Use MeshInstance2D as well?
+
+		Ref<NavigationPolygon> navigation_polygon;
+		navigation_polygon.instantiate();
+		Ref<NavigationMeshSourceGeometryData2D> source_geometry;
+		source_geometry.instantiate();
+		CHECK_EQ(source_geometry->get_traversable_outlines().size(), 0);
+		CHECK_EQ(source_geometry->get_obstruction_outlines().size(), 0);
+
+		navigation_server->parse_source_geometry_data(navigation_polygon, source_geometry, polygon);
+		CHECK_EQ(source_geometry->get_traversable_outlines().size(), 0);
+		REQUIRE_EQ(source_geometry->get_obstruction_outlines().size(), 1);
+		CHECK_EQ(((PackedVector2Array)source_geometry->get_obstruction_outlines()[0]).size(), 4);
+
+		SUBCASE("By default, parsing should remove any data that was parsed before") {
+			navigation_server->parse_source_geometry_data(navigation_polygon, source_geometry, polygon);
+			CHECK_EQ(source_geometry->get_traversable_outlines().size(), 0);
+			REQUIRE_EQ(source_geometry->get_obstruction_outlines().size(), 1);
+			CHECK_EQ(((PackedVector2Array)source_geometry->get_obstruction_outlines()[0]).size(), 4);
+		}
+
+		SUBCASE("Parsed geometry should be extendible with other geometry") {
+			source_geometry->merge(source_geometry); // Merging with itself.
+			CHECK_EQ(source_geometry->get_traversable_outlines().size(), 0);
+			REQUIRE_EQ(source_geometry->get_obstruction_outlines().size(), 2);
+			const PackedVector2Array obstruction_outline_1 = source_geometry->get_obstruction_outlines()[0];
+			const PackedVector2Array obstruction_outline_2 = source_geometry->get_obstruction_outlines()[1];
+			REQUIRE_EQ(obstruction_outline_1.size(), 4);
+			REQUIRE_EQ(obstruction_outline_2.size(), 4);
+			CHECK_EQ(obstruction_outline_1[0], obstruction_outline_2[0]);
+			CHECK_EQ(obstruction_outline_1[1], obstruction_outline_2[1]);
+			CHECK_EQ(obstruction_outline_1[2], obstruction_outline_2[2]);
+			CHECK_EQ(obstruction_outline_1[3], obstruction_outline_2[3]);
+		}
+
+		memdelete(polygon);
+		memdelete(node_2d);
+	}
+
+	// This test case uses only public APIs on purpose - other test cases use simplified baking.
+	TEST_CASE("[NavigationServer2D][SceneTree] Server should be able to bake map correctly") {
+		NavigationServer2D *navigation_server = NavigationServer2D::get_singleton();
+
+		// Prepare scene tree with simple mesh to serve as an input geometry.
+		Node2D *node_2d = memnew(Node2D);
+		SceneTree::get_singleton()->get_root()->add_child(node_2d);
+		Polygon2D *polygon = memnew(Polygon2D);
+		polygon->set_polygon(PackedVector2Array({ Vector2(-200.0, -200.0), Vector2(200.0, -200.0), Vector2(200.0, 200.0), Vector2(-200.0, 200.0) }));
+		node_2d->add_child(polygon);
+
+		// TODO: Use MeshInstance2D as well?
+
+		// Prepare anything necessary to bake navigation polygon.
+		RID map = navigation_server->map_create();
+		RID region = navigation_server->region_create();
+		Ref<NavigationPolygon> navigation_polygon;
+		navigation_polygon.instantiate();
+		navigation_polygon->add_outline(PackedVector2Array({ Vector2(-1000.0, -1000.0), Vector2(1000.0, -1000.0), Vector2(1000.0, 1000.0), Vector2(-1000.0, 1000.0) }));
+		navigation_server->map_set_active(map, true);
+		navigation_server->map_set_use_async_iterations(map, false);
+		navigation_server->region_set_map(region, map);
+		navigation_server->region_set_navigation_polygon(region, navigation_polygon);
+		navigation_server->process(0.0); // Give server some cycles to commit.
+
+		CHECK_EQ(navigation_polygon->get_polygon_count(), 0);
+		CHECK_EQ(navigation_polygon->get_vertices().size(), 0);
+		CHECK_EQ(navigation_polygon->get_outline_count(), 1);
+
+		Ref<NavigationMeshSourceGeometryData2D> source_geometry;
+		source_geometry.instantiate();
+		navigation_server->parse_source_geometry_data(navigation_polygon, source_geometry, node_2d);
+		navigation_server->bake_from_source_geometry_data(navigation_polygon, source_geometry, Callable());
+		// FIXME: The above line should trigger the update (line below) under the hood.
+		navigation_server->region_set_navigation_polygon(region, navigation_polygon); // Force update.
+		CHECK_EQ(navigation_polygon->get_polygon_count(), 4);
+		CHECK_EQ(navigation_polygon->get_vertices().size(), 8);
+		CHECK_EQ(navigation_polygon->get_outline_count(), 1);
+
+		SUBCASE("Map should emit signal and take newly baked navigation mesh into account") {
+			SIGNAL_WATCH(navigation_server, "map_changed");
+			SIGNAL_CHECK_FALSE("map_changed");
+			navigation_server->process(0.0); // Give server some cycles to commit.
+			SIGNAL_CHECK("map_changed", build_array(build_array(map)));
+			SIGNAL_UNWATCH(navigation_server, "map_changed");
+			CHECK_NE(navigation_server->map_get_closest_point(map, Vector2(0, 0)), Vector2(0, 0));
+		}
+
+		navigation_server->free(region);
+		navigation_server->free(map);
+		navigation_server->process(0.0); // Give server some cycles to commit.
+
+		memdelete(polygon);
+		memdelete(node_2d);
+	}
+
+	// This test case does not check precise values on purpose - to not be too sensitivte.
+	TEST_CASE("[NavigationServer2D] Server should respond to queries against valid map properly") {
+		NavigationServer2D *navigation_server = NavigationServer2D::get_singleton();
+		Ref<NavigationPolygon> navigation_polygon;
+		navigation_polygon.instantiate();
+		Ref<NavigationMeshSourceGeometryData2D> source_geometry;
+		source_geometry.instantiate();
+
+		navigation_polygon->add_outline(PackedVector2Array({ Vector2(-1000.0, -1000.0), Vector2(1000.0, -1000.0), Vector2(1000.0, 1000.0), Vector2(-1000.0, 1000.0) }));
+
+		// TODO: Other input?
+		source_geometry->add_obstruction_outline(PackedVector2Array({ Vector2(-200.0, -200.0), Vector2(200.0, -200.0), Vector2(200.0, 200.0), Vector2(-200.0, 200.0) }));
+
+		navigation_server->bake_from_source_geometry_data(navigation_polygon, source_geometry, Callable());
+		CHECK_NE(navigation_polygon->get_polygon_count(), 0);
+		CHECK_NE(navigation_polygon->get_vertices().size(), 0);
+		CHECK_NE(navigation_polygon->get_outline_count(), 0);
+
+		RID map = navigation_server->map_create();
+		RID region = navigation_server->region_create();
+		navigation_server->map_set_active(map, true);
+		navigation_server->map_set_use_async_iterations(map, false);
+		navigation_server->region_set_map(region, map);
+		navigation_server->region_set_navigation_polygon(region, navigation_polygon);
+		navigation_server->process(0.0); // Give server some cycles to commit.
+
+		SUBCASE("Simple queries should return non-default values") {
+			CHECK_NE(navigation_server->map_get_closest_point(map, Vector2(0.0, 0.0)), Vector2(0, 0));
+			CHECK(navigation_server->map_get_closest_point_owner(map, Vector2(0.0, 0.0)).is_valid());
+			CHECK_NE(navigation_server->map_get_path(map, Vector2(0, 0), Vector2(10, 10), true).size(), 0);
+			CHECK_NE(navigation_server->map_get_path(map, Vector2(0, 0), Vector2(10, 10), false).size(), 0);
+		}
+
+		SUBCASE("Elaborate query with 'CORRIDORFUNNEL' post-processing should yield non-empty result") {
+			Ref<NavigationPathQueryParameters2D> query_parameters;
+			query_parameters.instantiate();
+			query_parameters->set_map(map);
+			query_parameters->set_start_position(Vector2(0, 0));
+			query_parameters->set_target_position(Vector2(10, 10));
+			query_parameters->set_path_postprocessing(NavigationPathQueryParameters2D::PATH_POSTPROCESSING_CORRIDORFUNNEL);
+			Ref<NavigationPathQueryResult2D> query_result;
+			query_result.instantiate();
+			navigation_server->query_path(query_parameters, query_result);
+			CHECK_NE(query_result->get_path().size(), 0);
+			CHECK_NE(query_result->get_path_types().size(), 0);
+			CHECK_NE(query_result->get_path_rids().size(), 0);
+			CHECK_NE(query_result->get_path_owner_ids().size(), 0);
+		}
+
+		SUBCASE("Elaborate query with 'EDGECENTERED' post-processing should yield non-empty result") {
+			Ref<NavigationPathQueryParameters2D> query_parameters;
+			query_parameters.instantiate();
+			query_parameters->set_map(map);
+			query_parameters->set_start_position(Vector2(10, 10));
+			query_parameters->set_target_position(Vector2(0, 0));
+			query_parameters->set_path_postprocessing(NavigationPathQueryParameters2D::PATH_POSTPROCESSING_EDGECENTERED);
+			Ref<NavigationPathQueryResult2D> query_result;
+			query_result.instantiate();
+			navigation_server->query_path(query_parameters, query_result);
+			CHECK_NE(query_result->get_path().size(), 0);
+			CHECK_NE(query_result->get_path_types().size(), 0);
+			CHECK_NE(query_result->get_path_rids().size(), 0);
+			CHECK_NE(query_result->get_path_owner_ids().size(), 0);
+		}
+
+		SUBCASE("Elaborate query with non-matching navigation layer mask should yield empty result") {
+			Ref<NavigationPathQueryParameters2D> query_parameters;
+			query_parameters.instantiate();
+			query_parameters->set_map(map);
+			query_parameters->set_start_position(Vector2(10, 10));
+			query_parameters->set_target_position(Vector2(0, 0));
+			query_parameters->set_navigation_layers(2);
+			Ref<NavigationPathQueryResult2D> query_result;
+			query_result.instantiate();
+			navigation_server->query_path(query_parameters, query_result);
+			CHECK_EQ(query_result->get_path().size(), 0);
+			CHECK_EQ(query_result->get_path_types().size(), 0);
+			CHECK_EQ(query_result->get_path_rids().size(), 0);
+			CHECK_EQ(query_result->get_path_owner_ids().size(), 0);
+		}
+
+		SUBCASE("Elaborate query without metadata flags should yield path only") {
+			Ref<NavigationPathQueryParameters2D> query_parameters;
+			query_parameters.instantiate();
+			query_parameters->set_map(map);
+			query_parameters->set_start_position(Vector2(10, 10));
+			query_parameters->set_target_position(Vector2(0, 0));
+			query_parameters->set_metadata_flags(0);
+			Ref<NavigationPathQueryResult2D> query_result;
+			query_result.instantiate();
+			navigation_server->query_path(query_parameters, query_result);
+			CHECK_NE(query_result->get_path().size(), 0);
+			CHECK_EQ(query_result->get_path_types().size(), 0);
+			CHECK_EQ(query_result->get_path_rids().size(), 0);
+			CHECK_EQ(query_result->get_path_owner_ids().size(), 0);
+		}
+
+		navigation_server->free(region);
+		navigation_server->free(map);
+		navigation_server->process(0.0); // Give server some cycles to commit.
+	}
+
+	TEST_CASE("[NavigationServer2D] Server should simplify path properly") {
+		real_t simplify_epsilon = 0.2;
+		Vector<Vector2> source_path;
+		source_path.resize(7);
+		source_path.write[0] = Vector2(0.0, 0.0);
+		source_path.write[1] = Vector2(0.0, 1.0); // This point needs to go.
+		source_path.write[2] = Vector2(0.0, 2.0); // This point needs to go.
+		source_path.write[3] = Vector2(0.0, 2.0);
+		source_path.write[4] = Vector2(2.0, 3.0);
+		source_path.write[5] = Vector2(2.5, 4.0); // This point needs to go.
+		source_path.write[6] = Vector2(3.0, 5.0);
+		Vector<Vector2> simplified_path = NavigationServer2D::get_singleton()->simplify_path(source_path, simplify_epsilon);
+		CHECK_EQ(simplified_path.size(), 4);
 	}
 }
 } //namespace TestNavigationServer2D

tests/servers/test_navigation_server_3d.h

@@ -60,7 +60,7 @@ static inline Array build_array(Variant item, Targs... Fargs) {
 	return a;
 }
 
-TEST_SUITE("[Navigation]") {
+TEST_SUITE("[Navigation3D]") {
 	TEST_CASE("[NavigationServer3D] Server should be empty when initialized") {
 		NavigationServer3D *navigation_server = NavigationServer3D::get_singleton();
 		CHECK_EQ(navigation_server->get_maps().size(), 0);

tests/test_main.cpp

@@ -163,17 +163,20 @@
 #include "tests/scene/test_tree.h"
 #endif // ADVANCED_GUI_DISABLED
 
-#ifndef _3D_DISABLED
-#ifdef MODULE_NAVIGATION_ENABLED
+#ifdef MODULE_NAVIGATION_2D_ENABLED
 #include "tests/scene/test_navigation_agent_2d.h"
-#include "tests/scene/test_navigation_agent_3d.h"
 #include "tests/scene/test_navigation_obstacle_2d.h"
-#include "tests/scene/test_navigation_obstacle_3d.h"
 #include "tests/scene/test_navigation_region_2d.h"
-#include "tests/scene/test_navigation_region_3d.h"
 #include "tests/servers/test_navigation_server_2d.h"
+#endif // MODULE_NAVIGATION_2D_ENABLED
+
+#ifndef _3D_DISABLED
+#ifdef MODULE_NAVIGATION_3D_ENABLED
+#include "tests/scene/test_navigation_agent_3d.h"
+#include "tests/scene/test_navigation_obstacle_3d.h"
+#include "tests/scene/test_navigation_region_3d.h"
 #include "tests/servers/test_navigation_server_3d.h"
-#endif // MODULE_NAVIGATION_ENABLED
+#endif // MODULE_NAVIGATION_3D_ENABLED
 
 #include "tests/scene/test_arraymesh.h"
 #include "tests/scene/test_camera_3d.h"
@@ -194,8 +197,8 @@
 #include "tests/test_macros.h"
 
 #include "scene/theme/theme_db.h"
-#ifndef _3D_DISABLED
 #include "servers/navigation_server_2d.h"
+#ifndef _3D_DISABLED
 #include "servers/navigation_server_3d.h"
 #endif // _3D_DISABLED
 #ifndef PHYSICS_2D_DISABLED
@@ -288,8 +291,8 @@ struct GodotTestCaseListener : public doctest::IReporter {
 #endif // PHYSICS_3D_DISABLED
 #ifndef _3D_DISABLED
 	NavigationServer3D *navigation_server_3d = nullptr;
-	NavigationServer2D *navigation_server_2d = nullptr;
 #endif // _3D_DISABLED
+	NavigationServer2D *navigation_server_2d = nullptr;
 
 	void test_case_start(const doctest::TestCaseData &p_in) override {
 		reinitialize();
@@ -337,12 +340,12 @@ struct GodotTestCaseListener : public doctest::IReporter {
 			physics_server_2d->init();
 #endif // PHYSICS_2D_DISABLED
 
-#ifndef _3D_DISABLED
 			ERR_PRINT_OFF;
+#ifndef _3D_DISABLED
 			navigation_server_3d = NavigationServer3DManager::new_default_server();
+#endif // _3D_DISABLED
 			navigation_server_2d = NavigationServer2DManager::new_default_server();
 			ERR_PRINT_ON;
-#endif // _3D_DISABLED
 
 			memnew(InputMap);
 			InputMap::get_singleton()->load_default();
@@ -374,14 +377,20 @@ struct GodotTestCaseListener : public doctest::IReporter {
 		}
 
 #ifndef _3D_DISABLED
-		if (suite_name.contains("[Navigation]") && navigation_server_2d == nullptr && navigation_server_3d == nullptr) {
+		if (suite_name.contains("[Navigation3D]") && navigation_server_3d == nullptr) {
 			ERR_PRINT_OFF;
 			navigation_server_3d = NavigationServer3DManager::new_default_server();
-			navigation_server_2d = NavigationServer2DManager::new_default_server();
 			ERR_PRINT_ON;
 			return;
 		}
 #endif // _3D_DISABLED
+
+		if (suite_name.contains("[Navigation2D]") && navigation_server_2d == nullptr) {
+			ERR_PRINT_OFF;
+			navigation_server_2d = NavigationServer2DManager::new_default_server();
+			ERR_PRINT_ON;
+			return;
+		}
 	}
 
 	void test_case_end(const doctest::CurrentTestCaseStats &) override {
@@ -416,12 +425,12 @@ struct GodotTestCaseListener : public doctest::IReporter {
 			memdelete(navigation_server_3d);
 			navigation_server_3d = nullptr;
 		}
+#endif // _3D_DISABLED
 
 		if (navigation_server_2d) {
 			memdelete(navigation_server_2d);
 			navigation_server_2d = nullptr;
 		}
-#endif // _3D_DISABLED
 
 #ifndef PHYSICS_3D_DISABLED
 		if (physics_server_3d) {