initial commit, 4.5 stable

modules/navigation_2d/2d/godot_navigation_server_2d.h

@@ -0,0 +1,355 @@
+/**************************************************************************/
+/*  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.                 */
+/**************************************************************************/
+
+#pragma once
+
+#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;
+
+#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;
+	COMMAND_2(map_set_active, RID, p_map, bool, p_active);
+	virtual bool map_is_active(RID p_map) const 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;
+
+	COMMAND_2(map_set_merge_rasterizer_cell_scale, RID, p_map, float, p_value);
+	virtual float map_get_merge_rasterizer_cell_scale(RID p_map) const 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;
+
+	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;
+
+	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 uint32_t map_get_iteration_id(RID p_map) const 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 uint32_t region_get_iteration_id(RID p_region) const override;
+
+	COMMAND_2(region_set_use_async_iterations, RID, p_region, bool, p_enabled);
+	virtual bool region_get_use_async_iterations(RID p_region) const override;
+
+	COMMAND_2(region_set_enabled, RID, p_region, bool, p_enabled);
+	virtual bool region_get_enabled(RID p_region) const 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;
+
+	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;
+	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;
+
+	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;
+
+	COMMAND_2(region_set_map, RID, p_region, RID, p_map);
+	virtual RID region_get_map(RID p_region) const 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;
+	COMMAND_2(region_set_transform, RID, p_region, Transform2D, p_transform);
+	virtual Transform2D region_get_transform(RID p_region) const 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;
+	virtual Vector2 region_get_closest_point(RID p_region, const Vector2 &p_point) const override;
+	virtual Vector2 region_get_random_point(RID p_region, uint32_t p_navigation_layers, bool p_uniformly) const override;
+	virtual Rect2 region_get_bounds(RID p_region) const override;
+
+	virtual RID link_create() override;
+	virtual uint32_t link_get_iteration_id(RID p_link) const override;
+
+	/// Set the map of this link.
+	COMMAND_2(link_set_map, RID, p_link, RID, p_map);
+	virtual RID link_get_map(RID p_link) const 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.
+	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.
+	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.
+	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.
+	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.
+	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.
+	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.
+	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.
+	COMMAND_2(agent_set_map, RID, p_agent, RID, p_map);
+	virtual RID agent_get_map(RID p_agent) const override;
+
+	COMMAND_2(agent_set_paused, RID, p_agent, bool, p_paused);
+	virtual bool agent_get_paused(RID p_agent) const 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
+	/// 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.
+	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
+	/// 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.
+	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
+	/// 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.
+	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;
+	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.
+	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.
+	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
+	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'.
+	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.
+	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
+	COMMAND_2(agent_set_avoidance_callback, RID, p_agent, Callable, p_callback);
+	virtual bool agent_has_avoidance_callback(RID p_agent) const 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;
+
+	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;
+
+	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;
+	COMMAND_2(obstacle_set_avoidance_enabled, RID, p_obstacle, bool, p_enabled);
+	virtual bool obstacle_get_avoidance_enabled(RID p_obstacle) const override;
+	COMMAND_2(obstacle_set_map, RID, p_obstacle, RID, p_map);
+	virtual RID obstacle_get_map(RID p_obstacle) const override;
+	COMMAND_2(obstacle_set_paused, RID, p_obstacle, bool, p_paused);
+	virtual bool obstacle_get_paused(RID p_obstacle) const override;
+	COMMAND_2(obstacle_set_radius, RID, p_obstacle, real_t, p_radius);
+	virtual real_t obstacle_get_radius(RID p_obstacle) const override;
+	COMMAND_2(obstacle_set_velocity, RID, p_obstacle, Vector2, p_velocity);
+	virtual Vector2 obstacle_get_velocity(RID p_obstacle) const override;
+	COMMAND_2(obstacle_set_position, RID, p_obstacle, Vector2, p_position);
+	virtual Vector2 obstacle_get_position(RID p_obstacle) const 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;
+	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 = Callable()) override;
+
+	COMMAND_1(free, RID, p_object);
+
+	virtual void set_active(bool p_active) override;
+
+	void flush_queries();
+
+	virtual void process(double p_delta_time) override;
+	virtual void physics_process(double p_delta_time) override;
+	virtual void init() override;
+	virtual void sync() override;
+	virtual void finish() 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;
+	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;
+	virtual bool is_baking_navigation_polygon(Ref<NavigationPolygon> p_navigation_polygon) const override;
+
+	virtual RID source_geometry_parser_create() override;
+	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,68 @@
+/**************************************************************************/
+/*  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 "core/object/ref_counted.h"
+#include "servers/navigation/navigation_utilities.h"
+
+class NavBaseIteration2D : public RefCounted {
+	GDCLASS(NavBaseIteration2D, RefCounted);
+
+public:
+	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<Nav2D::Polygon> navmesh_polygons;
+	LocalVector<LocalVector<Nav2D::Connection>> internal_connections;
+
+	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<Nav2D::Polygon> &get_navmesh_polygons() const { return navmesh_polygons; }
+	const LocalVector<LocalVector<Nav2D::Connection>> &get_internal_connections() const { return internal_connections; }
+
+	virtual ~NavBaseIteration2D() {
+		navmesh_polygons.clear();
+		internal_connections.clear();
+	}
+};

modules/navigation_2d/2d/nav_map_builder_2d.cpp

@@ -0,0 +1,425 @@
+/**************************************************************************/
+/*  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 Nav2D;
+
+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;
+
+	const LocalVector<Ref<NavRegionIteration2D>> &regions = map_iteration->region_iterations;
+	HashMap<const NavBaseIteration2D *, LocalVector<Connection>> &region_external_connections = map_iteration->external_region_connections;
+
+	map_iteration->navbases_polygons_external_connections.clear();
+
+	// Remove regions connections.
+	region_external_connections.clear();
+
+	// Copy all region polygons in the map.
+	int polygon_count = 0;
+	for (const Ref<NavRegionIteration2D> &region : regions) {
+		const uint32_t polygons_size = region->navmesh_polygons.size();
+		polygon_count += polygons_size;
+
+		region_external_connections[region.ptr()] = LocalVector<Connection>();
+		map_iteration->navbases_polygons_external_connections[region.ptr()] = LocalVector<LocalVector<Connection>>();
+		map_iteration->navbases_polygons_external_connections[region.ptr()].resize(polygons_size);
+	}
+
+	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 (const Ref<NavRegionIteration2D> &region : map_iteration->region_iterations) {
+		for (const ConnectableEdge &connectable_edge : region->get_external_edges()) {
+			const EdgeKey &ek = connectable_edge.ek;
+
+			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.
+				Connection new_connection;
+				new_connection.polygon = &region->navmesh_polygons[connectable_edge.polygon_index];
+				new_connection.edge = connectable_edge.edge;
+				new_connection.pathway_start = connectable_edge.pathway_start;
+				new_connection.pathway_end = connectable_edge.pathway_end;
+
+				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/2d/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<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);
+
+	NavMapIteration2D *map_iteration = r_build.map_iteration;
+
+	HashMap<const NavBaseIteration2D *, LocalVector<LocalVector<Nav2D::Connection>>> &navbases_polygons_external_connections = map_iteration->navbases_polygons_external_connections;
+
+	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 Connection &c1 = pair.connections[0];
+			const Connection &c2 = pair.connections[1];
+
+			navbases_polygons_external_connections[c1.polygon->owner][c1.polygon->id].push_back(c2);
+			navbases_polygons_external_connections[c2.polygon->owner][c2.polygon->id].push_back(c1);
+			performance_data.pm_edge_connection_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<Connection> &free_edges = r_build.iter_free_edges;
+	HashMap<const NavBaseIteration2D *, LocalVector<Connection>> &region_external_connections = map_iteration->external_region_connections;
+
+	HashMap<const NavBaseIteration2D *, LocalVector<LocalVector<Nav2D::Connection>>> &navbases_polygons_external_connections = map_iteration->navbases_polygons_external_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 Connection &free_edge = free_edges[i];
+		const Vector2 &edge_p1 = free_edge.pathway_start;
+		const Vector2 &edge_p2 = free_edge.pathway_end;
+
+		for (uint32_t j = 0; j < free_edges.size(); j++) {
+			const Connection &other_edge = free_edges[j];
+			if (i == j || free_edge.polygon->owner == other_edge.polygon->owner) {
+				continue;
+			}
+
+			const Vector2 &other_edge_p1 = other_edge.pathway_start;
+			const Vector2 &other_edge_p2 = other_edge.pathway_end;
+
+			// 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.
+			Connection new_connection = other_edge;
+			new_connection.pathway_start = (self1 + other1) / 2.0;
+			new_connection.pathway_end = (self2 + other2) / 2.0;
+			//free_edge.polygon->connections.push_back(new_connection);
+
+			// Add the connection to the region_connection map.
+			region_external_connections[free_edge.polygon->owner].push_back(new_connection);
+			navbases_polygons_external_connections[free_edge.polygon->owner][free_edge.polygon->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;
+
+	const LocalVector<Ref<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;
+
+	HashMap<const NavBaseIteration2D *, LocalVector<LocalVector<Nav2D::Connection>>> &navbases_polygons_external_connections = map_iteration->navbases_polygons_external_connections;
+	LocalVector<Nav2D::Polygon> &navlink_polygons = map_iteration->navlink_polygons;
+	navlink_polygons.clear();
+	navlink_polygons.resize(links.size());
+	uint32_t navlink_index = 0;
+
+	// Search for polygons within range of a nav link.
+	for (const Ref<NavLinkIteration2D> &link : links) {
+		polygon_count++;
+		Polygon &new_polygon = navlink_polygons[navlink_index++];
+
+		new_polygon.id = 0;
+		new_polygon.owner = link.ptr();
+
+		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 (const Ref<NavRegionIteration2D> &region : map_iteration->region_iterations) {
+			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.vertices.size(); point_id += 1) {
+					const Triangle2 triangle(polyon.vertices[0], polyon.vertices[point_id - 1], polyon.vertices[point_id]);
+
+					{
+						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) {
+			new_polygon.vertices.resize(4);
+
+			// Build a set of vertices that create a thin polygon going from the start to the end point.
+			new_polygon.vertices[0] = closest_start_point;
+			new_polygon.vertices[1] = closest_start_point;
+			new_polygon.vertices[2] = closest_end_point;
+			new_polygon.vertices[3] = closest_end_point;
+
+			// Setup connections to go forward in the link.
+			{
+				Connection entry_connection;
+				entry_connection.polygon = &new_polygon;
+				entry_connection.edge = -1;
+				entry_connection.pathway_start = new_polygon.vertices[0];
+				entry_connection.pathway_end = new_polygon.vertices[1];
+				navbases_polygons_external_connections[closest_start_polygon->owner][closest_start_polygon->id].push_back(entry_connection);
+
+				Connection exit_connection;
+				exit_connection.polygon = closest_end_polygon;
+				exit_connection.edge = -1;
+				exit_connection.pathway_start = new_polygon.vertices[2];
+				exit_connection.pathway_end = new_polygon.vertices[3];
+				navbases_polygons_external_connections[link.ptr()].push_back(LocalVector<Nav2D::Connection>());
+				navbases_polygons_external_connections[link.ptr()][new_polygon.id].push_back(exit_connection);
+			}
+
+			// If the link is bi-directional, create connections from the end to the start.
+			if (link->is_bidirectional()) {
+				Connection entry_connection;
+				entry_connection.polygon = &new_polygon;
+				entry_connection.edge = -1;
+				entry_connection.pathway_start = new_polygon.vertices[2];
+				entry_connection.pathway_end = new_polygon.vertices[3];
+				navbases_polygons_external_connections[closest_end_polygon->owner][closest_end_polygon->id].push_back(entry_connection);
+
+				Connection exit_connection;
+				exit_connection.polygon = closest_start_polygon;
+				exit_connection.edge = -1;
+				exit_connection.pathway_start = new_polygon.vertices[0];
+				exit_connection.pathway_end = new_polygon.vertices[1];
+				navbases_polygons_external_connections[link.ptr()].push_back(LocalVector<Nav2D::Connection>());
+				navbases_polygons_external_connections[link.ptr()][new_polygon.id].push_back(exit_connection);
+			}
+		}
+	}
+
+	r_build.polygon_count = polygon_count;
+}
+
+void NavMapBuilder2D::_build_update_map_iteration(NavMapIterationBuild2D &r_build) {
+	NavMapIteration2D *map_iteration = r_build.map_iteration;
+
+	map_iteration->navmesh_polygon_count = r_build.polygon_count;
+
+	uint32_t navmesh_polygon_count = r_build.polygon_count;
+	uint32_t total_polygon_count = navmesh_polygon_count;
+
+	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(navmesh_polygon_count * 0.25);
+		p_path_query_slot.path_corridor.clear();
+
+		p_path_query_slot.path_corridor.resize(total_polygon_count);
+
+		p_path_query_slot.poly_to_id.clear();
+		p_path_query_slot.poly_to_id.reserve(total_polygon_count);
+
+		int polygon_id = 0;
+		for (Ref<NavRegionIteration2D> &region : map_iteration->region_iterations) {
+			for (const Polygon &polygon : region->navmesh_polygons) {
+				p_path_query_slot.poly_to_id[&polygon] = polygon_id;
+				polygon_id++;
+			}
+		}
+
+		for (const Polygon &polygon : map_iteration->navlink_polygons) {
+			p_path_query_slot.poly_to_id[&polygon] = polygon_id;
+			polygon_id++;
+		}
+
+		DEV_ASSERT(p_path_query_slot.path_corridor.size() == p_path_query_slot.poly_to_id.size());
+	}
+
+	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 Nav2D::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,119 @@
+/**************************************************************************/
+/*  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"
+
+class NavLinkIteration2D;
+class NavRegion2D;
+class 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;
+	Nav2D::PerformanceData performance_data;
+	int polygon_count = 0;
+	int free_edge_count = 0;
+
+	HashMap<Nav2D::EdgeKey, Nav2D::EdgeConnectionPair, Nav2D::EdgeKey> iter_connection_pairs_map;
+	LocalVector<Nav2D::Connection> iter_free_edges;
+
+	NavMapIteration2D *map_iteration = nullptr;
+
+	int navmesh_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;
+	}
+};
+
+struct NavMapIteration2D {
+	mutable SafeNumeric<uint32_t> users;
+	RWLock rwlock;
+
+	LocalVector<Ref<NavRegionIteration2D>> region_iterations;
+	LocalVector<Ref<NavLinkIteration2D>> link_iterations;
+
+	int navmesh_polygon_count = 0;
+
+	// The edge connections that the map builds on top with the edge connection margin.
+	HashMap<const NavBaseIteration2D *, LocalVector<Nav2D::Connection>> external_region_connections;
+	HashMap<const NavBaseIteration2D *, LocalVector<LocalVector<Nav2D::Connection>>> navbases_polygons_external_connections;
+
+	LocalVector<Nav2D::Polygon> navlink_polygons;
+
+	HashMap<NavRegion2D *, Ref<NavRegionIteration2D>> region_ptr_to_region_iteration;
+
+	LocalVector<NavMeshQueries2D::PathQuerySlot> path_query_slots;
+	Mutex path_query_slots_mutex;
+	Semaphore path_query_slots_semaphore;
+
+	void clear() {
+		navmesh_polygon_count = 0;
+
+		region_iterations.clear();
+		link_iterations.clear();
+		external_region_connections.clear();
+		navbases_polygons_external_connections.clear();
+		navlink_polygons.clear();
+		region_ptr_to_region_iteration.clear();
+	}
+};
+
+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_generator_2d.cpp

@@ -0,0 +1,528 @@
+/**************************************************************************/
+/*  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.                 */
+/**************************************************************************/
+
+#ifdef CLIPPER2_ENABLED
+
+#include "nav_mesh_generator_2d.h"
+
+#include "core/config/project_settings.h"
+#include "scene/resources/2d/navigation_mesh_source_geometry_data_2d.h"
+#include "scene/resources/2d/navigation_polygon.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;
+RWLock NavMeshGenerator2D::generator_parsers_rwlock;
+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;
+LocalVector<NavMeshGeometryParser2D *> NavMeshGenerator2D::generator_parsers;
+
+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;
+}
+
+NavMeshGenerator2D::~NavMeshGenerator2D() {
+	cleanup();
+}
+
+void NavMeshGenerator2D::sync() {
+	if (generator_tasks.is_empty()) {
+		return;
+	}
+
+	MutexLock baking_navmesh_lock(baking_navmesh_mutex);
+	{
+		MutexLock generator_task_lock(generator_task_mutex);
+
+		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);
+				}
+				generator_task->navigation_mesh->emit_changed();
+				memdelete(generator_task);
+			}
+		}
+
+		for (WorkerThreadPool::TaskID finished_task_id : finished_task_ids) {
+			generator_tasks.erase(finished_task_id);
+		}
+	}
+}
+
+void NavMeshGenerator2D::cleanup() {
+	MutexLock baking_navmesh_lock(baking_navmesh_mutex);
+	{
+		MutexLock generator_task_lock(generator_task_mutex);
+
+		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_parsers_rwlock.write_lock();
+		generator_parsers.clear();
+		generator_parsers_rwlock.write_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_null());
+	ERR_FAIL_NULL(p_root_node);
+	ERR_FAIL_COND(!p_root_node->is_inside_tree());
+	ERR_FAIL_COND(p_source_geometry_data.is_null());
+
+	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_null());
+	ERR_FAIL_COND(p_source_geometry_data.is_null());
+
+	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);
+		}
+		p_navigation_mesh->emit_changed();
+		return;
+	}
+
+	if (is_baking(p_navigation_mesh)) {
+		ERR_FAIL_MSG("NavigationPolygon is already baking. Wait for current bake to finish.");
+	}
+	baking_navmesh_mutex.lock();
+	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);
+	}
+
+	p_navigation_mesh->emit_changed();
+}
+
+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_null());
+	ERR_FAIL_COND(p_source_geometry_data.is_null());
+
+	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);
+		}
+		p_navigation_mesh->emit_changed();
+		return;
+	}
+
+	if (!use_threads) {
+		bake_from_source_geometry_data(p_navigation_mesh, p_source_geometry_data, p_callback);
+		return;
+	}
+
+	if (is_baking(p_navigation_mesh)) {
+		ERR_FAIL_MSG("NavigationPolygon is already baking. Wait for current bake to finish.");
+	}
+	baking_navmesh_mutex.lock();
+	baking_navmeshes.insert(p_navigation_mesh);
+	baking_navmesh_mutex.unlock();
+
+	MutexLock generator_task_lock(generator_task_mutex);
+	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);
+}
+
+bool NavMeshGenerator2D::is_baking(Ref<NavigationPolygon> p_navigation_polygon) {
+	MutexLock baking_navmesh_lock(baking_navmesh_mutex);
+	return baking_navmeshes.has(p_navigation_polygon);
+}
+
+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_parsers_rwlock.read_lock();
+	for (const NavMeshGeometryParser2D *parser : generator_parsers) {
+		if (!parser->callback.is_valid()) {
+			continue;
+		}
+		parser->callback.call(p_navigation_mesh, p_source_geometry_data, p_node);
+	}
+	generator_parsers_rwlock.read_unlock();
+
+	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::set_generator_parsers(LocalVector<NavMeshGeometryParser2D *> p_parsers) {
+	RWLockWrite write_lock(generator_parsers_rwlock);
+	generator_parsers = p_parsers;
+}
+
+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::PolyPathD *p_polypath_item) {
+	using namespace Clipper2Lib;
+
+	TPPLPoly tp;
+	int size = p_polypath_item->Polygon().size();
+	tp.Init(size);
+
+	int j = 0;
+	for (const PointD &polypath_point : p_polypath_item->Polygon()) {
+		tp[j] = Vector2(static_cast<real_t>(polypath_point.x), static_cast<real_t>(polypath_point.y));
+		++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 PolyPathD *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;
+	}
+
+	using namespace Clipper2Lib;
+	PathsD traversable_polygon_paths;
+	PathsD obstruction_polygon_paths;
+	bool empty_projected_obstructions = true;
+	{
+		RWLockRead read_lock(p_source_geometry_data->geometry_rwlock);
+
+		const Vector<Vector<Vector2>> &traversable_outlines = p_source_geometry_data->traversable_outlines;
+		int outline_count = p_navigation_mesh->get_outline_count();
+
+		if (outline_count == 0 && (!p_source_geometry_data->has_data() || (traversable_outlines.is_empty()))) {
+			return;
+		}
+
+		const Vector<Vector<Vector2>> &obstruction_outlines = p_source_geometry_data->obstruction_outlines;
+		const Vector<NavigationMeshSourceGeometryData2D::ProjectedObstruction> &projected_obstructions = p_source_geometry_data->_projected_obstructions;
+
+		traversable_polygon_paths.reserve(outline_count + traversable_outlines.size());
+		obstruction_polygon_paths.reserve(obstruction_outlines.size());
+
+		for (int i = 0; i < outline_count; i++) {
+			const Vector<Vector2> &traversable_outline = p_navigation_mesh->get_outline(i);
+			PathD subject_path;
+			subject_path.reserve(traversable_outline.size());
+			for (const Vector2 &traversable_point : traversable_outline) {
+				subject_path.emplace_back(traversable_point.x, traversable_point.y);
+			}
+			traversable_polygon_paths.push_back(std::move(subject_path));
+		}
+
+		for (const Vector<Vector2> &traversable_outline : traversable_outlines) {
+			PathD subject_path;
+			subject_path.reserve(traversable_outline.size());
+			for (const Vector2 &traversable_point : traversable_outline) {
+				subject_path.emplace_back(traversable_point.x, traversable_point.y);
+			}
+			traversable_polygon_paths.push_back(std::move(subject_path));
+		}
+
+		empty_projected_obstructions = projected_obstructions.is_empty();
+		if (!empty_projected_obstructions) {
+			for (const NavigationMeshSourceGeometryData2D::ProjectedObstruction &projected_obstruction : projected_obstructions) {
+				if (projected_obstruction.carve) {
+					continue;
+				}
+				if (projected_obstruction.vertices.is_empty() || projected_obstruction.vertices.size() % 2 != 0) {
+					continue;
+				}
+
+				PathD clip_path;
+				clip_path.reserve(projected_obstruction.vertices.size() / 2);
+				for (int i = 0; i < projected_obstruction.vertices.size() / 2; i++) {
+					clip_path.emplace_back(projected_obstruction.vertices[i * 2], projected_obstruction.vertices[i * 2 + 1]);
+				}
+				if (!IsPositive(clip_path)) {
+					std::reverse(clip_path.begin(), clip_path.end());
+				}
+				obstruction_polygon_paths.push_back(std::move(clip_path));
+			}
+		}
+
+		for (const Vector<Vector2> &obstruction_outline : obstruction_outlines) {
+			PathD clip_path;
+			clip_path.reserve(obstruction_outline.size());
+			for (const Vector2 &obstruction_point : obstruction_outline) {
+				clip_path.emplace_back(obstruction_point.x, obstruction_point.y);
+			}
+			obstruction_polygon_paths.push_back(std::move(clip_path));
+		}
+	}
+
+	Rect2 baking_rect = p_navigation_mesh->get_baking_rect();
+	if (baking_rect.has_area()) {
+		Vector2 baking_rect_offset = p_navigation_mesh->get_baking_rect_offset();
+
+		const int rect_begin_x = baking_rect.position[0] + baking_rect_offset.x;
+		const int rect_begin_y = baking_rect.position[1] + baking_rect_offset.y;
+		const int rect_end_x = baking_rect.position[0] + baking_rect.size[0] + baking_rect_offset.x;
+		const int rect_end_y = baking_rect.position[1] + baking_rect.size[1] + baking_rect_offset.y;
+
+		RectD clipper_rect = RectD(rect_begin_x, rect_begin_y, rect_end_x, rect_end_y);
+
+		traversable_polygon_paths = RectClip(clipper_rect, traversable_polygon_paths);
+		obstruction_polygon_paths = RectClip(clipper_rect, obstruction_polygon_paths);
+	}
+
+	// first merge all traversable polygons according to user specified fill rule
+	PathsD 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);
+
+	PathsD 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);
+	}
+
+	// Apply obstructions that are not affected by agent radius, the ones with carve enabled.
+	if (!empty_projected_obstructions) {
+		RWLockRead read_lock(p_source_geometry_data->geometry_rwlock);
+		const Vector<NavigationMeshSourceGeometryData2D::ProjectedObstruction> &projected_obstructions = p_source_geometry_data->_projected_obstructions;
+		obstruction_polygon_paths.resize(0);
+		for (const NavigationMeshSourceGeometryData2D::ProjectedObstruction &projected_obstruction : projected_obstructions) {
+			if (!projected_obstruction.carve) {
+				continue;
+			}
+			if (projected_obstruction.vertices.is_empty() || projected_obstruction.vertices.size() % 2 != 0) {
+				continue;
+			}
+
+			PathD clip_path;
+			clip_path.reserve(projected_obstruction.vertices.size() / 2);
+			for (int i = 0; i < projected_obstruction.vertices.size() / 2; i++) {
+				clip_path.emplace_back(projected_obstruction.vertices[i * 2], projected_obstruction.vertices[i * 2 + 1]);
+			}
+			if (!IsPositive(clip_path)) {
+				std::reverse(clip_path.begin(), clip_path.end());
+			}
+			obstruction_polygon_paths.push_back(std::move(clip_path));
+		}
+		if (obstruction_polygon_paths.size() > 0) {
+			path_solution = Difference(path_solution, obstruction_polygon_paths, FillRule::NonZero);
+		}
+	}
+
+	//path_solution = RamerDouglasPeucker(path_solution, 0.025); //
+
+	real_t border_size = p_navigation_mesh->get_border_size();
+	if (baking_rect.has_area() && border_size > 0.0) {
+		Vector2 baking_rect_offset = p_navigation_mesh->get_baking_rect_offset();
+
+		const int rect_begin_x = baking_rect.position[0] + baking_rect_offset.x + border_size;
+		const int rect_begin_y = baking_rect.position[1] + baking_rect_offset.y + border_size;
+		const int rect_end_x = baking_rect.position[0] + baking_rect.size[0] + baking_rect_offset.x - border_size;
+		const int rect_end_y = baking_rect.position[1] + baking_rect.size[1] + baking_rect_offset.y - border_size;
+
+		RectD clipper_rect = RectD(rect_begin_x, rect_begin_y, rect_end_x, rect_end_y);
+
+		path_solution = RectClip(clipper_rect, path_solution);
+	}
+
+	if (path_solution.size() == 0) {
+		p_navigation_mesh->clear();
+		return;
+	}
+
+	ClipType clipper_cliptype = ClipType::Union;
+
+	List<TPPLPoly> tppl_in_polygon, tppl_out_polygon;
+
+	PolyTreeD polytree;
+	ClipperD clipper_D;
+
+	clipper_D.AddSubject(path_solution);
+	clipper_D.Execute(clipper_cliptype, FillRule::NonZero, polytree);
+
+	for (size_t i = 0; i < polytree.Count(); i++) {
+		const PolyPathD *polypath_item = polytree[i];
+		generator_recursive_process_polytree_items(tppl_in_polygon, polypath_item);
+	}
+
+	TPPLPartition tpart;
+
+	NavigationPolygon::SamplePartitionType sample_partition_type = p_navigation_mesh->get_sample_partition_type();
+
+	switch (sample_partition_type) {
+		case NavigationPolygon::SamplePartitionType::SAMPLE_PARTITION_CONVEX_PARTITION:
+			if (tpart.ConvexPartition_HM(&tppl_in_polygon, &tppl_out_polygon) == 0) {
+				ERR_PRINT("NavigationPolygon polygon convex partition failed. Unable to create a valid navigation mesh polygon layout from provided source geometry.");
+				p_navigation_mesh->set_vertices(Vector<Vector2>());
+				p_navigation_mesh->clear_polygons();
+				return;
+			}
+			break;
+		case NavigationPolygon::SamplePartitionType::SAMPLE_PARTITION_TRIANGULATE:
+			if (tpart.Triangulate_EC(&tppl_in_polygon, &tppl_out_polygon) == 0) {
+				ERR_PRINT("NavigationPolygon polygon triangulation failed. Unable to create a valid navigation mesh polygon layout from provided source geometry.");
+				p_navigation_mesh->set_vertices(Vector<Vector2>());
+				p_navigation_mesh->clear_polygons();
+				return;
+			}
+			break;
+		default: {
+			ERR_PRINT("NavigationPolygon polygon partitioning failed. Unrecognized partition type.");
+			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 (const TPPLPoly &tp : tppl_out_polygon) {
+		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_data(new_vertices, new_polygons);
+}
+
+#endif // CLIPPER2_ENABLED

modules/navigation_2d/2d/nav_mesh_generator_2d.h

@@ -0,0 +1,103 @@
+/**************************************************************************/
+/*  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.                 */
+/**************************************************************************/
+
+#pragma once
+
+#ifdef CLIPPER2_ENABLED
+
+#include "core/object/class_db.h"
+#include "core/object/worker_thread_pool.h"
+#include "core/templates/rid_owner.h"
+#include "servers/navigation_server_2d.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 RWLock generator_parsers_rwlock;
+	static LocalVector<NavMeshGeometryParser2D *> generator_parsers;
+
+	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 bool generator_emit_callback(const Callable &p_callback);
+
+public:
+	static NavMeshGenerator2D *get_singleton();
+
+	static void sync();
+	static void cleanup();
+	static void finish();
+
+	static void set_generator_parsers(LocalVector<NavMeshGeometryParser2D *> p_parsers);
+
+	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());
+	static bool is_baking(Ref<NavigationPolygon> p_navigation_polygon);
+
+	NavMeshGenerator2D();
+	~NavMeshGenerator2D();
+};
+
+#endif // CLIPPER2_ENABLED

