Add 2D navigation mesh baking

Adds 2D navigation mesh baking.

8 files changed, 1550 insertions, 11 deletions

diff --git a/modules/navigation/config.py b/modules/navigation/config.py
index d22f9454ed..a42f27fbe1 100644
--- a/modules/navigation/config.py
+++ b/modules/navigation/config.py
@@ -1,5 +1,5 @@
 def can_build(env, platform):
-    return True
+    return not env["disable_3d"]
 
 
 def configure(env):
diff --git a/modules/navigation/godot_navigation_server.cpp b/modules/navigation/godot_navigation_server.cpp
index 9162fcf171..32482e0c9d 100644
--- a/modules/navigation/godot_navigation_server.cpp
+++ b/modules/navigation/godot_navigation_server.cpp
@@ -1086,6 +1086,14 @@ void GodotNavigationServer::map_force_update(RID p_map) {
 	map->sync();
 }
 
+void GodotNavigationServer::sync() {
+#ifndef _3D_DISABLED
+	if (navmesh_generator_3d) {
+		navmesh_generator_3d->sync();
+	}
+#endif // _3D_DISABLED
+}
+
 void GodotNavigationServer::process(real_t p_delta_time) {
 	flush_queries();
 
@@ -1093,16 +1101,6 @@ void GodotNavigationServer::process(real_t p_delta_time) {
 		return;
 	}
 
-#ifndef _3D_DISABLED
-	// Sync finished navmesh bakes before doing NavMap updates.
-	if (navmesh_generator_3d) {
-		navmesh_generator_3d->sync();
-		// Finished bakes emit callbacks and users might have reacted to those.
-		// Flush queue again so users do not have to wait for the next sync.
-		flush_queries();
-	}
-#endif // _3D_DISABLED
-
 	int _new_pm_region_count = 0;
 	int _new_pm_agent_count = 0;
 	int _new_pm_link_count = 0;
diff --git a/modules/navigation/godot_navigation_server.h b/modules/navigation/godot_navigation_server.h
index c12605bc7a..4ead4fc398 100644
--- a/modules/navigation/godot_navigation_server.h
+++ b/modules/navigation/godot_navigation_server.h
@@ -243,6 +243,7 @@ public:
 	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 NavigationUtilities::PathQueryResult _query_path(const NavigationUtilities::PathQueryParameters &p_parameters) const override;
diff --git a/modules/navigation/godot_navigation_server_2d.cpp b/modules/navigation/godot_navigation_server_2d.cpp
new file mode 100644
index 0000000000..b54729e06f
--- /dev/null
+++ b/modules/navigation/godot_navigation_server_2d.cpp
@@ -0,0 +1,369 @@
+/**************************************************************************/
+/*  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))); \
+	}
+
+#define FORWARD_5_R_C(CONV_R, FUNC_NAME, T_0, D_0, T_1, D_1, T_2, D_2, T_3, D_3, T_4, D_4, CONV_0, CONV_1, CONV_2, CONV_3, CONV_4)      \
+	GodotNavigationServer2D::FUNC_NAME(T_0 D_0, T_1 D_1, T_2 D_2, T_3 D_3, T_4 D_4)                                                     \
+			const {                                                                                                                     \
+		return CONV_R(NavigationServer3D::get_singleton()->FUNC_NAME(CONV_0(D_0), CONV_1(D_1), CONV_2(D_2), CONV_3(D_3), CONV_4(D_4))); \
+	}
+
+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 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>();
+	}
+}
+
+void GodotNavigationServer2D::init() {
+#ifdef CLIPPER2_ENABLED
+	navmesh_generator_2d = memnew(NavMeshGenerator2D);
+	ERR_FAIL_NULL_MSG(navmesh_generator_2d, "Failed to init NavMeshGenerator2D.");
+#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_valid(), "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_valid(), "Invalid navigation polygon.");
+	ERR_FAIL_COND_MSG(!p_source_geometry_data.is_valid(), "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_valid(), "Invalid navigation mesh.");
+	ERR_FAIL_COND_MSG(!p_source_geometry_data.is_valid(), "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
+}
+
+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);
+}
+
+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> FORWARD_5_R_C(vector_v3_to_v2, map_get_path, RID, p_map, Vector2, p_origin, Vector2, p_destination, bool, p_optimize, uint32_t, p_layers, rid_to_rid, v2_to_v3, v2_to_v3, bool_to_bool, uint32_to_uint32);
+
+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);
+
+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);
+
+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);
+
+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);
+void FORWARD_2(agent_set_max_neighbors, RID, p_agent, int, p_count, rid_to_rid, int_to_int);
+void FORWARD_2(agent_set_time_horizon_agents, RID, p_agent, real_t, p_time_horizon, rid_to_rid, real_to_real);
+void FORWARD_2(agent_set_time_horizon_obstacles, RID, p_agent, real_t, p_time_horizon, rid_to_rid, real_to_real);
+void FORWARD_2(agent_set_radius, RID, p_agent, real_t, p_radius, rid_to_rid, real_to_real);
+void FORWARD_2(agent_set_max_speed, RID, p_agent, real_t, p_max_speed, rid_to_rid, real_to_real);
+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);
+void FORWARD_2(agent_set_position, RID, p_agent, Vector2, p_position, rid_to_rid, v2_to_v3);
+
+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 FORWARD_1(free, RID, p_object, rid_to_rid);
+void FORWARD_2(agent_set_avoidance_callback, RID, p_agent, Callable, p_callback, rid_to_rid, callable_to_callable);
+
+void FORWARD_2(agent_set_avoidance_layers, RID, p_agent, uint32_t, p_layers, rid_to_rid, uint32_to_uint32);
+void FORWARD_2(agent_set_avoidance_mask, RID, p_agent, uint32_t, p_mask, rid_to_rid, uint32_to_uint32);
+void FORWARD_2(agent_set_avoidance_priority, RID, p_agent, real_t, p_priority, rid_to_rid, real_to_real);
+
+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);
+void FORWARD_2(obstacle_set_velocity, RID, p_obstacle, Vector2, p_velocity, rid_to_rid, v2_to_v3);
+void FORWARD_2(obstacle_set_position, RID, p_obstacle, Vector2, p_position, rid_to_rid, v2_to_v3);
+void FORWARD_2(obstacle_set_avoidance_layers, RID, p_obstacle, uint32_t, p_layers, rid_to_rid, uint32_to_uint32);
+
+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));
+}
+
+void GodotNavigationServer2D::query_path(const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result) const {
+	ERR_FAIL_COND(!p_query_parameters.is_valid());
+	ERR_FAIL_COND(!p_query_result.is_valid());
+
+	const NavigationUtilities::PathQueryResult _query_result = NavigationServer3D::get_singleton()->_query_path(p_query_parameters->get_parameters());
+
+	p_query_result->set_path(vector_v3_to_v2(_query_result.path));
+	p_query_result->set_path_types(_query_result.path_types);
+	p_query_result->set_path_rids(_query_result.path_rids);
+	p_query_result->set_path_owner_ids(_query_result.path_owner_ids);
+}
diff --git a/modules/navigation/godot_navigation_server_2d.h b/modules/navigation/godot_navigation_server_2d.h
new file mode 100644
index 0000000000..337f5f40d8
--- /dev/null
+++ b/modules/navigation/godot_navigation_server_2d.h
@@ -0,0 +1,234 @@
+/**************************************************************************/
+/*  godot_navigation_server_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.                 */
+/**************************************************************************/
+
+#ifndef GODOT_NAVIGATION_SERVER_2D_H
+#define GODOT_NAVIGATION_SERVER_2D_H
+
+#include "nav_agent.h"
+#include "nav_link.h"
+#include "nav_map.h"
+#include "nav_obstacle.h"
+#include "nav_region.h"
+
+#include "servers/navigation_server_2d.h"
+
+#ifdef CLIPPER2_ENABLED
+class NavMeshGenerator2D;
+#endif // CLIPPER2_ENABLED
+
+// This server exposes the `NavigationServer3D` features in the 2D world.
+class GodotNavigationServer2D : public NavigationServer2D {
+	GDCLASS(GodotNavigationServer2D, NavigationServer2D);
+
+#ifdef CLIPPER2_ENABLED
+	NavMeshGenerator2D *navmesh_generator_2d = nullptr;
+#endif // CLIPPER2_ENABLED
+
+public:
+	GodotNavigationServer2D();
+	virtual ~GodotNavigationServer2D();
+
+	virtual TypedArray<RID> get_maps() const override;
+
+	virtual RID map_create() override;
+	virtual void map_set_active(RID p_map, bool p_active) override;
+	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;
+	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;
+	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;
+	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;
+	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) const 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 RID region_create() override;
+	virtual void region_set_enabled(RID p_region, bool p_enabled) override;
+	virtual bool region_get_enabled(RID p_region) const override;
+	virtual void region_set_use_edge_connections(RID p_region, bool p_enabled) override;
+	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;
+	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;
+	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;
+	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;
+	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;
+	virtual uint32_t region_get_navigation_layers(RID p_region) const override;
+	virtual void region_set_transform(RID p_region, Transform2D p_transform) override;
+	virtual void region_set_navigation_polygon(RID p_region, Ref<NavigationPolygon> p_navigation_polygon) override;
+	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;
+
+	virtual RID link_create() override;
+
+	/// Set the map of this link.
+	virtual void link_set_map(RID p_link, RID p_map) override;
+	virtual RID link_get_map(RID p_link) const override;
+
+	virtual void link_set_enabled(RID p_link, bool p_enabled) override;
+	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;
+	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;
+	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;
+	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;
+	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;
+	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;
+	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;
+	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;
+	virtual RID agent_get_map(RID p_agent) const override;
+
+	virtual void agent_set_paused(RID p_agent, bool p_paused) override;
+	virtual bool agent_get_paused(RID p_agent) const override;
+
+	virtual void agent_set_avoidance_enabled(RID p_agent, bool p_enabled) override;
+	virtual bool agent_get_avoidance_enabled(RID p_agent) const override;
+
+	/// The maximum distance (center point to
+	/// center point) to other agents this agent
+	/// takes into account in the navigation. The
+	/// larger this number, the longer the running
+	/// 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;
+
+	/// The maximum number of other agents this
+	/// agent takes into account in the navigation.
+	/// The larger this number, the longer the
+	/// 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;
+
+	/// The minimal amount of time for which this
+	/// agent's velocities that are computed by the
+	/// simulation are safe with respect to other
+	/// agents. The larger this number, the sooner
+	/// this agent will respond to the presence of
+	/// 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;
+	virtual void agent_set_time_horizon_obstacles(RID p_agent, real_t p_time_horizon) override;
+
+	/// The radius of this agent.
+	/// Must be non-negative.
+	virtual void agent_set_radius(RID p_agent, real_t p_radius) 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;
+
+	/// 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;
+
+	/// 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;
+
+	/// Position of the agent in world space.
+	virtual void agent_set_position(RID p_agent, Vector2 p_position) 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;
+
+	virtual void agent_set_avoidance_layers(RID p_agent, uint32_t p_layers) override;
+	virtual void agent_set_avoidance_mask(RID p_agent, uint32_t p_mask) override;
+	virtual void agent_set_avoidance_priority(RID p_agent, real_t p_priority) override;
+
+	virtual RID obstacle_create() override;
+	virtual void obstacle_set_avoidance_enabled(RID p_obstacle, bool p_enabled) override;
+	virtual bool obstacle_get_avoidance_enabled(RID p_obstacle) const override;
+	virtual void obstacle_set_map(RID p_obstacle, RID p_map) override;
+	virtual RID obstacle_get_map(RID p_obstacle) const override;
+	virtual void obstacle_set_paused(RID p_obstacle, bool p_paused) override;
+	virtual bool obstacle_get_paused(RID p_obstacle) const override;
+	virtual void obstacle_set_radius(RID p_obstacle, real_t p_radius) override;
+	virtual void obstacle_set_velocity(RID p_obstacle, Vector2 p_velocity) override;
+	virtual void obstacle_set_position(RID p_obstacle, Vector2 p_position) override;
+	virtual void obstacle_set_vertices(RID p_obstacle, const Vector<Vector2> &p_vertices) override;
+	virtual void obstacle_set_avoidance_layers(RID p_obstacle, uint32_t p_layers) override;
+
+	virtual void query_path(const Ref<NavigationPathQueryParameters2D> &p_query_parameters, Ref<NavigationPathQueryResult2D> p_query_result) const override;
+
+	virtual void init() override;
+	virtual void sync() override;
+	virtual void finish() override;
+	virtual void free(RID p_object) 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;
+	virtual void bake_from_source_geometry_data_async(const Ref<NavigationPolygon> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData2D> &p_source_geometry_data, const Callable &p_callback = Callable()) override;
+};
+
+#endif // GODOT_NAVIGATION_SERVER_2D_H
diff --git a/modules/navigation/nav_mesh_generator_2d.cpp b/modules/navigation/nav_mesh_generator_2d.cpp
new file mode 100644
index 0000000000..0c9f7e80fb
--- /dev/null
+++ b/modules/navigation/nav_mesh_generator_2d.cpp
@@ -0,0 +1,830 @@
+/**************************************************************************/
+/*  nav_mesh_generator_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_generator_2d.h"
+
+#include "core/config/project_settings.h"
+#include "scene/2d/mesh_instance_2d.h"
+#include "scene/2d/multimesh_instance_2d.h"
+#include "scene/2d/physics_body_2d.h"
+#include "scene/2d/polygon_2d.h"
+#include "scene/2d/tile_map.h"
+#include "scene/resources/capsule_shape_2d.h"
+#include "scene/resources/circle_shape_2d.h"
+#include "scene/resources/concave_polygon_shape_2d.h"
+#include "scene/resources/convex_polygon_shape_2d.h"
+#include "scene/resources/navigation_mesh_source_geometry_data_2d.h"
+#include "scene/resources/navigation_polygon.h"
+#include "scene/resources/rectangle_shape_2d.h"
+
+#include "thirdparty/clipper2/include/clipper2/clipper.h"
+#include "thirdparty/misc/polypartition.h"
+
+NavMeshGenerator2D *NavMeshGenerator2D::singleton = nullptr;
+Mutex NavMeshGenerator2D::baking_navmesh_mutex;
+Mutex NavMeshGenerator2D::generator_task_mutex;
+bool NavMeshGenerator2D::use_threads = true;
+bool NavMeshGenerator2D::baking_use_multiple_threads = true;
+bool NavMeshGenerator2D::baking_use_high_priority_threads = true;
+HashSet<Ref<NavigationPolygon>> NavMeshGenerator2D::baking_navmeshes;
+HashMap<WorkerThreadPool::TaskID, NavMeshGenerator2D::NavMeshGeneratorTask2D *> NavMeshGenerator2D::generator_tasks;
+
+NavMeshGenerator2D *NavMeshGenerator2D::get_singleton() {
+	return singleton;
+}
+
+NavMeshGenerator2D::NavMeshGenerator2D() {
+	ERR_FAIL_COND(singleton != nullptr);
+	singleton = this;
+
+	baking_use_multiple_threads = GLOBAL_GET("navigation/baking/thread_model/baking_use_multiple_threads");
+	baking_use_high_priority_threads = GLOBAL_GET("navigation/baking/thread_model/baking_use_high_priority_threads");
+
+	// Using threads might cause problems on certain exports or with the Editor on certain devices.
+	// This is the main switch to turn threaded navmesh baking off should the need arise.
+	use_threads = baking_use_multiple_threads && !Engine::get_singleton()->is_editor_hint();
+}
+
+NavMeshGenerator2D::~NavMeshGenerator2D() {
+	cleanup();
+}
+
+void NavMeshGenerator2D::sync() {
+	if (generator_tasks.size() == 0) {
+		return;
+	}
+
+	baking_navmesh_mutex.lock();
+	generator_task_mutex.lock();
+
+	LocalVector<WorkerThreadPool::TaskID> finished_task_ids;
+
+	for (KeyValue<WorkerThreadPool::TaskID, NavMeshGeneratorTask2D *> &E : generator_tasks) {
+		if (WorkerThreadPool::get_singleton()->is_task_completed(E.key)) {
+			WorkerThreadPool::get_singleton()->wait_for_task_completion(E.key);
+			finished_task_ids.push_back(E.key);
+
+			NavMeshGeneratorTask2D *generator_task = E.value;
+			DEV_ASSERT(generator_task->status = NavMeshGeneratorTask2D::TaskStatus::BAKING_FINISHED);
+
+			baking_navmeshes.erase(generator_task->navigation_mesh);
+			if (generator_task->callback.is_valid()) {
+				generator_emit_callback(generator_task->callback);
+			}
+			memdelete(generator_task);
+		}
+	}
+
+	for (WorkerThreadPool::TaskID finished_task_id : finished_task_ids) {
+		generator_tasks.erase(finished_task_id);
+	}
+
+	generator_task_mutex.unlock();
+	baking_navmesh_mutex.unlock();
+}
+
+void NavMeshGenerator2D::cleanup() {
+	baking_navmesh_mutex.lock();
+	generator_task_mutex.lock();
+
+	baking_navmeshes.clear();
+
+	for (KeyValue<WorkerThreadPool::TaskID, NavMeshGeneratorTask2D *> &E : generator_tasks) {
+		WorkerThreadPool::get_singleton()->wait_for_task_completion(E.key);
+		NavMeshGeneratorTask2D *generator_task = E.value;
+		memdelete(generator_task);
+	}
+	generator_tasks.clear();
+
+	generator_task_mutex.unlock();
+	baking_navmesh_mutex.unlock();
+}
+
+void NavMeshGenerator2D::finish() {
+	cleanup();
+}
+
+void NavMeshGenerator2D::parse_source_geometry_data(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_root_node, const Callable &p_callback) {
+	ERR_FAIL_COND(!Thread::is_main_thread());
+	ERR_FAIL_COND(!p_navigation_mesh.is_valid());
+	ERR_FAIL_NULL(p_root_node);
+	ERR_FAIL_COND(!p_root_node->is_inside_tree());
+	ERR_FAIL_COND(!p_source_geometry_data.is_valid());
+
+	generator_parse_source_geometry_data(p_navigation_mesh, p_source_geometry_data, p_root_node);
+
+	if (p_callback.is_valid()) {
+		generator_emit_callback(p_callback);
+	}
+}
+
+void NavMeshGenerator2D::bake_from_source_geometry_data(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, const Callable &p_callback) {
+	ERR_FAIL_COND(!p_navigation_mesh.is_valid());
+	ERR_FAIL_COND(!p_source_geometry_data.is_valid());
+
+	if (p_navigation_mesh->get_outline_count() == 0 && !p_source_geometry_data->has_data()) {
+		p_navigation_mesh->clear();
+		if (p_callback.is_valid()) {
+			generator_emit_callback(p_callback);
+		}
+		return;
+	}
+
+	baking_navmesh_mutex.lock();
+	if (baking_navmeshes.has(p_navigation_mesh)) {
+		baking_navmesh_mutex.unlock();
+		ERR_FAIL_MSG("NavigationPolygon is already baking. Wait for current bake to finish.");
+	}
+	baking_navmeshes.insert(p_navigation_mesh);
+	baking_navmesh_mutex.unlock();
+
+	generator_bake_from_source_geometry_data(p_navigation_mesh, p_source_geometry_data);
+
+	baking_navmesh_mutex.lock();
+	baking_navmeshes.erase(p_navigation_mesh);
+	baking_navmesh_mutex.unlock();
+
+	if (p_callback.is_valid()) {
+		generator_emit_callback(p_callback);
+	}
+}
+
+void NavMeshGenerator2D::bake_from_source_geometry_data_async(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, const Callable &p_callback) {
+	ERR_FAIL_COND(!p_navigation_mesh.is_valid());
+	ERR_FAIL_COND(!p_source_geometry_data.is_valid());
+
+	if (p_navigation_mesh->get_outline_count() == 0 && !p_source_geometry_data->has_data()) {
+		p_navigation_mesh->clear();
+		if (p_callback.is_valid()) {
+			generator_emit_callback(p_callback);
+		}
+		return;
+	}
+
+	if (!use_threads) {
+		bake_from_source_geometry_data(p_navigation_mesh, p_source_geometry_data, p_callback);
+		return;
+	}
+
+	baking_navmesh_mutex.lock();
+	if (baking_navmeshes.has(p_navigation_mesh)) {
+		baking_navmesh_mutex.unlock();
+		ERR_FAIL_MSG("NavigationPolygon is already baking. Wait for current bake to finish.");
+	}
+	baking_navmeshes.insert(p_navigation_mesh);
+	baking_navmesh_mutex.unlock();
+
+	generator_task_mutex.lock();
+	NavMeshGeneratorTask2D *generator_task = memnew(NavMeshGeneratorTask2D);
+	generator_task->navigation_mesh = p_navigation_mesh;
+	generator_task->source_geometry_data = p_source_geometry_data;
+	generator_task->callback = p_callback;
+	generator_task->status = NavMeshGeneratorTask2D::TaskStatus::BAKING_STARTED;
+	generator_task->thread_task_id = WorkerThreadPool::get_singleton()->add_native_task(&NavMeshGenerator2D::generator_thread_bake, generator_task, NavMeshGenerator2D::baking_use_high_priority_threads, "NavMeshGeneratorBake2D");
+	generator_tasks.insert(generator_task->thread_task_id, generator_task);
+	generator_task_mutex.unlock();
+}
+
+void NavMeshGenerator2D::generator_thread_bake(void *p_arg) {
+	NavMeshGeneratorTask2D *generator_task = static_cast<NavMeshGeneratorTask2D *>(p_arg);
+
+	generator_bake_from_source_geometry_data(generator_task->navigation_mesh, generator_task->source_geometry_data);
+
+	generator_task->status = NavMeshGeneratorTask2D::TaskStatus::BAKING_FINISHED;
+}
+
+void NavMeshGenerator2D::generator_parse_geometry_node(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node, bool p_recurse_children) {
+	generator_parse_meshinstance2d_node(p_navigation_mesh, p_source_geometry_data, p_node);
+	generator_parse_multimeshinstance2d_node(p_navigation_mesh, p_source_geometry_data, p_node);
+	generator_parse_polygon2d_node(p_navigation_mesh, p_source_geometry_data, p_node);
+	generator_parse_staticbody2d_node(p_navigation_mesh, p_source_geometry_data, p_node);
+	generator_parse_tilemap_node(p_navigation_mesh, p_source_geometry_data, p_node);
+
+	if (p_recurse_children) {
+		for (int i = 0; i < p_node->get_child_count(); i++) {
+			generator_parse_geometry_node(p_navigation_mesh, p_source_geometry_data, p_node->get_child(i), p_recurse_children);
+		}
+	}
+}
+
+void NavMeshGenerator2D::generator_parse_meshinstance2d_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node) {
+	MeshInstance2D *mesh_instance = Object::cast_to<MeshInstance2D>(p_node);
+
+	if (mesh_instance == nullptr) {
+		return;
+	}
+
+	NavigationPolygon::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
+
+	if (!(parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_BOTH)) {
+		return;
+	}
+
+	Ref<Mesh> mesh = mesh_instance->get_mesh();
+	if (!mesh.is_valid()) {
+		return;
+	}
+
+	const Transform2D mesh_instance_xform = p_source_geometry_data->root_node_transform * mesh_instance->get_global_transform();
+
+	using namespace Clipper2Lib;
+
+	Paths64 subject_paths, dummy_clip_paths;
+
+	for (int i = 0; i < mesh->get_surface_count(); i++) {
+		if (mesh->surface_get_primitive_type(i) != Mesh::PRIMITIVE_TRIANGLES) {
+			continue;
+		}
+
+		if (!(mesh->surface_get_format(i) & Mesh::ARRAY_FLAG_USE_2D_VERTICES)) {
+			continue;
+		}
+
+		Path64 subject_path;
+
+		int index_count = 0;
+		if (mesh->surface_get_format(i) & Mesh::ARRAY_FORMAT_INDEX) {
+			index_count = mesh->surface_get_array_index_len(i);
+		} else {
+			index_count = mesh->surface_get_array_len(i);
+		}
+
+		ERR_CONTINUE((index_count == 0 || (index_count % 3) != 0));
+
+		Array a = mesh->surface_get_arrays(i);
+
+		Vector<Vector2> mesh_vertices = a[Mesh::ARRAY_VERTEX];
+
+		if (mesh->surface_get_format(i) & Mesh::ARRAY_FORMAT_INDEX) {
+			Vector<int> mesh_indices = a[Mesh::ARRAY_INDEX];
+			for (int vertex_index : mesh_indices) {
+				const Vector2 &vertex = mesh_vertices[vertex_index];
+				const Point64 &point = Point64(vertex.x, vertex.y);
+				subject_path.push_back(point);
+			}
+		} else {
+			for (const Vector2 &vertex : mesh_vertices) {
+				const Point64 &point = Point64(vertex.x, vertex.y);
+				subject_path.push_back(point);
+			}
+		}
+		subject_paths.push_back(subject_path);
+	}
+
+	Paths64 path_solution;
+
+	path_solution = Union(subject_paths, dummy_clip_paths, FillRule::NonZero);
+
+	//path_solution = RamerDouglasPeucker(path_solution, 0.025);
+
+	Vector<Vector<Vector2>> polypaths;
+
+	for (const Path64 &scaled_path : path_solution) {
+		Vector<Vector2> shape_outline;
+		for (const Point64 &scaled_point : scaled_path) {
+			shape_outline.push_back(Point2(static_cast<real_t>(scaled_point.x), static_cast<real_t>(scaled_point.y)));
+		}
+
+		for (int i = 0; i < shape_outline.size(); i++) {
+			shape_outline.write[i] = mesh_instance_xform.xform(shape_outline[i]);
+		}
+
+		p_source_geometry_data->add_obstruction_outline(shape_outline);
+	}
+}
+
+void NavMeshGenerator2D::generator_parse_multimeshinstance2d_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node) {
+	MultiMeshInstance2D *multimesh_instance = Object::cast_to<MultiMeshInstance2D>(p_node);
+
+	if (multimesh_instance == nullptr) {
+		return;
+	}
+
+	NavigationPolygon::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
+	if (!(parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_BOTH)) {
+		return;
+	}
+
+	Ref<MultiMesh> multimesh = multimesh_instance->get_multimesh();
+	if (!(multimesh.is_valid() && multimesh->get_transform_format() == MultiMesh::TRANSFORM_2D)) {
+		return;
+	}
+
+	Ref<Mesh> mesh = multimesh->get_mesh();
+	if (!mesh.is_valid()) {
+		return;
+	}
+
+	using namespace Clipper2Lib;
+
+	Paths64 mesh_subject_paths, dummy_clip_paths;
+
+	for (int i = 0; i < mesh->get_surface_count(); i++) {
+		if (mesh->surface_get_primitive_type(i) != Mesh::PRIMITIVE_TRIANGLES) {
+			continue;
+		}
+
+		if (!(mesh->surface_get_format(i) & Mesh::ARRAY_FLAG_USE_2D_VERTICES)) {
+			continue;
+		}
+
+		Path64 subject_path;
+
+		int index_count = 0;
+		if (mesh->surface_get_format(i) & Mesh::ARRAY_FORMAT_INDEX) {
+			index_count = mesh->surface_get_array_index_len(i);
+		} else {
+			index_count = mesh->surface_get_array_len(i);
+		}
+
+		ERR_CONTINUE((index_count == 0 || (index_count % 3) != 0));
+
+		Array a = mesh->surface_get_arrays(i);
+
+		Vector<Vector2> mesh_vertices = a[Mesh::ARRAY_VERTEX];
+
+		if (mesh->surface_get_format(i) & Mesh::ARRAY_FORMAT_INDEX) {
+			Vector<int> mesh_indices = a[Mesh::ARRAY_INDEX];
+			for (int vertex_index : mesh_indices) {
+				const Vector2 &vertex = mesh_vertices[vertex_index];
+				const Point64 &point = Point64(vertex.x, vertex.y);
+				subject_path.push_back(point);
+			}
+		} else {
+			for (const Vector2 &vertex : mesh_vertices) {
+				const Point64 &point = Point64(vertex.x, vertex.y);
+				subject_path.push_back(point);
+			}
+		}
+		mesh_subject_paths.push_back(subject_path);
+	}
+
+	Paths64 mesh_path_solution = Union(mesh_subject_paths, dummy_clip_paths, FillRule::NonZero);
+
+	//path_solution = RamerDouglasPeucker(path_solution, 0.025);
+
+	int multimesh_instance_count = multimesh->get_visible_instance_count();
+	if (multimesh_instance_count == -1) {
+		multimesh_instance_count = multimesh->get_instance_count();
+	}
+
+	const Transform2D multimesh_instance_xform = p_source_geometry_data->root_node_transform * multimesh_instance->get_global_transform();
+
+	for (int i = 0; i < multimesh_instance_count; i++) {
+		const Transform2D multimesh_instance_mesh_instance_xform = multimesh_instance_xform * multimesh->get_instance_transform_2d(i);
+
+		for (const Path64 &mesh_path : mesh_path_solution) {
+			Vector<Vector2> shape_outline;
+
+			for (const Point64 &mesh_path_point : mesh_path) {
+				shape_outline.push_back(Point2(static_cast<real_t>(mesh_path_point.x), static_cast<real_t>(mesh_path_point.y)));
+			}
+
+			for (int j = 0; j < shape_outline.size(); j++) {
+				shape_outline.write[j] = multimesh_instance_mesh_instance_xform.xform(shape_outline[j]);
+			}
+			p_source_geometry_data->add_obstruction_outline(shape_outline);
+		}
+	}
+}
+
+void NavMeshGenerator2D::generator_parse_polygon2d_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node) {
+	Polygon2D *polygon_2d = Object::cast_to<Polygon2D>(p_node);
+
+	if (polygon_2d == nullptr) {
+		return;
+	}
+
+	NavigationPolygon::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
+
+	if (parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_MESH_INSTANCES || parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_BOTH) {
+		const Transform2D polygon_2d_xform = p_source_geometry_data->root_node_transform * polygon_2d->get_global_transform();
+
+		Vector<Vector2> shape_outline = polygon_2d->get_polygon();
+		for (int i = 0; i < shape_outline.size(); i++) {
+			shape_outline.write[i] = polygon_2d_xform.xform(shape_outline[i]);
+		}
+
+		p_source_geometry_data->add_obstruction_outline(shape_outline);
+	}
+}
+
+void NavMeshGenerator2D::generator_parse_staticbody2d_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node) {
+	StaticBody2D *static_body = Object::cast_to<StaticBody2D>(p_node);
+
+	if (static_body == nullptr) {
+		return;
+	}
+
+	NavigationPolygon::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
+	if (!(parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_STATIC_COLLIDERS || parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_BOTH)) {
+		return;
+	}
+
+	uint32_t parsed_collision_mask = p_navigation_mesh->get_parsed_collision_mask();
+	if (!(static_body->get_collision_layer() & parsed_collision_mask)) {
+		return;
+	}
+
+	List<uint32_t> shape_owners;
+	static_body->get_shape_owners(&shape_owners);
+
+	for (uint32_t shape_owner : shape_owners) {
+		if (static_body->is_shape_owner_disabled(shape_owner)) {
+			continue;
+		}
+
+		const int shape_count = static_body->shape_owner_get_shape_count(shape_owner);
+
+		for (int shape_index = 0; shape_index < shape_count; shape_index++) {
+			Ref<Shape2D> s = static_body->shape_owner_get_shape(shape_owner, shape_index);
+
+			if (s.is_null()) {
+				continue;
+			}
+
+			const Transform2D static_body_xform = p_source_geometry_data->root_node_transform * static_body->get_global_transform() * static_body->shape_owner_get_transform(shape_owner);
+
+			RectangleShape2D *rectangle_shape = Object::cast_to<RectangleShape2D>(*s);
+			if (rectangle_shape) {
+				Vector<Vector2> shape_outline;
+
+				const Vector2 &rectangle_size = rectangle_shape->get_size();
+
+				shape_outline.resize(5);
+				shape_outline.write[0] = static_body_xform.xform(-rectangle_size * 0.5);
+				shape_outline.write[1] = static_body_xform.xform(Vector2(rectangle_size.x, -rectangle_size.y) * 0.5);
+				shape_outline.write[2] = static_body_xform.xform(rectangle_size * 0.5);
+				shape_outline.write[3] = static_body_xform.xform(Vector2(-rectangle_size.x, rectangle_size.y) * 0.5);
+				shape_outline.write[4] = static_body_xform.xform(-rectangle_size * 0.5);
+
+				p_source_geometry_data->add_obstruction_outline(shape_outline);
+			}
+
+			CapsuleShape2D *capsule_shape = Object::cast_to<CapsuleShape2D>(*s);
+			if (capsule_shape) {
+				const real_t capsule_height = capsule_shape->get_height();
+				const real_t capsule_radius = capsule_shape->get_radius();
+
+				Vector<Vector2> shape_outline;
+				const real_t turn_step = Math_TAU / 12.0;
+				shape_outline.resize(14);
+				int shape_outline_inx = 0;
+				for (int i = 0; i < 12; i++) {
+					Vector2 ofs = Vector2(0, (i > 3 && i <= 9) ? -capsule_height * 0.5 + capsule_radius : capsule_height * 0.5 - capsule_radius);
+
+					shape_outline.write[shape_outline_inx] = static_body_xform.xform(Vector2(Math::sin(i * turn_step), Math::cos(i * turn_step)) * capsule_radius + ofs);
+					shape_outline_inx += 1;
+					if (i == 3 || i == 9) {
+						shape_outline.write[shape_outline_inx] = static_body_xform.xform(Vector2(Math::sin(i * turn_step), Math::cos(i * turn_step)) * capsule_radius - ofs);
+						shape_outline_inx += 1;
+					}
+				}
+
+				p_source_geometry_data->add_obstruction_outline(shape_outline);
+			}
+
+			CircleShape2D *circle_shape = Object::cast_to<CircleShape2D>(*s);
+			if (circle_shape) {
+				const real_t circle_radius = circle_shape->get_radius();
+
+				Vector<Vector2> shape_outline;
+				int circle_edge_count = 12;
+				shape_outline.resize(circle_edge_count);
+
+				const real_t turn_step = Math_TAU / real_t(circle_edge_count);
+				for (int i = 0; i < circle_edge_count; i++) {
+					shape_outline.write[i] = static_body_xform.xform(Vector2(Math::cos(i * turn_step), Math::sin(i * turn_step)) * circle_radius);
+				}
+
+				p_source_geometry_data->add_obstruction_outline(shape_outline);
+			}
+
+			ConcavePolygonShape2D *concave_polygon_shape = Object::cast_to<ConcavePolygonShape2D>(*s);
+			if (concave_polygon_shape) {
+				Vector<Vector2> shape_outline = concave_polygon_shape->get_segments();
+
+				for (int i = 0; i < shape_outline.size(); i++) {
+					shape_outline.write[i] = static_body_xform.xform(shape_outline[i]);
+				}
+
+				p_source_geometry_data->add_obstruction_outline(shape_outline);
+			}
+
+			ConvexPolygonShape2D *convex_polygon_shape = Object::cast_to<ConvexPolygonShape2D>(*s);
+			if (convex_polygon_shape) {
+				Vector<Vector2> shape_outline = convex_polygon_shape->get_points();
+
+				for (int i = 0; i < shape_outline.size(); i++) {
+					shape_outline.write[i] = static_body_xform.xform(shape_outline[i]);
+				}
+
+				p_source_geometry_data->add_obstruction_outline(shape_outline);
+			}
+		}
+	}
+}
+
+void NavMeshGenerator2D::generator_parse_tilemap_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node) {
+	TileMap *tilemap = Object::cast_to<TileMap>(p_node);
+
+	if (tilemap == nullptr) {
+		return;
+	}
+
+	NavigationPolygon::ParsedGeometryType parsed_geometry_type = p_navigation_mesh->get_parsed_geometry_type();
+	uint32_t parsed_collision_mask = p_navigation_mesh->get_parsed_collision_mask();
+
+	if (tilemap->get_layers_count() <= 0) {
+		return;
+	}
+
+	int tilemap_layer = 0; // only main tile map layer is supported
+
+	Ref<TileSet> tile_set = tilemap->get_tileset();
+	if (!tile_set.is_valid()) {
+		return;
+	}
+
+	int physics_layers_count = tile_set->get_physics_layers_count();
+	int navigation_layers_count = tile_set->get_navigation_layers_count();
+
+	if (physics_layers_count <= 0 && navigation_layers_count <= 0) {
+		return;
+	}
+
+	const Transform2D tilemap_xform = p_source_geometry_data->root_node_transform * tilemap->get_global_transform();
+	TypedArray<Vector2i> used_cells = tilemap->get_used_cells(tilemap_layer);
+
+	for (int used_cell_index = 0; used_cell_index < used_cells.size(); used_cell_index++) {
+		const Vector2i &cell = used_cells[used_cell_index];
+
+		const TileData *tile_data = tilemap->get_cell_tile_data(tilemap_layer, cell, false);
+
+		Transform2D tile_transform;
+		tile_transform.set_origin(tilemap->map_to_local(cell));
+
+		const Transform2D tile_transform_offset = tilemap_xform * tile_transform;
+
+		if (navigation_layers_count > 0) {
+			Ref<NavigationPolygon> navigation_polygon = tile_data->get_navigation_polygon(tilemap_layer);
+			if (navigation_polygon.is_valid()) {
+				for (int outline_index = 0; outline_index < navigation_polygon->get_outline_count(); outline_index++) {
+					Vector<Vector2> traversable_outline = navigation_polygon->get_outline(outline_index);
+
+					for (int traversable_outline_index = 0; traversable_outline_index < traversable_outline.size(); traversable_outline_index++) {
+						traversable_outline.write[traversable_outline_index] = tile_transform_offset.xform(traversable_outline[traversable_outline_index]);
+					}
+
+					p_source_geometry_data->_add_traversable_outline(traversable_outline);
+				}
+			}
+		}
+
+		if (physics_layers_count > 0 && (parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_STATIC_COLLIDERS || parsed_geometry_type == NavigationPolygon::PARSED_GEOMETRY_BOTH) && (tile_set->get_physics_layer_collision_layer(tilemap_layer) & parsed_collision_mask)) {
+			for (int collision_polygon_index = 0; collision_polygon_index < tile_data->get_collision_polygons_count(tilemap_layer); collision_polygon_index++) {
+				Vector<Vector2> obstruction_outline = tile_data->get_collision_polygon_points(tilemap_layer, collision_polygon_index);
+
+				for (int obstruction_outline_index = 0; obstruction_outline_index < obstruction_outline.size(); obstruction_outline_index++) {
+					obstruction_outline.write[obstruction_outline_index] = tile_transform_offset.xform(obstruction_outline[obstruction_outline_index]);
+				}
+
+				p_source_geometry_data->_add_obstruction_outline(obstruction_outline);
+			}
+		}
+	}
+}
+
+void NavMeshGenerator2D::generator_parse_source_geometry_data(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_root_node) {
+	List<Node *> parse_nodes;
+
+	if (p_navigation_mesh->get_source_geometry_mode() == NavigationPolygon::SOURCE_GEOMETRY_ROOT_NODE_CHILDREN) {
+		parse_nodes.push_back(p_root_node);
+	} else {
+		p_root_node->get_tree()->get_nodes_in_group(p_navigation_mesh->get_source_geometry_group_name(), &parse_nodes);
+	}
+
+	Transform2D root_node_transform = Transform2D();
+	if (Object::cast_to<Node2D>(p_root_node)) {
+		root_node_transform = Object::cast_to<Node2D>(p_root_node)->get_global_transform().affine_inverse();
+	}
+
+	p_source_geometry_data->clear();
+	p_source_geometry_data->root_node_transform = root_node_transform;
+
+	bool recurse_children = p_navigation_mesh->get_source_geometry_mode() != NavigationPolygon::SOURCE_GEOMETRY_GROUPS_EXPLICIT;
+
+	for (Node *E : parse_nodes) {
+		generator_parse_geometry_node(p_navigation_mesh, p_source_geometry_data, E, recurse_children);
+	}
+};
+
+static void generator_recursive_process_polytree_items(List<TPPLPoly> &p_tppl_in_polygon, const Clipper2Lib::PolyPath64 *p_polypath_item) {
+	using namespace Clipper2Lib;
+
+	Vector<Vector2> polygon_vertices;
+
+	for (const Point64 &polypath_point : p_polypath_item->Polygon()) {
+		polygon_vertices.push_back(Vector2(static_cast<real_t>(polypath_point.x), static_cast<real_t>(polypath_point.y)));
+	}
+
+	TPPLPoly tp;
+	tp.Init(polygon_vertices.size());
+	for (int j = 0; j < polygon_vertices.size(); j++) {
+		tp[j] = polygon_vertices[j];
+	}
+
+	if (p_polypath_item->IsHole()) {
+		tp.SetOrientation(TPPL_ORIENTATION_CW);
+		tp.SetHole(true);
+	} else {
+		tp.SetOrientation(TPPL_ORIENTATION_CCW);
+	}
+	p_tppl_in_polygon.push_back(tp);
+
+	for (size_t i = 0; i < p_polypath_item->Count(); i++) {
+		const PolyPath64 *polypath_item = p_polypath_item->Child(i);
+		generator_recursive_process_polytree_items(p_tppl_in_polygon, polypath_item);
+	}
+}
+
+bool NavMeshGenerator2D::generator_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;
+}
+
+void NavMeshGenerator2D::generator_bake_from_source_geometry_data(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data) {
+	if (p_navigation_mesh.is_null() || p_source_geometry_data.is_null()) {
+		return;
+	}
+
+	if (p_navigation_mesh->get_outline_count() == 0 && !p_source_geometry_data->has_data()) {
+		return;
+	}
+
+	int outline_count = p_navigation_mesh->get_outline_count();
+	const Vector<Vector<Vector2>> &traversable_outlines = p_source_geometry_data->_get_traversable_outlines();
+	const Vector<Vector<Vector2>> &obstruction_outlines = p_source_geometry_data->_get_obstruction_outlines();
+
+	if (outline_count == 0 && traversable_outlines.size() == 0) {
+		return;
+	}
+
+	using namespace Clipper2Lib;
+
+	Paths64 traversable_polygon_paths;
+	Paths64 obstruction_polygon_paths;
+
+	for (int i = 0; i < outline_count; i++) {
+		const Vector<Vector2> &traversable_outline = p_navigation_mesh->get_outline(i);
+		Path64 subject_path;
+		for (const Vector2 &traversable_point : traversable_outline) {
+			const Point64 &point = Point64(traversable_point.x, traversable_point.y);
+			subject_path.push_back(point);
+		}
+		traversable_polygon_paths.push_back(subject_path);
+	}
+
+	for (const Vector<Vector2> &traversable_outline : traversable_outlines) {
+		Path64 subject_path;
+		for (const Vector2 &traversable_point : traversable_outline) {
+			const Point64 &point = Point64(traversable_point.x, traversable_point.y);
+			subject_path.push_back(point);
+		}
+		traversable_polygon_paths.push_back(subject_path);
+	}
+
+	for (const Vector<Vector2> &obstruction_outline : obstruction_outlines) {
+		Path64 clip_path;
+		for (const Vector2 &obstruction_point : obstruction_outline) {
+			const Point64 &point = Point64(obstruction_point.x, obstruction_point.y);
+			clip_path.push_back(point);
+		}
+		obstruction_polygon_paths.push_back(clip_path);
+	}
+
+	Paths64 path_solution;
+
+	// first merge all traversable polygons according to user specified fill rule
+	Paths64 dummy_clip_path;
+	traversable_polygon_paths = Union(traversable_polygon_paths, dummy_clip_path, FillRule::NonZero);
+	// merge all obstruction polygons, don't allow holes for what is considered "solid" 2D geometry
+	obstruction_polygon_paths = Union(obstruction_polygon_paths, dummy_clip_path, FillRule::NonZero);
+
+	path_solution = Difference(traversable_polygon_paths, obstruction_polygon_paths, FillRule::NonZero);
+
+	real_t agent_radius_offset = p_navigation_mesh->get_agent_radius();
+	if (agent_radius_offset > 0.0) {
+		path_solution = InflatePaths(path_solution, -agent_radius_offset, JoinType::Miter, EndType::Polygon);
+	}
+	//path_solution = RamerDouglasPeucker(path_solution, 0.025); //
+
+	Vector<Vector<Vector2>> new_baked_outlines;
+
+	for (const Path64 &scaled_path : path_solution) {
+		Vector<Vector2> polypath;
+		for (const Point64 &scaled_point : scaled_path) {
+			polypath.push_back(Vector2(static_cast<real_t>(scaled_point.x), static_cast<real_t>(scaled_point.y)));
+		}
+		new_baked_outlines.push_back(polypath);
+	}
+
+	if (new_baked_outlines.size() == 0) {
+		p_navigation_mesh->set_vertices(Vector<Vector2>());
+		p_navigation_mesh->clear_polygons();
+		return;
+	}
+
+	Paths64 polygon_paths;
+
+	for (const Vector<Vector2> &baked_outline : new_baked_outlines) {
+		Path64 polygon_path;
+		for (const Vector2 &baked_outline_point : baked_outline) {
+			const Point64 &point = Point64(baked_outline_point.x, baked_outline_point.y);
+			polygon_path.push_back(point);
+		}
+		polygon_paths.push_back(polygon_path);
+	}
+
+	ClipType clipper_cliptype = ClipType::Union;
+
+	List<TPPLPoly> tppl_in_polygon, tppl_out_polygon;
+
+	PolyTree64 polytree;
+	Clipper64 clipper_64;
+
+	clipper_64.AddSubject(polygon_paths);
+	clipper_64.Execute(clipper_cliptype, FillRule::NonZero, polytree);
+
+	for (size_t i = 0; i < polytree.Count(); i++) {
+		const PolyPath64 *polypath_item = polytree[i];
+		generator_recursive_process_polytree_items(tppl_in_polygon, polypath_item);
+	}
+
+	TPPLPartition tpart;
+	if (tpart.ConvexPartition_HM(&tppl_in_polygon, &tppl_out_polygon) == 0) { //failed!
+		ERR_PRINT("NavigationPolygon Convex partition failed. Unable to create a valid NavigationMesh from defined polygon outline paths.");
+		p_navigation_mesh->set_vertices(Vector<Vector2>());
+		p_navigation_mesh->clear_polygons();
+		return;
+	}
+
+	Vector<Vector2> new_vertices;
+	Vector<Vector<int>> new_polygons;
+
+	HashMap<Vector2, int> points;
+	for (List<TPPLPoly>::Element *I = tppl_out_polygon.front(); I; I = I->next()) {
+		TPPLPoly &tp = I->get();
+
+		Vector<int> new_polygon;
+
+		for (int64_t i = 0; i < tp.GetNumPoints(); i++) {
+			HashMap<Vector2, int>::Iterator E = points.find(tp[i]);
+			if (!E) {
+				E = points.insert(tp[i], new_vertices.size());
+				new_vertices.push_back(tp[i]);
+			}
+			new_polygon.push_back(E->value);
+		}
+
+		new_polygons.push_back(new_polygon);
+	}
+
+	p_navigation_mesh->set_vertices(new_vertices);
+	p_navigation_mesh->clear_polygons();
+	for (int i = 0; i < new_polygons.size(); i++) {
+		p_navigation_mesh->add_polygon(new_polygons[i]);
+	}
+}
diff --git a/modules/navigation/nav_mesh_generator_2d.h b/modules/navigation/nav_mesh_generator_2d.h
new file mode 100644
index 0000000000..763ad24636
--- /dev/null
+++ b/modules/navigation/nav_mesh_generator_2d.h
@@ -0,0 +1,100 @@
+/**************************************************************************/
+/*  nav_mesh_generator_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.                 */
+/**************************************************************************/
+
+#ifndef NAV_MESH_GENERATOR_2D_H
+#define NAV_MESH_GENERATOR_2D_H
+
+#include "core/object/class_db.h"
+#include "core/object/worker_thread_pool.h"
+
+class Node;
+class NavigationPolygon;
+class NavigationMeshSourceGeometryData2D;
+
+class NavMeshGenerator2D : public Object {
+	static NavMeshGenerator2D *singleton;
+
+	static Mutex baking_navmesh_mutex;
+	static Mutex generator_task_mutex;
+
+	static bool use_threads;
+	static bool baking_use_multiple_threads;
+	static bool baking_use_high_priority_threads;
+
+	struct NavMeshGeneratorTask2D {
+		enum TaskStatus {
+			BAKING_STARTED,
+			BAKING_FINISHED,
+			BAKING_FAILED,
+			CALLBACK_DISPATCHED,
+			CALLBACK_FAILED,
+		};
+
+		Ref<NavigationPolygon> navigation_mesh;
+		Ref<NavigationMeshSourceGeometryData2D> source_geometry_data;
+		Callable callback;
+		WorkerThreadPool::TaskID thread_task_id = WorkerThreadPool::INVALID_TASK_ID;
+		NavMeshGeneratorTask2D::TaskStatus status = NavMeshGeneratorTask2D::TaskStatus::BAKING_STARTED;
+	};
+
+	static HashMap<WorkerThreadPool::TaskID, NavMeshGeneratorTask2D *> generator_tasks;
+
+	static void generator_thread_bake(void *p_arg);
+
+	static HashSet<Ref<NavigationPolygon>> baking_navmeshes;
+
+	static void generator_parse_geometry_node(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node, bool p_recurse_children);
+	static void generator_parse_source_geometry_data(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_root_node);
+	static void generator_bake_from_source_geometry_data(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data);
+
+	static void generator_parse_meshinstance2d_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
+	static void generator_parse_multimeshinstance2d_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
+	static void generator_parse_polygon2d_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
+	static void generator_parse_staticbody2d_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
+	static void generator_parse_tilemap_node(const Ref<NavigationPolygon> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_node);
+
+	static bool generator_emit_callback(const Callable &p_callback);
+
+public:
+	static NavMeshGenerator2D *get_singleton();
+
+	static void sync();
+	static void cleanup();
+	static void finish();
+
+	static void parse_source_geometry_data(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, Node *p_root_node, const Callable &p_callback = Callable());
+	static void bake_from_source_geometry_data(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, const Callable &p_callback = Callable());
+	static void bake_from_source_geometry_data_async(Ref<NavigationPolygon> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData2D> p_source_geometry_data, const Callable &p_callback = Callable());
+
+	NavMeshGenerator2D();
+	~NavMeshGenerator2D();
+};
+
+#endif // NAV_MESH_GENERATOR_2D_H
diff --git a/modules/navigation/register_types.cpp b/modules/navigation/register_types.cpp
index 1548ff4b9c..525fe71134 100644
--- a/modules/navigation/register_types.cpp
+++ b/modules/navigation/register_types.cpp
@@ -31,6 +31,7 @@
 #include "register_types.h"
 
 #include "godot_navigation_server.h"
+#include "godot_navigation_server_2d.h"
 
 #ifndef DISABLE_DEPRECATED
 #ifndef _3D_DISABLED
@@ -43,6 +44,7 @@
 #endif
 
 #include "core/config/engine.h"
+#include "servers/navigation_server_2d.h"
 #include "servers/navigation_server_3d.h"
 
 #ifndef DISABLE_DEPRECATED
@@ -55,9 +57,14 @@ NavigationServer3D *new_server() {
 	return memnew(GodotNavigationServer);
 }
 
+NavigationServer2D *new_navigation_server_2d() {
+	return memnew(GodotNavigationServer2D);
+}
+
 void initialize_navigation_module(ModuleInitializationLevel p_level) {
 	if (p_level == MODULE_INITIALIZATION_LEVEL_SERVERS) {
 		NavigationServer3DManager::set_default_server(new_server);
+		NavigationServer2DManager::set_default_server(new_navigation_server_2d);
 
 #ifndef DISABLE_DEPRECATED
 #ifndef _3D_DISABLED