modules/navigation_2d/2d/nav_mesh_queries_2d.h

@@ -0,0 +1,157 @@
+/**************************************************************************/
+/*  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 "core/templates/a_hash_map.h"
+
+#include "servers/navigation/navigation_globals.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<Nav2D::NavigationPoly> path_corridor;
+		Heap<Nav2D::NavigationPoly *, Nav2D::NavPolyTravelCostGreaterThan, Nav2D::NavPolyHeapIndexer> traversable_polys;
+		bool in_use = false;
+		uint32_t slot_index = 0;
+		AHashMap<const Nav2D::Polygon *, uint32_t> poly_to_id;
+	};
+
+	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;
+
+		float path_return_max_length = 0.0;
+		float path_return_max_radius = 0.0;
+		int path_search_max_polygons = NavigationDefaults2D::path_search_max_polygons;
+		float path_search_max_distance = 0.0;
+
+		// Path building.
+		Vector2 begin_position;
+		Vector2 end_position;
+		const Nav2D::Polygon *begin_polygon = nullptr;
+		const Nav2D::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;
+		float path_length = 0.0;
+
+		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.reverse();
+			path_meta_point_types.reverse();
+			path_meta_point_rids.reverse();
+			path_meta_point_owners.reverse();
+		}
+	};
+
+	static bool emit_callback(const Callable &p_callback);
+
+	static Vector2 polygons_get_random_point(const LocalVector<Nav2D::Polygon> &p_polygons, uint32_t p_navigation_layers, bool p_uniformly);
+
+	static Vector2 polygons_get_closest_point(const LocalVector<Nav2D::Polygon> &p_polygons, const Vector2 &p_point);
+	static Nav2D::ClosestPointQueryResult polygons_get_closest_point_info(const LocalVector<Nav2D::Polygon> &p_polygons, const Vector2 &p_point);
+	static RID polygons_get_closest_point_owner(const LocalVector<Nav2D::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 Nav2D::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 Nav2D::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, const NavMapIteration2D &p_map_iteration);
+	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 Nav2D::NavigationPoly *p_from_poly, const Vector2 &p_to_point, const Nav2D::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 _query_task_process_path_result_limits(NavMeshPathQueryTask2D &p_query_task);
+
+	static void _query_task_search_polygon_connections(NavMeshPathQueryTask2D &p_query_task, const Nav2D::Connection &p_connection, uint32_t p_least_cost_id, const Nav2D::NavigationPoly &p_least_cost_poly, real_t p_poly_enter_cost, const Vector2 &p_end_point);
+
+	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);
+
+	static float _calculate_path_length(const LocalVector<Vector2> &p_path, uint32_t p_start_index, uint32_t p_end_index);
+};

modules/navigation_2d/2d/nav_region_builder_2d.cpp

@@ -0,0 +1,256 @@
+/**************************************************************************/
+/*  nav_region_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_region_builder_2d.h"
+
+#include "../nav_map_2d.h"
+#include "../nav_region_2d.h"
+#include "../triangle2.h"
+#include "nav_region_iteration_2d.h"
+
+using namespace Nav2D;
+
+void NavRegionBuilder2D::build_iteration(NavRegionIterationBuild2D &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_process_navmesh_data(r_build);
+
+	_build_step_find_edge_connection_pairs(r_build);
+
+	_build_step_merge_edge_connection_pairs(r_build);
+
+	_build_update_iteration(r_build);
+}
+
+void NavRegionBuilder2D::_build_step_process_navmesh_data(NavRegionIterationBuild2D &r_build) {
+	Vector<Vector2> _navmesh_vertices = r_build.navmesh_data.vertices;
+	Vector<Vector<int>> _navmesh_polygons = r_build.navmesh_data.polygons;
+
+	if (_navmesh_vertices.is_empty() || _navmesh_polygons.is_empty()) {
+		return;
+	}
+
+	PerformanceData &performance_data = r_build.performance_data;
+	Ref<NavRegionIteration2D> region_iteration = r_build.region_iteration;
+
+	const Transform2D &region_transform = region_iteration->transform;
+	LocalVector<Nav2D::Polygon> &navmesh_polygons = region_iteration->navmesh_polygons;
+
+	const int vertex_count = _navmesh_vertices.size();
+
+	const Vector2 *vertices_ptr = _navmesh_vertices.ptr();
+	const Vector<int> *polygons_ptr = _navmesh_polygons.ptr();
+
+	navmesh_polygons.resize(_navmesh_polygons.size());
+
+	real_t _new_region_surface_area = 0.0;
+	Rect2 _new_region_bounds;
+
+	bool first_vertex = true;
+
+	for (uint32_t i = 0; i < navmesh_polygons.size(); i++) {
+		Polygon &polygon = navmesh_polygons[i];
+		polygon.id = i;
+		polygon.owner = region_iteration.ptr();
+		polygon.surface_area = 0.0;
+
+		Vector<int> polygon_indices = polygons_ptr[i];
+
+		int polygon_size = polygon_indices.size();
+		if (polygon_size < 3) {
+			continue;
+		}
+
+		const int *indices_ptr = polygon_indices.ptr();
+
+		bool polygon_valid = true;
+
+		polygon.vertices.resize(polygon_size);
+
+		{
+			real_t _new_polygon_surface_area = 0.0;
+
+			for (int j(2); j < polygon_size; j++) {
+				const Triangle2 triangle = Triangle2(
+						region_transform.xform(vertices_ptr[indices_ptr[0]]),
+						region_transform.xform(vertices_ptr[indices_ptr[j - 1]]),
+						region_transform.xform(vertices_ptr[indices_ptr[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 < polygon_size; j++) {
+			int vertex_index = indices_ptr[j];
+			if (vertex_index < 0 || vertex_index >= vertex_count) {
+				polygon_valid = false;
+				break;
+			}
+
+			const Vector2 point_position = region_transform.xform(vertices_ptr[vertex_index]);
+			polygon.vertices[j] = point_position;
+
+			if (first_vertex) {
+				first_vertex = false;
+				_new_region_bounds.position = point_position;
+			} else {
+				_new_region_bounds.expand_to(point_position);
+			}
+		}
+
+		if (!polygon_valid) {
+			polygon.surface_area = 0.0;
+			polygon.vertices.clear();
+			ERR_FAIL_COND_MSG(!polygon_valid, "Corrupted navigation mesh set on region. The indices of a polygon are out of range.");
+		}
+	}
+
+	region_iteration->surface_area = _new_region_surface_area;
+	region_iteration->bounds = _new_region_bounds;
+
+	performance_data.pm_polygon_count = navmesh_polygons.size();
+}
+
+Nav2D::PointKey NavRegionBuilder2D::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;
+}
+
+Nav2D::EdgeKey NavRegionBuilder2D::get_edge_key(const Vector2 &p_vertex1, const Vector2 &p_vertex2, const Vector2 &p_cell_size) {
+	EdgeKey ek(get_point_key(p_vertex1, p_cell_size), get_point_key(p_vertex2, p_cell_size));
+	return ek;
+}
+
+void NavRegionBuilder2D::_build_step_find_edge_connection_pairs(NavRegionIterationBuild2D &r_build) {
+	PerformanceData &performance_data = r_build.performance_data;
+
+	const Vector2 &map_cell_size = r_build.map_cell_size;
+	Ref<NavRegionIteration2D> region_iteration = r_build.region_iteration;
+	LocalVector<Nav2D::Polygon> &navmesh_polygons = region_iteration->navmesh_polygons;
+
+	HashMap<EdgeKey, EdgeConnectionPair, EdgeKey> &connection_pairs_map = r_build.iter_connection_pairs_map;
+	connection_pairs_map.clear();
+
+	region_iteration->internal_connections.clear();
+	region_iteration->internal_connections.resize(navmesh_polygons.size());
+
+	region_iteration->external_edges.clear();
+
+	int free_edges_count = 0;
+
+	for (Polygon &poly : region_iteration->navmesh_polygons) {
+		for (uint32_t p = 0; p < poly.vertices.size(); p++) {
+			const int next_point = (p + 1) % poly.vertices.size();
+			const EdgeKey ek = get_edge_key(poly.vertices[p], poly.vertices[next_point], map_cell_size);
+
+			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.
+				Connection new_connection;
+				new_connection.polygon = &poly;
+				new_connection.edge = p;
+				new_connection.pathway_start = poly.vertices[p];
+				new_connection.pathway_end = poly.vertices[next_point];
+
+				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_FAIL_COND_MSG(pair.size >= 2, "Navigation region synchronization error. More than 2 edges tried to occupy the same map rasterization space. This is a logical error in the navigation mesh caused by overlap or too densely placed edges.");
+			}
+		}
+	}
+
+	performance_data.pm_edge_free_count = free_edges_count;
+}
+
+void NavRegionBuilder2D::_build_step_merge_edge_connection_pairs(NavRegionIterationBuild2D &r_build) {
+	PerformanceData &performance_data = r_build.performance_data;
+
+	Ref<NavRegionIteration2D> region_iteration = r_build.region_iteration;
+
+	HashMap<EdgeKey, EdgeConnectionPair, EdgeKey> &connection_pairs_map = r_build.iter_connection_pairs_map;
+
+	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 Connection &c1 = pair.connections[0];
+			const Connection &c2 = pair.connections[1];
+			region_iteration->internal_connections[c1.polygon->id].push_back(c2);
+			region_iteration->internal_connections[c2.polygon->id].push_back(c1);
+			performance_data.pm_edge_merge_count += 1;
+
+		} else {
+			ERR_FAIL_COND_MSG(pair.size != 1, vformat("Number of connection != 1. Found: %d", pair.size));
+
+			const Connection &connection = pair.connections[0];
+
+			ConnectableEdge ce;
+			ce.ek = pair_it.key;
+			ce.polygon_index = connection.polygon->id;
+			ce.edge = connection.edge;
+			ce.pathway_start = connection.pathway_start;
+			ce.pathway_end = connection.pathway_end;
+
+			region_iteration->external_edges.push_back(ce);
+		}
+	}
+}
+
+void NavRegionBuilder2D::_build_update_iteration(NavRegionIterationBuild2D &r_build) {
+	ERR_FAIL_NULL(r_build.region);
+	// Stub. End of the build.
+}

modules/navigation_2d/2d/nav_region_builder_2d.h

@@ -0,0 +1,48 @@
+/**************************************************************************/
+/*  nav_region_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 NavRegionIterationBuild2D;
+
+class NavRegionBuilder2D {
+	static void _build_step_process_navmesh_data(NavRegionIterationBuild2D &r_build);
+	static void _build_step_find_edge_connection_pairs(NavRegionIterationBuild2D &r_build);
+	static void _build_step_merge_edge_connection_pairs(NavRegionIterationBuild2D &r_build);
+	static void _build_update_iteration(NavRegionIterationBuild2D &r_build);
+
+public:
+	static Nav2D::PointKey get_point_key(const Vector2 &p_pos, const Vector2 &p_cell_size);
+	static Nav2D::EdgeKey get_edge_key(const Vector2 &p_vertex1, const Vector2 &p_vertex2, const Vector2 &p_cell_size);
+
+	static void build_iteration(NavRegionIterationBuild2D &r_build);
+};

modules/navigation_2d/2d/nav_region_iteration_2d.h

@@ -0,0 +1,92 @@
+/**************************************************************************/
+/*  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 "scene/resources/2d/navigation_polygon.h"
+
+#include "core/math/rect2.h"
+
+class NavRegion2D;
+class NavRegionIteration2D;
+
+struct NavRegionIterationBuild2D {
+	Nav2D::PerformanceData performance_data;
+
+	NavRegion2D *region = nullptr;
+
+	Vector2 map_cell_size;
+	Transform2D region_transform;
+
+	struct NavMeshData {
+		Vector<Vector2> vertices;
+		Vector<Vector<int>> polygons;
+
+		void clear() {
+			vertices.clear();
+			polygons.clear();
+		}
+	} navmesh_data;
+
+	Ref<NavRegionIteration2D> region_iteration;
+
+	HashMap<Nav2D::EdgeKey, Nav2D::EdgeConnectionPair, Nav2D::EdgeKey> iter_connection_pairs_map;
+
+	void reset() {
+		performance_data.reset();
+
+		navmesh_data.clear();
+		region_iteration = Ref<NavRegionIteration2D>();
+		iter_connection_pairs_map.clear();
+	}
+};
+
+class NavRegionIteration2D : public NavBaseIteration2D {
+	GDCLASS(NavRegionIteration2D, NavBaseIteration2D);
+
+public:
+	Transform2D transform;
+	real_t surface_area = 0.0;
+	Rect2 bounds;
+	LocalVector<Nav2D::ConnectableEdge> external_edges;
+
+	const Transform2D &get_transform() const { return transform; }
+	real_t get_surface_area() const { return surface_area; }
+	Rect2 get_bounds() const { return bounds; }
+	const LocalVector<Nav2D::ConnectableEdge> &get_external_edges() const { return external_edges; }
+
+	virtual ~NavRegionIteration2D() override {
+		external_edges.clear();
+		navmesh_polygons.clear();
+		internal_connections.clear();
+	}
+};

modules/navigation_2d/SCsub

@@ -0,0 +1,44 @@
+#!/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(CPPEXTPATH=[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")
+if env.editor_build:
+    env_navigation_2d.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.
+env.Depends(module_obj, thirdparty_obj)

modules/navigation_2d/config.py

@@ -0,0 +1,6 @@
+def can_build(env, platform):
+    return not env["disable_navigation_2d"]
+
+
+def configure(env):
+    pass

modules/navigation_2d/editor/navigation_link_2d_editor_plugin.cpp

@@ -0,0 +1,198 @@
+/**************************************************************************/
+/*  navigation_link_2d_editor_plugin.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 "navigation_link_2d_editor_plugin.h"
+
+#include "editor/editor_node.h"
+#include "editor/editor_undo_redo_manager.h"
+#include "editor/scene/canvas_item_editor_plugin.h"
+#include "editor/settings/editor_settings.h"
+#include "scene/main/viewport.h"
+
+void NavigationLink2DEditor::_notification(int p_what) {
+	switch (p_what) {
+		case NOTIFICATION_ENTER_TREE: {
+			get_tree()->connect("node_removed", callable_mp(this, &NavigationLink2DEditor::_node_removed));
+		} break;
+
+		case NOTIFICATION_EXIT_TREE: {
+			get_tree()->disconnect("node_removed", callable_mp(this, &NavigationLink2DEditor::_node_removed));
+		} break;
+	}
+}
+
+void NavigationLink2DEditor::_node_removed(Node *p_node) {
+	if (p_node == node) {
+		node = nullptr;
+	}
+}
+
+bool NavigationLink2DEditor::forward_canvas_gui_input(const Ref<InputEvent> &p_event) {
+	if (!node || !node->is_visible_in_tree()) {
+		return false;
+	}
+
+	Viewport *vp = node->get_viewport();
+	if (vp && !vp->is_visible_subviewport()) {
+		return false;
+	}
+
+	real_t grab_threshold = EDITOR_GET("editors/polygon_editor/point_grab_radius");
+	Transform2D xform = canvas_item_editor->get_canvas_transform() * node->get_screen_transform();
+
+	Ref<InputEventMouseButton> mb = p_event;
+	if (mb.is_valid() && mb->get_button_index() == MouseButton::LEFT) {
+		if (mb->is_pressed()) {
+			// Start position
+			if (xform.xform(node->get_start_position()).distance_to(mb->get_position()) < grab_threshold) {
+				start_grabbed = true;
+				original_start_position = node->get_start_position();
+
+				return true;
+			} else {
+				start_grabbed = false;
+			}
+
+			// End position
+			if (xform.xform(node->get_end_position()).distance_to(mb->get_position()) < grab_threshold) {
+				end_grabbed = true;
+				original_end_position = node->get_end_position();
+
+				return true;
+			} else {
+				end_grabbed = false;
+			}
+		} else {
+			EditorUndoRedoManager *undo_redo = EditorUndoRedoManager::get_singleton();
+			if (start_grabbed) {
+				undo_redo->create_action(TTR("Set start_position"));
+				undo_redo->add_do_method(node, "set_start_position", node->get_start_position());
+				undo_redo->add_do_method(canvas_item_editor, "update_viewport");
+				undo_redo->add_undo_method(node, "set_start_position", original_start_position);
+				undo_redo->add_undo_method(canvas_item_editor, "update_viewport");
+				undo_redo->commit_action();
+
+				start_grabbed = false;
+
+				return true;
+			}
+
+			if (end_grabbed) {
+				undo_redo->create_action(TTR("Set end_position"));
+				undo_redo->add_do_method(node, "set_end_position", node->get_end_position());
+				undo_redo->add_do_method(canvas_item_editor, "update_viewport");
+				undo_redo->add_undo_method(node, "set_end_position", original_end_position);
+				undo_redo->add_undo_method(canvas_item_editor, "update_viewport");
+				undo_redo->commit_action();
+
+				end_grabbed = false;
+
+				return true;
+			}
+		}
+	}
+
+	Ref<InputEventMouseMotion> mm = p_event;
+	if (mm.is_valid()) {
+		Vector2 point = canvas_item_editor->snap_point(canvas_item_editor->get_canvas_transform().affine_inverse().xform(mm->get_position()));
+		point = node->get_screen_transform().affine_inverse().xform(point);
+
+		if (start_grabbed) {
+			node->set_start_position(point);
+			canvas_item_editor->update_viewport();
+
+			return true;
+		}
+
+		if (end_grabbed) {
+			node->set_end_position(point);
+			canvas_item_editor->update_viewport();
+
+			return true;
+		}
+	}
+
+	return false;
+}
+
+void NavigationLink2DEditor::forward_canvas_draw_over_viewport(Control *p_overlay) {
+	if (!node || !node->is_visible_in_tree()) {
+		return;
+	}
+
+	Viewport *vp = node->get_viewport();
+	if (vp && !vp->is_visible_subviewport()) {
+		return;
+	}
+
+	Transform2D gt = canvas_item_editor->get_canvas_transform() * node->get_screen_transform();
+	Vector2 global_start_position = gt.xform(node->get_start_position());
+	Vector2 global_end_position = gt.xform(node->get_end_position());
+
+	// Only drawing the handles here, since the debug rendering will fill in the rest.
+	const Ref<Texture2D> handle = get_editor_theme_icon(SNAME("EditorHandle"));
+	p_overlay->draw_texture(handle, global_start_position - handle->get_size() / 2);
+	p_overlay->draw_texture(handle, global_end_position - handle->get_size() / 2);
+}
+
+void NavigationLink2DEditor::edit(NavigationLink2D *p_node) {
+	if (!canvas_item_editor) {
+		canvas_item_editor = CanvasItemEditor::get_singleton();
+	}
+
+	if (p_node) {
+		node = p_node;
+	} else {
+		node = nullptr;
+	}
+
+	canvas_item_editor->update_viewport();
+}
+
+///////////////////////
+
+void NavigationLink2DEditorPlugin::edit(Object *p_object) {
+	editor->edit(Object::cast_to<NavigationLink2D>(p_object));
+}
+
+bool NavigationLink2DEditorPlugin::handles(Object *p_object) const {
+	return Object::cast_to<NavigationLink2D>(p_object) != nullptr;
+}
+
+void NavigationLink2DEditorPlugin::make_visible(bool p_visible) {
+	if (!p_visible) {
+		edit(nullptr);
+	}
+}
+
+NavigationLink2DEditorPlugin::NavigationLink2DEditorPlugin() {
+	editor = memnew(NavigationLink2DEditor);
+	EditorNode::get_singleton()->get_gui_base()->add_child(editor);
+}

modules/navigation_2d/editor/navigation_link_2d_editor_plugin.h

@@ -0,0 +1,76 @@
+/**************************************************************************/
+/*  navigation_link_2d_editor_plugin.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 "editor/plugins/editor_plugin.h"
+#include "scene/2d/navigation/navigation_link_2d.h"
+
+class CanvasItemEditor;
+
+class NavigationLink2DEditor : public Control {
+	GDCLASS(NavigationLink2DEditor, Control);
+
+	CanvasItemEditor *canvas_item_editor = nullptr;
+	NavigationLink2D *node = nullptr;
+
+	bool start_grabbed = false;
+	Vector2 original_start_position;
+
+	bool end_grabbed = false;
+	Vector2 original_end_position;
+
+protected:
+	void _notification(int p_what);
+	void _node_removed(Node *p_node);
+
+public:
+	bool forward_canvas_gui_input(const Ref<InputEvent> &p_event);
+	void forward_canvas_draw_over_viewport(Control *p_overlay);
+	void edit(NavigationLink2D *p_node);
+};
+
+class NavigationLink2DEditorPlugin : public EditorPlugin {
+	GDCLASS(NavigationLink2DEditorPlugin, EditorPlugin);
+
+	NavigationLink2DEditor *editor = nullptr;
+
+public:
+	virtual bool forward_canvas_gui_input(const Ref<InputEvent> &p_event) override { return editor->forward_canvas_gui_input(p_event); }
+	virtual void forward_canvas_draw_over_viewport(Control *p_overlay) override { editor->forward_canvas_draw_over_viewport(p_overlay); }
+
+	virtual String get_plugin_name() const override { return "NavigationLink2D"; }
+	bool has_main_screen() const override { return false; }
+	virtual void edit(Object *p_object) override;
+	virtual bool handles(Object *p_object) const override;
+	virtual void make_visible(bool p_visible) override;
+
+	NavigationLink2DEditorPlugin();
+};

modules/navigation_2d/editor/navigation_obstacle_2d_editor_plugin.cpp

@@ -0,0 +1,71 @@
+/**************************************************************************/
+/*  navigation_obstacle_2d_editor_plugin.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 "navigation_obstacle_2d_editor_plugin.h"
+
+#include "editor/editor_undo_redo_manager.h"
+
+Node2D *NavigationObstacle2DEditor::_get_node() const {
+	return node;
+}
+
+void NavigationObstacle2DEditor::_set_node(Node *p_polygon) {
+	node = Object::cast_to<NavigationObstacle2D>(p_polygon);
+}
+
+Variant NavigationObstacle2DEditor::_get_polygon(int p_idx) const {
+	return node->get_vertices();
+}
+
+void NavigationObstacle2DEditor::_set_polygon(int p_idx, const Variant &p_polygon) const {
+	node->set_vertices(p_polygon);
+}
+
+void NavigationObstacle2DEditor::_action_add_polygon(const Variant &p_polygon) {
+	EditorUndoRedoManager *undo_redo = EditorUndoRedoManager::get_singleton();
+	undo_redo->add_do_method(node, "set_vertices", p_polygon);
+	undo_redo->add_undo_method(node, "set_vertices", node->get_vertices());
+}
+
+void NavigationObstacle2DEditor::_action_remove_polygon(int p_idx) {
+	EditorUndoRedoManager *undo_redo = EditorUndoRedoManager::get_singleton();
+	undo_redo->add_do_method(node, "set_vertices", Variant(Vector<Vector2>()));
+	undo_redo->add_undo_method(node, "set_vertices", node->get_vertices());
+}
+
+void NavigationObstacle2DEditor::_action_set_polygon(int p_idx, const Variant &p_previous, const Variant &p_polygon) {
+	EditorUndoRedoManager *undo_redo = EditorUndoRedoManager::get_singleton();
+	undo_redo->add_do_method(node, "set_vertices", p_polygon);
+	undo_redo->add_undo_method(node, "set_vertices", node->get_vertices());
+}
+
+NavigationObstacle2DEditorPlugin::NavigationObstacle2DEditorPlugin() :
+		AbstractPolygon2DEditorPlugin(memnew(NavigationObstacle2DEditor), "NavigationObstacle2D") {
+}

modules/navigation_2d/editor/navigation_obstacle_2d_editor_plugin.h

@@ -0,0 +1,58 @@
+/**************************************************************************/
+/*  navigation_obstacle_2d_editor_plugin.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 "editor/scene/2d/abstract_polygon_2d_editor.h"
+#include "scene/2d/navigation/navigation_obstacle_2d.h"
+
+class NavigationObstacle2DEditor : public AbstractPolygon2DEditor {
+	GDCLASS(NavigationObstacle2DEditor, AbstractPolygon2DEditor);
+
+	NavigationObstacle2D *node = nullptr;
+
+protected:
+	virtual Node2D *_get_node() const override;
+	virtual void _set_node(Node *p_polygon) override;
+
+	virtual Variant _get_polygon(int p_idx) const override;
+	virtual void _set_polygon(int p_idx, const Variant &p_polygon) const override;
+
+	virtual void _action_add_polygon(const Variant &p_polygon) override;
+	virtual void _action_remove_polygon(int p_idx) override;
+	virtual void _action_set_polygon(int p_idx, const Variant &p_previous, const Variant &p_polygon) override;
+};
+
+class NavigationObstacle2DEditorPlugin : public AbstractPolygon2DEditorPlugin {
+	GDCLASS(NavigationObstacle2DEditorPlugin, AbstractPolygon2DEditorPlugin);
+
+public:
+	NavigationObstacle2DEditorPlugin();
+};

modules/navigation_2d/editor/navigation_region_2d_editor_plugin.cpp

@@ -0,0 +1,261 @@
+/**************************************************************************/
+/*  navigation_region_2d_editor_plugin.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 "navigation_region_2d_editor_plugin.h"
+
+#include "editor/editor_node.h"
+#include "editor/editor_undo_redo_manager.h"
+#include "editor/settings/editor_settings.h"
+#include "scene/2d/navigation/navigation_region_2d.h"
+#include "scene/gui/dialogs.h"
+
+Ref<NavigationPolygon> NavigationRegion2DEditor::_ensure_navpoly() const {
+	Ref<NavigationPolygon> navpoly = node->get_navigation_polygon();
+	if (navpoly.is_null()) {
+		navpoly.instantiate();
+		node->set_navigation_polygon(navpoly);
+	}
+	return navpoly;
+}
+
+Node2D *NavigationRegion2DEditor::_get_node() const {
+	return node;
+}
+
+void NavigationRegion2DEditor::_set_node(Node *p_polygon) {
+	node = Object::cast_to<NavigationRegion2D>(p_polygon);
+	if (node) {
+		Ref<NavigationPolygon> navpoly = node->get_navigation_polygon();
+		if (navpoly.is_valid() && navpoly->get_outline_count() > 0 && navpoly->get_polygon_count() == 0) {
+			// We have outlines drawn / added by the user but no polygons were created for this navmesh yet so let's bake once immediately.
+			_rebake_timer_timeout();
+		}
+	}
+}
+
+int NavigationRegion2DEditor::_get_polygon_count() const {
+	Ref<NavigationPolygon> navpoly = node->get_navigation_polygon();
+	if (navpoly.is_valid()) {
+		return navpoly->get_outline_count();
+	} else {
+		return 0;
+	}
+}
+
+Variant NavigationRegion2DEditor::_get_polygon(int p_idx) const {
+	Ref<NavigationPolygon> navpoly = node->get_navigation_polygon();
+	if (navpoly.is_valid()) {
+		return navpoly->get_outline(p_idx);
+	} else {
+		return Variant(Vector<Vector2>());
+	}
+}
+
+void NavigationRegion2DEditor::_set_polygon(int p_idx, const Variant &p_polygon) const {
+	Ref<NavigationPolygon> navpoly = _ensure_navpoly();
+	navpoly->set_outline(p_idx, p_polygon);
+
+	if (rebake_timer && _rebake_timer_delay >= 0.0) {
+		rebake_timer->start();
+	}
+}
+
+void NavigationRegion2DEditor::_action_add_polygon(const Variant &p_polygon) {
+	Ref<NavigationPolygon> navpoly = _ensure_navpoly();
+	EditorUndoRedoManager *undo_redo = EditorUndoRedoManager::get_singleton();
+	undo_redo->add_do_method(navpoly.ptr(), "add_outline", p_polygon);
+	undo_redo->add_undo_method(navpoly.ptr(), "remove_outline", navpoly->get_outline_count());
+
+	if (rebake_timer && _rebake_timer_delay >= 0.0) {
+		rebake_timer->start();
+	}
+}
+
+void NavigationRegion2DEditor::_action_remove_polygon(int p_idx) {
+	Ref<NavigationPolygon> navpoly = _ensure_navpoly();
+	EditorUndoRedoManager *undo_redo = EditorUndoRedoManager::get_singleton();
+	undo_redo->add_do_method(navpoly.ptr(), "remove_outline", p_idx);
+	undo_redo->add_undo_method(navpoly.ptr(), "add_outline_at_index", navpoly->get_outline(p_idx), p_idx);
+
+	if (rebake_timer && _rebake_timer_delay >= 0.0) {
+		rebake_timer->start();
+	}
+}
+
+void NavigationRegion2DEditor::_action_set_polygon(int p_idx, const Variant &p_previous, const Variant &p_polygon) {
+	Ref<NavigationPolygon> navpoly = _ensure_navpoly();
+	EditorUndoRedoManager *undo_redo = EditorUndoRedoManager::get_singleton();
+	undo_redo->add_do_method(navpoly.ptr(), "set_outline", p_idx, p_polygon);
+	undo_redo->add_undo_method(navpoly.ptr(), "set_outline", p_idx, p_previous);
+
+	if (rebake_timer && _rebake_timer_delay >= 0.0) {
+		rebake_timer->start();
+	}
+}
+
+bool NavigationRegion2DEditor::_has_resource() const {
+	return node && node->get_navigation_polygon().is_valid();
+}
+
+void NavigationRegion2DEditor::_create_resource() {
+	if (!node) {
+		return;
+	}
+
+	EditorUndoRedoManager *undo_redo = EditorUndoRedoManager::get_singleton();
+	undo_redo->create_action(TTR("Create Navigation Polygon"));
+	undo_redo->add_do_method(node, "set_navigation_polygon", Ref<NavigationPolygon>(memnew(NavigationPolygon)));
+	undo_redo->add_undo_method(node, "set_navigation_polygon", Variant(Ref<RefCounted>()));
+	undo_redo->commit_action();
+
+	_menu_option(MODE_CREATE);
+}
+
+NavigationRegion2DEditor::NavigationRegion2DEditor() {
+	bake_hbox = memnew(HBoxContainer);
+	add_child(bake_hbox);
+
+	button_bake = memnew(Button);
+	button_bake->set_flat(true);
+	bake_hbox->add_child(button_bake);
+	button_bake->set_toggle_mode(true);
+	button_bake->set_text(TTR("Bake NavigationPolygon"));
+	button_bake->set_tooltip_text(TTR("Bakes the NavigationPolygon by first parsing the scene for source geometry and then creating the navigation polygon vertices and polygons."));
+	button_bake->connect(SceneStringName(pressed), callable_mp(this, &NavigationRegion2DEditor::_bake_pressed));
+
+	button_reset = memnew(Button);
+	button_reset->set_flat(true);
+	bake_hbox->add_child(button_reset);
+	button_reset->set_text(TTR("Clear NavigationPolygon"));
+	button_reset->set_tooltip_text(TTR("Clears the internal NavigationPolygon outlines, vertices and polygons."));
+	button_reset->connect(SceneStringName(pressed), callable_mp(this, &NavigationRegion2DEditor::_clear_pressed));
+
+	bake_info = memnew(Label);
+	bake_info->set_focus_mode(FOCUS_ACCESSIBILITY);
+	bake_hbox->add_child(bake_info);
+
+	rebake_timer = memnew(Timer);
+	add_child(rebake_timer);
+	rebake_timer->set_one_shot(true);
+	_rebake_timer_delay = EDITOR_GET("editors/polygon_editor/auto_bake_delay");
+	if (_rebake_timer_delay >= 0.0) {
+		rebake_timer->set_wait_time(_rebake_timer_delay);
+	}
+	rebake_timer->connect("timeout", callable_mp(this, &NavigationRegion2DEditor::_rebake_timer_timeout));
+
+	err_dialog = memnew(AcceptDialog);
+	add_child(err_dialog);
+	node = nullptr;
+}
+
+void NavigationRegion2DEditor::_notification(int p_what) {
+	switch (p_what) {
+		case NOTIFICATION_ENTER_TREE: {
+			button_bake->set_button_icon(get_editor_theme_icon(SNAME("Bake")));
+			button_reset->set_button_icon(get_editor_theme_icon(SNAME("Reload")));
+		} break;
+		case EditorSettings::NOTIFICATION_EDITOR_SETTINGS_CHANGED: {
+			if (rebake_timer) {
+				_rebake_timer_delay = EDITOR_GET("editors/polygon_editor/auto_bake_delay");
+				if (_rebake_timer_delay >= 0.0) {
+					rebake_timer->set_wait_time(_rebake_timer_delay);
+				}
+			}
+		} break;
+	}
+}
+
+void NavigationRegion2DEditor::_bake_pressed() {
+	if (rebake_timer) {
+		rebake_timer->stop();
+	}
+	button_bake->set_pressed(false);
+
+	ERR_FAIL_NULL(node);
+	Ref<NavigationPolygon> navigation_polygon = node->get_navigation_polygon();
+	if (navigation_polygon.is_null()) {
+		err_dialog->set_text(TTR("A NavigationPolygon resource must be set or created for this node to work."));
+		err_dialog->popup_centered();
+		return;
+	}
+
+	node->bake_navigation_polygon(true);
+
+	node->queue_redraw();
+}
+
+void NavigationRegion2DEditor::_clear_pressed() {
+	if (rebake_timer) {
+		rebake_timer->stop();
+	}
+	if (node) {
+		if (node->get_navigation_polygon().is_valid()) {
+			node->get_navigation_polygon()->clear();
+			// Needed to update all the region internals.
+			node->set_navigation_polygon(node->get_navigation_polygon());
+		}
+	}
+
+	button_bake->set_pressed(false);
+	bake_info->set_text("");
+
+	if (node) {
+		node->queue_redraw();
+	}
+}
+
+void NavigationRegion2DEditor::_update_polygon_editing_state() {
+	if (!_get_node()) {
+		return;
+	}
+
+	if (node != nullptr && node->get_navigation_polygon().is_valid()) {
+		bake_hbox->show();
+	} else {
+		bake_hbox->hide();
+	}
+}
+
+void NavigationRegion2DEditor::_rebake_timer_timeout() {
+	if (!node) {
+		return;
+	}
+	Ref<NavigationPolygon> navigation_polygon = node->get_navigation_polygon();
+	if (navigation_polygon.is_null()) {
+		return;
+	}
+
+	node->bake_navigation_polygon(true);
+	node->queue_redraw();
+}
+
+NavigationRegion2DEditorPlugin::NavigationRegion2DEditorPlugin() :
+		AbstractPolygon2DEditorPlugin(memnew(NavigationRegion2DEditor), "NavigationRegion2D") {
+}

modules/navigation_2d/editor/navigation_region_2d_editor_plugin.h

@@ -0,0 +1,94 @@
+/**************************************************************************/
+/*  navigation_region_2d_editor_plugin.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 "editor/plugins/editor_plugin.h"
+#include "editor/scene/2d/abstract_polygon_2d_editor.h"
+
+class AcceptDialog;
+class HBoxContainer;
+class NavigationPolygon;
+class NavigationRegion2D;
+
+class NavigationRegion2DEditor : public AbstractPolygon2DEditor {
+	friend class NavigationRegion2DEditorPlugin;
+
+	GDCLASS(NavigationRegion2DEditor, AbstractPolygon2DEditor);
+
+	NavigationRegion2D *node = nullptr;
+
+	Ref<NavigationPolygon> _ensure_navpoly() const;
+
+	AcceptDialog *err_dialog = nullptr;
+
+	HBoxContainer *bake_hbox = nullptr;
+	Button *button_bake = nullptr;
+	Button *button_reset = nullptr;
+	Label *bake_info = nullptr;
+
+	Timer *rebake_timer = nullptr;
+	float _rebake_timer_delay = 1.5;
+	void _rebake_timer_timeout();
+
+	void _bake_pressed();
+	void _clear_pressed();
+
+	void _update_polygon_editing_state();
+
+protected:
+	void _notification(int p_what);
+
+	virtual Node2D *_get_node() const override;
+	virtual void _set_node(Node *p_polygon) override;
+
+	virtual int _get_polygon_count() const override;
+	virtual Variant _get_polygon(int p_idx) const override;
+	virtual void _set_polygon(int p_idx, const Variant &p_polygon) const override;
+
+	virtual void _action_add_polygon(const Variant &p_polygon) override;
+	virtual void _action_remove_polygon(int p_idx) override;
+	virtual void _action_set_polygon(int p_idx, const Variant &p_previous, const Variant &p_polygon) override;
+
+	virtual bool _has_resource() const override;
+	virtual void _create_resource() override;
+
+public:
+	NavigationRegion2DEditor();
+};
+
+class NavigationRegion2DEditorPlugin : public AbstractPolygon2DEditorPlugin {
+	GDCLASS(NavigationRegion2DEditorPlugin, AbstractPolygon2DEditorPlugin);
+
+	NavigationRegion2DEditor *navigation_polygon_editor = nullptr;
+
+public:
+	NavigationRegion2DEditorPlugin();
+};

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;
+	}
+
+	Vector2 new_velocity;
+
+	new_velocity = Vector2(rvo_agent.velocity_.x(), 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,149 @@
+/**************************************************************************/
+/*  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 "servers/navigation/navigation_globals.h"
+
+#include <Agent2d.h>
+
+class NavMap2D;
+
+class NavAgent2D : public NavRid2D {
+	Vector2 position;
+	Vector2 target_position;
+	Vector2 velocity;
+	Vector2 velocity_forced;
+	real_t radius = NavigationDefaults2D::AVOIDANCE_AGENT_RADIUS;
+	real_t max_speed = NavigationDefaults2D::AVOIDANCE_AGENT_MAX_SPEED;
+	real_t time_horizon_agents = NavigationDefaults2D::AVOIDANCE_AGENT_TIME_HORIZON_AGENTS;
+	real_t time_horizon_obstacles = NavigationDefaults2D::AVOIDANCE_AGENT_TIME_HORIZON_OBSTACLES;
+	int max_neighbors = NavigationDefaults2D::AVOIDANCE_AGENT_MAX_NEIGHBORS;
+	real_t neighbor_distance = NavigationDefaults2D::AVOIDANCE_AGENT_NEIGHBOR_DISTANCE;
+	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,214 @@
+/**************************************************************************/
+/*  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;
+	iteration_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;
+	iteration_dirty = true;
+
+	request_sync();
+}
+
+void NavLink2D::set_bidirectional(bool p_bidirectional) {
+	if (bidirectional == p_bidirectional) {
+		return;
+	}
+	bidirectional = p_bidirectional;
+	iteration_dirty = true;
+
+	request_sync();
+}
+
+void NavLink2D::set_start_position(const Vector2 p_position) {
+	if (start_position == p_position) {
+		return;
+	}
+	start_position = p_position;
+	iteration_dirty = true;
+
+	request_sync();
+}
+
+void NavLink2D::set_end_position(const Vector2 p_position) {
+	if (end_position == p_position) {
+		return;
+	}
+	end_position = p_position;
+	iteration_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;
+	iteration_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;
+	iteration_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;
+	iteration_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;
+	iteration_dirty = true;
+
+	request_sync();
+}
+
+bool NavLink2D::sync() {
+	bool requires_map_update = false;
+	if (!map) {
+		return requires_map_update;
+	}
+
+	if (iteration_dirty && !iteration_building && !iteration_ready) {
+		_build_iteration();
+		iteration_ready = false;
+		requires_map_update = true;
+	}
+
+	return requires_map_update;
+}
+
+void NavLink2D::_build_iteration() {
+	if (!iteration_dirty || iteration_building || iteration_ready) {
+		return;
+	}
+
+	iteration_dirty = false;
+	iteration_building = true;
+	iteration_ready = false;
+
+	Ref<NavLinkIteration2D> new_iteration;
+	new_iteration.instantiate();
+
+	new_iteration->navigation_layers = get_navigation_layers();
+	new_iteration->enter_cost = get_enter_cost();
+	new_iteration->travel_cost = get_travel_cost();
+	new_iteration->owner_object_id = get_owner_id();
+	new_iteration->owner_type = get_type();
+	new_iteration->owner_rid = get_self();
+
+	new_iteration->enabled = get_enabled();
+	new_iteration->start_position = get_start_position();
+	new_iteration->end_position = get_end_position();
+	new_iteration->bidirectional = is_bidirectional();
+
+	RWLockWrite write_lock(iteration_rwlock);
+	ERR_FAIL_COND(iteration.is_null());
+	iteration = Ref<NavLinkIteration2D>();
+	DEV_ASSERT(iteration.is_null());
+	iteration = new_iteration;
+	iteration_id = iteration_id % UINT32_MAX + 1;
+
+	iteration_building = false;
+	iteration_ready = true;
+}
+
+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);
+	}
+}
+
+Ref<NavLinkIteration2D> NavLink2D::get_iteration() {
+	RWLockRead read_lock(iteration_rwlock);
+	return iteration;
+}
+
+NavLink2D::NavLink2D() :
+		sync_dirty_request_list_element(this) {
+	type = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_LINK;
+	iteration.instantiate();
+}
+
+NavLink2D::~NavLink2D() {
+	cancel_sync_request();
+
+	iteration = Ref<NavLinkIteration2D>();
+}

modules/navigation_2d/nav_link_2d.h

@@ -0,0 +1,118 @@
+/**************************************************************************/
+/*  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"
+
+class NavLinkIteration2D : public NavBaseIteration2D {
+	GDCLASS(NavLinkIteration2D, NavBaseIteration2D);
+
+public:
+	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; }
+
+	virtual ~NavLinkIteration2D() override {
+		navmesh_polygons.clear();
+		internal_connections.clear();
+	}
+};
+
+#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;
+
+	SelfList<NavLink2D> sync_dirty_request_list_element;
+
+	uint32_t iteration_id = 0;
+
+	mutable RWLock iteration_rwlock;
+	Ref<NavLinkIteration2D> iteration;
+
+	bool iteration_dirty = true;
+	bool iteration_building = false;
+	bool iteration_ready = false;
+
+	void _build_iteration();
+	void _sync_iteration();
+
+public:
+	NavLink2D();
+	~NavLink2D();
+
+	uint32_t get_iteration_id() const { return iteration_id; }
+
+	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(Vector2 p_position);
+	Vector2 get_start_position() const {
+		return start_position;
+	}
+
+	void set_end_position(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 sync();
+	void request_sync();
+	void cancel_sync_request();
+
+	Ref<NavLinkIteration2D> get_iteration();
+};

modules/navigation_2d/nav_map_2d.cpp

@@ -0,0 +1,797 @@
+/**************************************************************************/
+/*  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 "servers/navigation_server_2d.h"
+
+#include <Obstacle2d.h>
+
+using namespace Nav2D;
+
+#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::NAV_MESH_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::NAV_MESH_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;
+}
+
+const 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) {
+	DEV_ASSERT(!regions.has(p_region));
+
+	regions.push_back(p_region);
+	iteration_dirty = true;
+}
+
+void NavMap2D::remove_region(NavRegion2D *p_region) {
+	if (regions.erase_unordered(p_region)) {
+		iteration_dirty = true;
+	}
+}
+
+void NavMap2D::add_link(NavLink2D *p_link) {
+	DEV_ASSERT(!links.has(p_link));
+
+	links.push_back(p_link);
+	iteration_dirty = true;
+}
+
+void NavMap2D::remove_link(NavLink2D *p_link) {
+	if (links.erase_unordered(p_link)) {
+		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);
+	if (agents.erase_unordered(p_agent)) {
+		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) {
+	if (obstacles.erase_unordered(p_obstacle)) {
+		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) {
+	if (active_avoidance_agents.erase_unordered(p_agent)) {
+		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();
+
+	next_map_iteration.clear();
+
+	next_map_iteration.region_iterations.resize(regions.size());
+	next_map_iteration.link_iterations.resize(links.size());
+
+	uint32_t region_id_count = 0;
+	uint32_t link_id_count = 0;
+
+	for (NavRegion2D *region : regions) {
+		const Ref<NavRegionIteration2D> region_iteration = region->get_iteration();
+		next_map_iteration.region_iterations[region_id_count++] = region_iteration;
+		next_map_iteration.region_ptr_to_region_iteration[region] = region_iteration;
+	}
+	for (NavLink2D *link : links) {
+		const Ref<NavLinkIteration2D> link_iteration = link->get_iteration();
+		next_map_iteration.link_iterations[link_id_count++] = 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_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_async_tasks();
+
+	_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();
+
+		NavigationServer2D::get_singleton()->emit_signal(SNAME("map_changed"), get_self());
+	}
+
+	map_settings_dirty = false;
+
+	_sync_avoidance();
+
+	performance_data.pm_polygon_count = 0;
+	performance_data.pm_edge_count = 0;
+	performance_data.pm_edge_merge_count = 0;
+
+	for (NavRegion2D *region : regions) {
+		performance_data.pm_polygon_count += region->get_pm_polygon_count();
+		performance_data.pm_edge_count += region->get_pm_edge_count();
+		performance_data.pm_edge_merge_count += region->get_pm_edge_merge_count();
+	}
+}
+
+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) {
+		if (!obstacle->is_avoidance_enabled()) {
+			continue;
+		}
+		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(double p_delta_time) {
+	rvo_simulation.setTimeStep(float(p_delta_time));
+
+	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 *, Ref<NavRegionIteration2D>>::ConstIterator found_id = map_iteration.region_ptr_to_region_iteration.find(p_region);
+	if (found_id) {
+		HashMap<const NavBaseIteration2D *, LocalVector<Connection>>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value.ptr());
+		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 *, Ref<NavRegionIteration2D>>::ConstIterator found_id = map_iteration.region_ptr_to_region_iteration.find(p_region);
+	if (found_id) {
+		HashMap<const NavBaseIteration2D *, LocalVector<Connection>>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value.ptr());
+		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 *, Ref<NavRegionIteration2D>>::ConstIterator found_id = map_iteration.region_ptr_to_region_iteration.find(p_region);
+	if (found_id) {
+		HashMap<const NavBaseIteration2D *, LocalVector<Connection>>::ConstIterator found_connections = map_iteration.external_region_connections.find(found_id->value.ptr());
+		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;
+	}
+	RWLockWrite write_lock(sync_dirty_requests.regions.rwlock);
+	sync_dirty_requests.regions.list.add(p_sync_request);
+}
+
+void NavMap2D::add_link_sync_dirty_request(SelfList<NavLink2D> *p_sync_request) {
+	if (p_sync_request->in_list()) {
+		return;
+	}
+	RWLockWrite write_lock(sync_dirty_requests.links.rwlock);
+	sync_dirty_requests.links.list.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.list.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.list.add(p_sync_request);
+}
+
+void NavMap2D::remove_region_sync_dirty_request(SelfList<NavRegion2D> *p_sync_request) {
+	if (!p_sync_request->in_list()) {
+		return;
+	}
+	RWLockWrite write_lock(sync_dirty_requests.regions.rwlock);
+	sync_dirty_requests.regions.list.remove(p_sync_request);
+}
+
+void NavMap2D::remove_link_sync_dirty_request(SelfList<NavLink2D> *p_sync_request) {
+	if (!p_sync_request->in_list()) {
+		return;
+	}
+	RWLockWrite write_lock(sync_dirty_requests.links.rwlock);
+	sync_dirty_requests.links.list.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.list.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.list.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;
+	}
+
+	// Sync NavRegions.
+	RWLockWrite write_lock_regions(sync_dirty_requests.regions.rwlock);
+	for (SelfList<NavRegion2D> *element = sync_dirty_requests.regions.list.first(); element; element = element->next()) {
+		bool requires_map_update = element->self()->sync();
+		if (requires_map_update) {
+			iteration_dirty = true;
+		}
+	}
+	sync_dirty_requests.regions.list.clear();
+
+	// Sync NavLinks.
+	RWLockWrite write_lock_links(sync_dirty_requests.links.rwlock);
+	for (SelfList<NavLink2D> *element = sync_dirty_requests.links.list.first(); element; element = element->next()) {
+		bool requires_map_update = element->self()->sync();
+		if (requires_map_update) {
+			iteration_dirty = true;
+		}
+	}
+	sync_dirty_requests.links.list.clear();
+}
+
+void NavMap2D::_sync_dirty_avoidance_update_requests() {
+	// Sync NavAgents.
+	if (!agents_dirty) {
+		agents_dirty = sync_dirty_requests.agents.list.first();
+	}
+	for (SelfList<NavAgent2D> *element = sync_dirty_requests.agents.list.first(); element; element = element->next()) {
+		element->self()->sync();
+	}
+	sync_dirty_requests.agents.list.clear();
+
+	// Sync NavObstacles.
+	if (!obstacles_dirty) {
+		obstacles_dirty = sync_dirty_requests.obstacles.list.first();
+	}
+	for (SelfList<NavObstacle2D> *element = sync_dirty_requests.obstacles.list.first(); element; element = element->next()) {
+		element->self()->sync();
+	}
+	sync_dirty_requests.obstacles.list.clear();
+}
+
+void NavMap2D::add_region_async_thread_join_request(SelfList<NavRegion2D> *p_async_request) {
+	if (p_async_request->in_list()) {
+		return;
+	}
+	RWLockWrite write_lock(async_dirty_requests.regions.rwlock);
+	async_dirty_requests.regions.list.add(p_async_request);
+}
+
+void NavMap2D::remove_region_async_thread_join_request(SelfList<NavRegion2D> *p_async_request) {
+	if (!p_async_request->in_list()) {
+		return;
+	}
+	RWLockWrite write_lock(async_dirty_requests.regions.rwlock);
+	async_dirty_requests.regions.list.remove(p_async_request);
+}
+
+void NavMap2D::_sync_async_tasks() {
+	// Sync NavRegions that run async thread tasks.
+	RWLockWrite write_lock_regions(async_dirty_requests.regions.rwlock);
+	for (SelfList<NavRegion2D> *element = async_dirty_requests.regions.list.first(); element; element = element->next()) {
+		element->self()->sync_async_tasks();
+	}
+}
+
+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;
+	}
+
+	RWLockWrite write_lock(iteration_slot_rwlock);
+	for (NavMapIteration2D &iteration_slot : iteration_slots) {
+		iteration_slot.clear();
+	}
+}

modules/navigation_2d/nav_map_2d.h

@@ -0,0 +1,270 @@
+/**************************************************************************/
+/*  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::NAV_MESH_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;
+
+	/// 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
+	Nav2D::PerformanceData performance_data;
+
+	struct {
+		struct {
+			RWLock rwlock;
+			SelfList<NavRegion2D>::List list;
+		} regions;
+		struct {
+			RWLock rwlock;
+			SelfList<NavLink2D>::List list;
+		} links;
+		struct {
+			RWLock rwlock;
+			SelfList<NavAgent2D>::List list;
+		} agents;
+		struct {
+			RWLock rwlock;
+			SelfList<NavObstacle2D>::List list;
+		} obstacles;
+	} sync_dirty_requests;
+
+	struct {
+		struct {
+			RWLock rwlock;
+			SelfList<NavRegion2D>::List list;
+		} regions;
+	} async_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;
+	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;
+	}
+
+	Nav2D::PointKey get_point_key(const Vector2 &p_pos) const;
+	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;
+	Nav2D::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(double p_delta_time);
+	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_async_thread_join_request(SelfList<NavRegion2D> *p_async_request);
+	void remove_region_async_thread_join_request(SelfList<NavRegion2D> *p_async_request);
+
+	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 _sync_async_tasks();
+
+	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,359 @@
+/**************************************************************************/
+/*  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 "2d/nav_mesh_queries_2d.h"
+#include "2d/nav_region_builder_2d.h"
+#include "2d/nav_region_iteration_2d.h"
+#include "core/config/project_settings.h"
+
+using namespace Nav2D;
+
+void NavRegion2D::set_map(NavMap2D *p_map) {
+	if (map == p_map) {
+		return;
+	}
+
+	cancel_async_thread_join();
+	cancel_sync_request();
+
+	if (map) {
+		map->remove_region(this);
+	}
+
+	map = p_map;
+	iteration_dirty = true;
+
+	if (map) {
+		map->add_region(this);
+		request_sync();
+		if (iteration_build_thread_task_id != WorkerThreadPool::INVALID_TASK_ID) {
+			request_async_thread_join();
+		}
+	}
+}
+
+void NavRegion2D::set_enabled(bool p_enabled) {
+	if (enabled == p_enabled) {
+		return;
+	}
+	enabled = p_enabled;
+	iteration_dirty = true;
+
+	request_sync();
+}
+
+void NavRegion2D::set_use_edge_connections(bool p_enabled) {
+	if (use_edge_connections != p_enabled) {
+		use_edge_connections = p_enabled;
+		iteration_dirty = true;
+	}
+
+	request_sync();
+}
+
+void NavRegion2D::set_transform(Transform2D p_transform) {
+	if (transform == p_transform) {
+		return;
+	}
+	transform = p_transform;
+	iteration_dirty = true;
+
+	request_sync();
+}
+
+void NavRegion2D::set_navigation_mesh(Ref<NavigationPolygon> p_navigation_mesh) {
+#ifdef DEBUG_ENABLED
+	if (map && p_navigation_mesh.is_valid() && !Math::is_equal_approx(double(map->get_cell_size()), double(p_navigation_mesh->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_mesh->get_cell_size()), double(map->get_cell_size())));
+	}
+#endif // DEBUG_ENABLED
+
+	navmesh = p_navigation_mesh;
+
+	iteration_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;
+	iteration_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;
+	iteration_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;
+	iteration_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;
+	iteration_dirty = true;
+
+	request_sync();
+}
+
+void NavRegion2D::scratch_polygons() {
+	iteration_dirty = true;
+
+	request_sync();
+}
+
+real_t NavRegion2D::get_surface_area() const {
+	RWLockRead read_lock(iteration_rwlock);
+	return iteration->get_surface_area();
+}
+
+Rect2 NavRegion2D::get_bounds() const {
+	RWLockRead read_lock(iteration_rwlock);
+	return iteration->get_bounds();
+}
+
+LocalVector<Nav2D::Polygon> const &NavRegion2D::get_polygons() const {
+	RWLockRead read_lock(iteration_rwlock);
+	return iteration->get_navmesh_polygons();
+}
+
+bool NavRegion2D::sync() {
+	bool requires_map_update = false;
+	if (!map) {
+		return requires_map_update;
+	}
+
+	if (iteration_dirty && !iteration_building && !iteration_ready) {
+		_build_iteration();
+	}
+
+	if (iteration_ready) {
+		_sync_iteration();
+		requires_map_update = true;
+	}
+
+	return requires_map_update;
+}
+
+void NavRegion2D::sync_async_tasks() {
+	if (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;
+			request_sync();
+		}
+	}
+}
+
+void NavRegion2D::_build_iteration() {
+	if (!iteration_dirty || iteration_building || iteration_ready) {
+		return;
+	}
+
+	iteration_dirty = false;
+	iteration_building = true;
+	iteration_ready = false;
+
+	iteration_build.reset();
+
+	if (navmesh.is_valid()) {
+		navmesh->get_data(iteration_build.navmesh_data.vertices, iteration_build.navmesh_data.polygons);
+	}
+
+	iteration_build.map_cell_size = map->get_merge_rasterizer_cell_size();
+
+	Ref<NavRegionIteration2D> new_iteration;
+	new_iteration.instantiate();
+
+	new_iteration->navigation_layers = get_navigation_layers();
+	new_iteration->enter_cost = get_enter_cost();
+	new_iteration->travel_cost = get_travel_cost();
+	new_iteration->owner_object_id = get_owner_id();
+	new_iteration->owner_type = get_type();
+	new_iteration->owner_rid = get_self();
+	new_iteration->enabled = get_enabled();
+	new_iteration->transform = get_transform();
+	new_iteration->owner_use_edge_connections = get_use_edge_connections();
+
+	iteration_build.region_iteration = new_iteration;
+
+	if (use_async_iterations) {
+		iteration_build_thread_task_id = WorkerThreadPool::get_singleton()->add_native_task(&NavRegion2D::_build_iteration_threaded, &iteration_build, true, SNAME("NavRegionBuilder2D"));
+		request_async_thread_join();
+	} else {
+		NavRegionBuilder2D::build_iteration(iteration_build);
+
+		iteration_building = false;
+		iteration_ready = true;
+	}
+}
+
+void NavRegion2D::_build_iteration_threaded(void *p_arg) {
+	NavRegionIterationBuild2D *_iteration_build = static_cast<NavRegionIterationBuild2D *>(p_arg);
+
+	NavRegionBuilder2D::build_iteration(*_iteration_build);
+}
+
+void NavRegion2D::_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;
+
+	RWLockWrite write_lock(iteration_rwlock);
+	ERR_FAIL_COND(iteration.is_null());
+	iteration = Ref<NavRegionIteration2D>();
+	DEV_ASSERT(iteration.is_null());
+	iteration = iteration_build.region_iteration;
+	iteration_build.region_iteration = Ref<NavRegionIteration2D>();
+	DEV_ASSERT(iteration_build.region_iteration.is_null());
+	iteration_id = iteration_id % UINT32_MAX + 1;
+
+	iteration_ready = false;
+
+	cancel_async_thread_join();
+}
+
+Ref<NavRegionIteration2D> NavRegion2D::get_iteration() {
+	RWLockRead read_lock(iteration_rwlock);
+	return iteration;
+}
+
+void NavRegion2D::request_async_thread_join() {
+	DEV_ASSERT(map);
+	if (map && !async_list_element.in_list()) {
+		map->add_region_async_thread_join_request(&async_list_element);
+	}
+}
+
+void NavRegion2D::cancel_async_thread_join() {
+	if (map && async_list_element.in_list()) {
+		map->remove_region_async_thread_join_request(&async_list_element);
+	}
+}
+
+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);
+	}
+}
+
+void NavRegion2D::set_use_async_iterations(bool p_enabled) {
+	if (use_async_iterations == p_enabled) {
+		return;
+	}
+#ifdef THREADS_ENABLED
+	use_async_iterations = p_enabled;
+#endif
+}
+
+bool NavRegion2D::get_use_async_iterations() const {
+	return use_async_iterations;
+}
+
+NavRegion2D::NavRegion2D() :
+		sync_dirty_request_list_element(this), async_list_element(this) {
+	type = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_REGION;
+	iteration_build.region = this;
+	iteration.instantiate();
+
+#ifdef THREADS_ENABLED
+	use_async_iterations = GLOBAL_GET("navigation/world/region_use_async_iterations");
+#else
+	use_async_iterations = false;
+#endif
+}
+
+NavRegion2D::~NavRegion2D() {
+	cancel_async_thread_join();
+	cancel_sync_request();
+
+	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;
+	}
+
+	iteration_build.region = nullptr;
+	iteration_build.region_iteration = Ref<NavRegionIteration2D>();
+	iteration = Ref<NavRegionIteration2D>();
+}

modules/navigation_2d/nav_region_2d.h

@@ -0,0 +1,133 @@
+/**************************************************************************/
+/*  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"
+
+#include "2d/nav_region_iteration_2d.h"
+
+class NavRegion2D : public NavBase2D {
+	RWLock region_rwlock;
+
+	NavMap2D *map = nullptr;
+	Transform2D transform;
+	bool enabled = true;
+
+	bool use_edge_connections = true;
+
+	Rect2 bounds;
+
+	Ref<NavigationPolygon> navmesh;
+
+	Nav2D::PerformanceData performance_data;
+
+	uint32_t iteration_id = 0;
+
+	SelfList<NavRegion2D> sync_dirty_request_list_element;
+	mutable RWLock iteration_rwlock;
+	Ref<NavRegionIteration2D> iteration;
+
+	NavRegionIterationBuild2D iteration_build;
+	bool use_async_iterations = true;
+	SelfList<NavRegion2D> async_list_element;
+	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:
+	NavRegion2D();
+	~NavRegion2D();
+
+	uint32_t get_iteration_id() const { return iteration_id; }
+
+	void scratch_polygons();
+
+	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(Transform2D transform);
+	const Transform2D &get_transform() const {
+		return transform;
+	}
+
+	void set_navigation_mesh(Ref<NavigationPolygon> p_navigation_mesh);
+	Ref<NavigationPolygon> get_navigation_mesh() const { return navmesh; }
+
+	LocalVector<Nav2D::Polygon> const &get_polygons() const;
+
+	Nav2D::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;
+	Rect2 get_bounds() const;
+
+	// 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 sync_async_tasks();
+	void request_async_thread_join();
+	void cancel_async_thread_join();
+
+	Ref<NavRegionIteration2D> get_iteration();
+
+	// Performance Monitor
+	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; }
+
+	void set_use_async_iterations(bool p_enabled);
+	bool get_use_async_iterations() const;
+};

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,207 @@
+/**************************************************************************/
+/*  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/object/ref_counted.h"
+#include "core/templates/hash_map.h"
+#include "core/templates/hashfuncs.h"
+#include "servers/navigation/nav_heap.h"
+#include "servers/navigation/navigation_utilities.h"
+
+class NavBaseIteration2D;
+
+namespace Nav2D {
+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 ConnectableEdge {
+	EdgeKey ek;
+	uint32_t polygon_index;
+	int edge = -1;
+	Vector2 pathway_start;
+	Vector2 pathway_end;
+};
+
+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;
+};
+
+struct Polygon {
+	uint32_t id = UINT32_MAX;
+
+	/// Navigation region or link that contains this polygon.
+	const NavBaseIteration2D *owner = nullptr;
+
+	LocalVector<Vector2> vertices;
+
+	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 {
+	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 Nav2D

modules/navigation_2d/register_types.cpp

@@ -0,0 +1,66 @@
+/**************************************************************************/
+/*  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"
+
+#ifdef TOOLS_ENABLED
+#include "editor/navigation_link_2d_editor_plugin.h"
+#include "editor/navigation_obstacle_2d_editor_plugin.h"
+#include "editor/navigation_region_2d_editor_plugin.h"
+#endif
+
+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);
+	}
+
+#ifdef TOOLS_ENABLED
+	if (p_level == MODULE_INITIALIZATION_LEVEL_EDITOR) {
+		EditorPlugins::add_by_type<NavigationLink2DEditorPlugin>();
+		EditorPlugins::add_by_type<NavigationRegion2DEditorPlugin>();
+		EditorPlugins::add_by_type<NavigationObstacle2DEditorPlugin>();
+	}
+#endif
+}
+
+void uninitialize_navigation_2d_module(ModuleInitializationLevel p_level) {
+	if (p_level != MODULE_INITIALIZATION_LEVEL_SERVERS) {
+		return;
+	}
+}

modules/navigation_2d/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_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;
+	}
+};