initial commit, 4.5 stable

modules/navigation_3d/3d/godot_navigation_server_3d.h

@@ -0,0 +1,305 @@
+/**************************************************************************/
+/*  godot_navigation_server_3d.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_3d.h"
+#include "../nav_link_3d.h"
+#include "../nav_map_3d.h"
+#include "../nav_obstacle_3d.h"
+#include "../nav_region_3d.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_3d.h"
+#include "servers/navigation/navigation_path_query_result_3d.h"
+#include "servers/navigation_server_3d.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 GodotNavigationServer3D;
+class NavMeshGenerator3D;
+
+struct SetCommand3D {
+	virtual ~SetCommand3D() {}
+	virtual void exec(GodotNavigationServer3D *server) = 0;
+};
+
+class GodotNavigationServer3D : public NavigationServer3D {
+	Mutex commands_mutex;
+	/// Mutex used to make any operation threadsafe.
+	Mutex operations_mutex;
+
+	LocalVector<SetCommand3D *> commands;
+
+	mutable RID_Owner<NavLink3D> link_owner;
+	mutable RID_Owner<NavMap3D> map_owner;
+	mutable RID_Owner<NavRegion3D> region_owner;
+	mutable RID_Owner<NavAgent3D> agent_owner;
+	mutable RID_Owner<NavObstacle3D> obstacle_owner;
+
+	bool active = true;
+	LocalVector<NavMap3D *> active_maps;
+
+	NavMeshGenerator3D *navmesh_generator_3d = nullptr;
+
+	// 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:
+	GodotNavigationServer3D();
+	virtual ~GodotNavigationServer3D();
+
+	void add_command(SetCommand3D *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_up, RID, p_map, Vector3, p_up);
+	virtual Vector3 map_get_up(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_cell_height, RID, p_map, real_t, p_cell_height);
+	virtual real_t map_get_cell_height(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<Vector3> map_get_path(RID p_map, Vector3 p_origin, Vector3 p_destination, bool p_optimize, uint32_t p_navigation_layers = 1) override;
+
+	virtual Vector3 map_get_closest_point_to_segment(RID p_map, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision = false) const override;
+	virtual Vector3 map_get_closest_point(RID p_map, const Vector3 &p_point) const override;
+	virtual Vector3 map_get_closest_point_normal(RID p_map, const Vector3 &p_point) const override;
+	virtual RID map_get_closest_point_owner(RID p_map, const Vector3 &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 Vector3 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 Vector3 &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, Transform3D, p_transform);
+	virtual Transform3D region_get_transform(RID p_region) const override;
+	COMMAND_2(region_set_navigation_mesh, RID, p_region, Ref<NavigationMesh>, p_navigation_mesh);
+#ifndef DISABLE_DEPRECATED
+	virtual void region_bake_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh, Node *p_root_node) override;
+#endif // DISABLE_DEPRECATED
+	virtual int region_get_connections_count(RID p_region) const override;
+	virtual Vector3 region_get_connection_pathway_start(RID p_region, int p_connection_id) const override;
+	virtual Vector3 region_get_connection_pathway_end(RID p_region, int p_connection_id) const override;
+	virtual Vector3 region_get_closest_point_to_segment(RID p_region, const Vector3 &p_from, const Vector3 &p_to, bool p_use_collision = false) const override;
+	virtual Vector3 region_get_closest_point(RID p_region, const Vector3 &p_point) const override;
+	virtual Vector3 region_get_closest_point_normal(RID p_region, const Vector3 &p_point) const override;
+	virtual Vector3 region_get_random_point(RID p_region, uint32_t p_navigation_layers, bool p_uniformly) const override;
+	virtual AABB 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;
+	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;
+	COMMAND_2(link_set_bidirectional, RID, p_link, bool, p_bidirectional);
+	virtual bool link_is_bidirectional(RID p_link) const override;
+	COMMAND_2(link_set_navigation_layers, RID, p_link, uint32_t, p_navigation_layers);
+	virtual uint32_t link_get_navigation_layers(RID p_link) const override;
+	COMMAND_2(link_set_start_position, RID, p_link, Vector3, p_position);
+	virtual Vector3 link_get_start_position(RID p_link) const override;
+	COMMAND_2(link_set_end_position, RID, p_link, Vector3, p_position);
+	virtual Vector3 link_get_end_position(RID p_link) const override;
+	COMMAND_2(link_set_enter_cost, RID, p_link, real_t, p_enter_cost);
+	virtual real_t link_get_enter_cost(RID p_link) const override;
+	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;
+	COMMAND_2(link_set_owner_id, RID, p_link, ObjectID, p_owner_id);
+	virtual ObjectID link_get_owner_id(RID p_link) const override;
+
+	virtual RID agent_create() override;
+	COMMAND_2(agent_set_avoidance_enabled, RID, p_agent, bool, p_enabled);
+	virtual bool agent_get_avoidance_enabled(RID p_agent) const override;
+	COMMAND_2(agent_set_use_3d_avoidance, RID, p_agent, bool, p_enabled);
+	virtual bool agent_get_use_3d_avoidance(RID p_agent) const override;
+	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_neighbor_distance, RID, p_agent, real_t, p_distance);
+	virtual real_t agent_get_neighbor_distance(RID p_agent) const override;
+	COMMAND_2(agent_set_max_neighbors, RID, p_agent, int, p_count);
+	virtual int agent_get_max_neighbors(RID p_agent) const override;
+	COMMAND_2(agent_set_time_horizon_agents, RID, p_agent, real_t, p_time_horizon);
+	virtual real_t agent_get_time_horizon_agents(RID p_agent) const override;
+	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;
+	COMMAND_2(agent_set_radius, RID, p_agent, real_t, p_radius);
+	virtual real_t agent_get_radius(RID p_agent) const override;
+	COMMAND_2(agent_set_height, RID, p_agent, real_t, p_height);
+	virtual real_t agent_get_height(RID p_agent) const override;
+	COMMAND_2(agent_set_max_speed, RID, p_agent, real_t, p_max_speed);
+	virtual real_t agent_get_max_speed(RID p_agent) const override;
+	COMMAND_2(agent_set_velocity, RID, p_agent, Vector3, p_velocity);
+	virtual Vector3 agent_get_velocity(RID p_agent) const override;
+	COMMAND_2(agent_set_velocity_forced, RID, p_agent, Vector3, p_velocity);
+	COMMAND_2(agent_set_position, RID, p_agent, Vector3, p_position);
+	virtual Vector3 agent_get_position(RID p_agent) const override;
+	virtual bool agent_is_map_changed(RID p_agent) const override;
+	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_use_3d_avoidance, RID, p_obstacle, bool, p_enabled);
+	virtual bool obstacle_get_use_3d_avoidance(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, Vector3, p_velocity);
+	virtual Vector3 obstacle_get_velocity(RID p_obstacle) const override;
+	COMMAND_2(obstacle_set_position, RID, p_obstacle, Vector3, p_position);
+	virtual Vector3 obstacle_get_position(RID p_obstacle) const override;
+	COMMAND_2(obstacle_set_height, RID, p_obstacle, real_t, p_height);
+	virtual real_t obstacle_get_height(RID p_obstacle) const override;
+	virtual void obstacle_set_vertices(RID p_obstacle, const Vector<Vector3> &p_vertices) override;
+	virtual Vector<Vector3> 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 parse_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data, Node *p_root_node, const Callable &p_callback = Callable()) override;
+	virtual void bake_from_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data, const Callable &p_callback = Callable()) override;
+	virtual void bake_from_source_geometry_data_async(const Ref<NavigationMesh> &p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data, const Callable &p_callback = Callable()) override;
+	virtual bool is_baking_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh) const override;
+	virtual String get_baking_navigation_mesh_state_msg(Ref<NavigationMesh> p_navigation_mesh) 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<Vector3> simplify_path(const Vector<Vector3> &p_path, real_t p_epsilon) override;
+
+public:
+	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 void query_path(const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result, const Callable &p_callback = Callable()) override;
+
+	int get_process_info(ProcessInfo p_info) const override;
+
+private:
+	void internal_free_agent(RID p_object);
+	void internal_free_obstacle(RID p_object);
+};
+
+#undef COMMAND_1
+#undef COMMAND_2

modules/navigation_3d/3d/nav_base_iteration_3d.h

@@ -0,0 +1,68 @@
+/**************************************************************************/
+/*  nav_base_iteration_3d.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_3d.h"
+
+#include "core/object/ref_counted.h"
+#include "servers/navigation/navigation_utilities.h"
+
+class NavBaseIteration3D : public RefCounted {
+	GDCLASS(NavBaseIteration3D, 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<Nav3D::Polygon> navmesh_polygons;
+	LocalVector<LocalVector<Nav3D::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<Nav3D::Polygon> &get_navmesh_polygons() const { return navmesh_polygons; }
+	const LocalVector<LocalVector<Nav3D::Connection>> &get_internal_connections() const { return internal_connections; }
+
+	virtual ~NavBaseIteration3D() {
+		navmesh_polygons.clear();
+		internal_connections.clear();
+	}
+};

modules/navigation_3d/3d/nav_map_builder_3d.cpp

@@ -0,0 +1,426 @@
+/**************************************************************************/
+/*  nav_map_builder_3d.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_3d.h"
+
+#include "../nav_link_3d.h"
+#include "../nav_map_3d.h"
+#include "../nav_region_3d.h"
+#include "nav_map_iteration_3d.h"
+#include "nav_region_iteration_3d.h"
+
+using namespace Nav3D;
+
+PointKey NavMapBuilder3D::get_point_key(const Vector3 &p_pos, const Vector3 &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));
+	const int z = static_cast<int>(Math::floor(p_pos.z / p_cell_size.z));
+
+	PointKey p;
+	p.key = 0;
+	p.x = x;
+	p.y = y;
+	p.z = z;
+	return p;
+}
+
+void NavMapBuilder3D::build_navmap_iteration(NavMapIterationBuild3D &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 NavMapBuilder3D::_build_step_gather_region_polygons(NavMapIterationBuild3D &r_build) {
+	PerformanceData &performance_data = r_build.performance_data;
+	NavMapIteration3D *map_iteration = r_build.map_iteration;
+
+	const LocalVector<Ref<NavRegionIteration3D>> &regions = map_iteration->region_iterations;
+	HashMap<const NavBaseIteration3D *, 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<NavRegionIteration3D> &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 NavMapBuilder3D::_build_step_find_edge_connection_pairs(NavMapIterationBuild3D &r_build) {
+	PerformanceData &performance_data = r_build.performance_data;
+	NavMapIteration3D *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<NavRegionIteration3D> &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/3d/merge_rasterizer_cell_scale' to 0.001.");
+			}
+		}
+	}
+
+	r_build.free_edge_count = free_edges_count;
+}
+
+void NavMapBuilder3D::_build_step_merge_edge_connection_pairs(NavMapIterationBuild3D &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);
+
+	NavMapIteration3D *map_iteration = r_build.map_iteration;
+
+	HashMap<const NavBaseIteration3D *, LocalVector<LocalVector<Nav3D::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 NavMapBuilder3D::_build_step_edge_connection_margin_connections(NavMapIterationBuild3D &r_build) {
+	PerformanceData &performance_data = r_build.performance_data;
+	NavMapIteration3D *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 NavBaseIteration3D *, LocalVector<Connection>> &region_external_connections = map_iteration->external_region_connections;
+
+	HashMap<const NavBaseIteration3D *, LocalVector<LocalVector<Nav3D::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 Vector3 &edge_p1 = free_edge.pathway_start;
+		const Vector3 &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 Vector3 &other_edge_p1 = other_edge.pathway_start;
+			const Vector3 &other_edge_p2 = other_edge.pathway_end;
+
+			// Compute the projection of the opposite edge on the current one
+			Vector3 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.
+			Vector3 self1 = edge_vector * CLAMP(projected_p1_ratio, 0.0, 1.0) + edge_p1;
+			Vector3 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;
+			}
+
+			Vector3 self2 = edge_vector * CLAMP(projected_p2_ratio, 0.0, 1.0) + edge_p1;
+			Vector3 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 NavMapBuilder3D::_build_step_navlink_connections(NavMapIterationBuild3D &r_build) {
+	NavMapIteration3D *map_iteration = r_build.map_iteration;
+
+	real_t link_connection_radius = r_build.link_connection_radius;
+
+	const LocalVector<Ref<NavLinkIteration3D>> &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 NavBaseIteration3D *, LocalVector<LocalVector<Nav3D::Connection>>> &navbases_polygons_external_connections = map_iteration->navbases_polygons_external_connections;
+	LocalVector<Nav3D::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<NavLinkIteration3D> &link : links) {
+		polygon_count++;
+		Polygon &new_polygon = navlink_polygons[navlink_index++];
+
+		new_polygon.id = 0;
+		new_polygon.owner = link.ptr();
+
+		const Vector3 link_start_pos = link->get_start_position();
+		const Vector3 link_end_pos = link->get_end_position();
+
+		Polygon *closest_start_polygon = nullptr;
+		real_t closest_start_sqr_dist = link_connection_radius_sqr;
+		Vector3 closest_start_point;
+
+		Polygon *closest_end_polygon = nullptr;
+		real_t closest_end_sqr_dist = link_connection_radius_sqr;
+		Vector3 closest_end_point;
+
+		for (const Ref<NavRegionIteration3D> &region : map_iteration->region_iterations) {
+			AABB 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 Face3 face(polyon.vertices[0], polyon.vertices[point_id - 1], polyon.vertices[point_id]);
+
+					{
+						const Vector3 start_point = face.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 Vector3 end_point = face.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<Nav3D::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<Nav3D::Connection>());
+				navbases_polygons_external_connections[link.ptr()][new_polygon.id].push_back(exit_connection);
+			}
+		}
+	}
+
+	r_build.polygon_count = polygon_count;
+}
+
+void NavMapBuilder3D::_build_update_map_iteration(NavMapIterationBuild3D &r_build) {
+	NavMapIteration3D *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 (NavMeshQueries3D::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<NavRegionIteration3D> &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_3d/3d/nav_map_builder_3d.h

@@ -0,0 +1,49 @@
+/**************************************************************************/
+/*  nav_map_builder_3d.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_3d.h"
+
+struct NavMapIterationBuild3D;
+
+class NavMapBuilder3D {
+	static void _build_step_gather_region_polygons(NavMapIterationBuild3D &r_build);
+	static void _build_step_find_edge_connection_pairs(NavMapIterationBuild3D &r_build);
+	static void _build_step_merge_edge_connection_pairs(NavMapIterationBuild3D &r_build);
+	static void _build_step_edge_connection_margin_connections(NavMapIterationBuild3D &r_build);
+	static void _build_step_navlink_connections(NavMapIterationBuild3D &r_build);
+	static void _build_update_map_iteration(NavMapIterationBuild3D &r_build);
+
+public:
+	static Nav3D::PointKey get_point_key(const Vector3 &p_pos, const Vector3 &p_cell_size);
+
+	static void build_navmap_iteration(NavMapIterationBuild3D &r_build);
+};

modules/navigation_3d/3d/nav_map_iteration_3d.h

@@ -0,0 +1,122 @@
+/**************************************************************************/
+/*  nav_map_iteration_3d.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_3d.h"
+#include "../nav_utils_3d.h"
+#include "nav_mesh_queries_3d.h"
+
+#include "core/math/math_defs.h"
+#include "core/os/semaphore.h"
+
+class NavLinkIteration3D;
+class NavRegion3D;
+class NavRegionIteration3D;
+struct NavMapIteration3D;
+
+struct NavMapIterationBuild3D {
+	Vector3 merge_rasterizer_cell_size;
+	bool use_edge_connections = true;
+	real_t edge_connection_margin;
+	real_t link_connection_radius;
+	Nav3D::PerformanceData performance_data;
+	int polygon_count = 0;
+	int free_edge_count = 0;
+
+	HashMap<Nav3D::EdgeKey, Nav3D::EdgeConnectionPair, Nav3D::EdgeKey> iter_connection_pairs_map;
+	LocalVector<Nav3D::Connection> iter_free_edges;
+
+	NavMapIteration3D *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 NavMapIteration3D {
+	mutable SafeNumeric<uint32_t> users;
+	RWLock rwlock;
+
+	Vector3 map_up;
+
+	LocalVector<Ref<NavRegionIteration3D>> region_iterations;
+	LocalVector<Ref<NavLinkIteration3D>> link_iterations;
+
+	int navmesh_polygon_count = 0;
+
+	// The edge connections that the map builds on top with the edge connection margin.
+	HashMap<const NavBaseIteration3D *, LocalVector<Nav3D::Connection>> external_region_connections;
+	HashMap<const NavBaseIteration3D *, LocalVector<LocalVector<Nav3D::Connection>>> navbases_polygons_external_connections;
+
+	LocalVector<Nav3D::Polygon> navlink_polygons;
+
+	HashMap<NavRegion3D *, Ref<NavRegionIteration3D>> region_ptr_to_region_iteration;
+
+	LocalVector<NavMeshQueries3D::PathQuerySlot> path_query_slots;
+	Mutex path_query_slots_mutex;
+	Semaphore path_query_slots_semaphore;
+
+	void clear() {
+		map_up = Vector3();
+		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 NavMapIterationRead3D {
+	const NavMapIteration3D &map_iteration;
+
+public:
+	_ALWAYS_INLINE_ NavMapIterationRead3D(const NavMapIteration3D &p_iteration) :
+			map_iteration(p_iteration) {
+		map_iteration.rwlock.read_lock();
+		map_iteration.users.increment();
+	}
+	_ALWAYS_INLINE_ ~NavMapIterationRead3D() {
+		map_iteration.users.decrement();
+		map_iteration.rwlock.read_unlock();
+	}
+};

modules/navigation_3d/3d/nav_mesh_generator_3d.cpp

@@ -0,0 +1,597 @@
+/**************************************************************************/
+/*  nav_mesh_generator_3d.cpp                                             */
+/**************************************************************************/
+/*                         This file is part of:                          */
+/*                             GODOT ENGINE                               */
+/*                        https://godotengine.org                         */
+/**************************************************************************/
+/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
+/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur.                  */
+/*                                                                        */
+/* Permission is hereby granted, free of charge, to any person obtaining  */
+/* a copy of this software and associated documentation files (the        */
+/* "Software"), to deal in the Software without restriction, including    */
+/* without limitation the rights to use, copy, modify, merge, publish,    */
+/* distribute, sublicense, and/or sell copies of the Software, and to     */
+/* permit persons to whom the Software is furnished to do so, subject to  */
+/* the following conditions:                                              */
+/*                                                                        */
+/* The above copyright notice and this permission notice shall be         */
+/* included in all copies or substantial portions of the Software.        */
+/*                                                                        */
+/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        */
+/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     */
+/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. */
+/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY   */
+/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,   */
+/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE      */
+/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                 */
+/**************************************************************************/
+
+#include "nav_mesh_generator_3d.h"
+
+#include "core/config/project_settings.h"
+#include "core/os/thread.h"
+#include "scene/3d/node_3d.h"
+#include "scene/resources/3d/navigation_mesh_source_geometry_data_3d.h"
+#include "scene/resources/navigation_mesh.h"
+
+#include <Recast.h>
+
+NavMeshGenerator3D *NavMeshGenerator3D::singleton = nullptr;
+Mutex NavMeshGenerator3D::baking_navmesh_mutex;
+Mutex NavMeshGenerator3D::generator_task_mutex;
+RWLock NavMeshGenerator3D::generator_parsers_rwlock;
+bool NavMeshGenerator3D::use_threads = true;
+bool NavMeshGenerator3D::baking_use_multiple_threads = true;
+bool NavMeshGenerator3D::baking_use_high_priority_threads = true;
+HashMap<Ref<NavigationMesh>, NavMeshGenerator3D::NavMeshGeneratorTask3D *> NavMeshGenerator3D::baking_navmeshes;
+HashMap<WorkerThreadPool::TaskID, NavMeshGenerator3D::NavMeshGeneratorTask3D *> NavMeshGenerator3D::generator_tasks;
+LocalVector<NavMeshGeometryParser3D *> NavMeshGenerator3D::generator_parsers;
+
+static const char *_navmesh_bake_state_msgs[(size_t)NavMeshGenerator3D::NavMeshBakeState::BAKE_STATE_MAX] = {
+	"",
+	"Setting up configuration...",
+	"Calculating grid size...",
+	"Creating heightfield...",
+	"Marking walkable triangles...",
+	"Constructing compact heightfield...", // step 5
+	"Eroding walkable area...",
+	"Sample partitioning...",
+	"Creating contours...",
+	"Creating polymesh...",
+	"Converting to native navigation mesh...", // step 10
+	"Baking cleanup...",
+	"Baking finished.",
+};
+
+NavMeshGenerator3D *NavMeshGenerator3D::get_singleton() {
+	return singleton;
+}
+
+NavMeshGenerator3D::NavMeshGenerator3D() {
+	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;
+}
+
+NavMeshGenerator3D::~NavMeshGenerator3D() {
+	cleanup();
+}
+
+void NavMeshGenerator3D::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, NavMeshGeneratorTask3D *> &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);
+
+				NavMeshGeneratorTask3D *generator_task = E.value;
+				DEV_ASSERT(generator_task->status == NavMeshGeneratorTask3D::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 NavMeshGenerator3D::cleanup() {
+	MutexLock baking_navmesh_lock(baking_navmesh_mutex);
+	{
+		MutexLock generator_task_lock(generator_task_mutex);
+
+		baking_navmeshes.clear();
+
+		for (KeyValue<WorkerThreadPool::TaskID, NavMeshGeneratorTask3D *> &E : generator_tasks) {
+			WorkerThreadPool::get_singleton()->wait_for_task_completion(E.key);
+			NavMeshGeneratorTask3D *generator_task = E.value;
+			memdelete(generator_task);
+		}
+		generator_tasks.clear();
+
+		generator_parsers_rwlock.write_lock();
+		generator_parsers.clear();
+		generator_parsers_rwlock.write_unlock();
+	}
+}
+
+void NavMeshGenerator3D::finish() {
+	cleanup();
+}
+
+void NavMeshGenerator3D::parse_source_geometry_data(Ref<NavigationMesh> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> 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 NavMeshGenerator3D::bake_from_source_geometry_data(Ref<NavigationMesh> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> 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_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("NavigationMesh is already baking. Wait for current bake to finish.");
+	}
+	baking_navmesh_mutex.lock();
+	NavMeshGeneratorTask3D generator_task;
+	baking_navmeshes.insert(p_navigation_mesh, &generator_task);
+	baking_navmesh_mutex.unlock();
+
+	generator_task.navigation_mesh = p_navigation_mesh;
+	generator_task.source_geometry_data = p_source_geometry_data;
+	generator_task.status = NavMeshGeneratorTask3D::TaskStatus::BAKING_STARTED;
+
+	generator_bake_from_source_geometry_data(&generator_task);
+
+	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 NavMeshGenerator3D::bake_from_source_geometry_data_async(Ref<NavigationMesh> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> 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_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("NavigationMesh is already baking. Wait for current bake to finish.");
+		return;
+	}
+	baking_navmesh_mutex.lock();
+	NavMeshGeneratorTask3D *generator_task = memnew(NavMeshGeneratorTask3D);
+	baking_navmeshes.insert(p_navigation_mesh, generator_task);
+	baking_navmesh_mutex.unlock();
+
+	generator_task->navigation_mesh = p_navigation_mesh;
+	generator_task->source_geometry_data = p_source_geometry_data;
+	generator_task->callback = p_callback;
+	generator_task->status = NavMeshGeneratorTask3D::TaskStatus::BAKING_STARTED;
+	generator_task->thread_task_id = WorkerThreadPool::get_singleton()->add_native_task(&NavMeshGenerator3D::generator_thread_bake, generator_task, NavMeshGenerator3D::baking_use_high_priority_threads, SNAME("NavMeshGeneratorBake3D"));
+	MutexLock generator_task_lock(generator_task_mutex);
+	generator_tasks.insert(generator_task->thread_task_id, generator_task);
+}
+
+bool NavMeshGenerator3D::is_baking(Ref<NavigationMesh> p_navigation_mesh) {
+	MutexLock baking_navmesh_lock(baking_navmesh_mutex);
+	return baking_navmeshes.has(p_navigation_mesh);
+}
+
+String NavMeshGenerator3D::get_baking_state_msg(Ref<NavigationMesh> p_navigation_mesh) {
+	String bake_state_msg;
+	MutexLock baking_navmesh_lock(baking_navmesh_mutex);
+	if (baking_navmeshes.has(p_navigation_mesh)) {
+		bake_state_msg = _navmesh_bake_state_msgs[baking_navmeshes[p_navigation_mesh]->bake_state];
+	} else {
+		bake_state_msg = _navmesh_bake_state_msgs[NavMeshBakeState::BAKE_STATE_NONE];
+	}
+	return bake_state_msg;
+}
+
+void NavMeshGenerator3D::generator_thread_bake(void *p_arg) {
+	NavMeshGeneratorTask3D *generator_task = static_cast<NavMeshGeneratorTask3D *>(p_arg);
+
+	generator_bake_from_source_geometry_data(generator_task);
+
+	generator_task->status = NavMeshGeneratorTask3D::TaskStatus::BAKING_FINISHED;
+}
+
+void NavMeshGenerator3D::generator_parse_geometry_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node, bool p_recurse_children) {
+	generator_parsers_rwlock.read_lock();
+	for (const NavMeshGeometryParser3D *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 NavMeshGenerator3D::set_generator_parsers(LocalVector<NavMeshGeometryParser3D *> p_parsers) {
+	RWLockWrite write_lock(generator_parsers_rwlock);
+	generator_parsers = p_parsers;
+}
+
+void NavMeshGenerator3D::generator_parse_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_root_node) {
+	List<Node *> parse_nodes;
+
+	if (p_navigation_mesh->get_source_geometry_mode() == NavigationMesh::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_group_name(), &parse_nodes);
+	}
+
+	Transform3D root_node_transform = Transform3D();
+	if (Object::cast_to<Node3D>(p_root_node)) {
+		root_node_transform = Object::cast_to<Node3D>(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() != NavigationMesh::SOURCE_GEOMETRY_GROUPS_EXPLICIT;
+
+	for (Node *parse_node : parse_nodes) {
+		generator_parse_geometry_node(p_navigation_mesh, p_source_geometry_data, parse_node, recurse_children);
+	}
+}
+
+void NavMeshGenerator3D::generator_bake_from_source_geometry_data(NavMeshGeneratorTask3D *p_generator_task) {
+	Ref<NavigationMesh> p_navigation_mesh = p_generator_task->navigation_mesh;
+	const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data = p_generator_task->source_geometry_data;
+
+	if (p_navigation_mesh.is_null() || p_source_geometry_data.is_null()) {
+		return;
+	}
+
+	Vector<float> source_geometry_vertices;
+	Vector<int> source_geometry_indices;
+	Vector<NavigationMeshSourceGeometryData3D::ProjectedObstruction> projected_obstructions;
+
+	p_source_geometry_data->get_data(
+			source_geometry_vertices,
+			source_geometry_indices,
+			projected_obstructions);
+
+	if (source_geometry_vertices.size() < 3 || source_geometry_indices.size() < 3) {
+		return;
+	}
+
+	rcHeightfield *hf = nullptr;
+	rcCompactHeightfield *chf = nullptr;
+	rcContourSet *cset = nullptr;
+	rcPolyMesh *poly_mesh = nullptr;
+	rcPolyMeshDetail *detail_mesh = nullptr;
+	rcContext ctx;
+
+	p_generator_task->bake_state = NavMeshBakeState::BAKE_STATE_CONFIGURATION; // step #1
+
+	const float *verts = source_geometry_vertices.ptr();
+	const int nverts = source_geometry_vertices.size() / 3;
+	const int *tris = source_geometry_indices.ptr();
+	const int ntris = source_geometry_indices.size() / 3;
+
+	float bmin[3], bmax[3];
+	rcCalcBounds(verts, nverts, bmin, bmax);
+
+	rcConfig cfg;
+	memset(&cfg, 0, sizeof(cfg));
+
+	cfg.cs = p_navigation_mesh->get_cell_size();
+	cfg.ch = p_navigation_mesh->get_cell_height();
+	if (p_navigation_mesh->get_border_size() > 0.0) {
+		cfg.borderSize = (int)Math::ceil(p_navigation_mesh->get_border_size() / cfg.cs);
+	}
+	cfg.walkableSlopeAngle = p_navigation_mesh->get_agent_max_slope();
+	cfg.walkableHeight = (int)Math::ceil(p_navigation_mesh->get_agent_height() / cfg.ch);
+	cfg.walkableClimb = (int)Math::floor(p_navigation_mesh->get_agent_max_climb() / cfg.ch);
+	cfg.walkableRadius = (int)Math::ceil(p_navigation_mesh->get_agent_radius() / cfg.cs);
+	cfg.maxEdgeLen = (int)(p_navigation_mesh->get_edge_max_length() / p_navigation_mesh->get_cell_size());
+	cfg.maxSimplificationError = p_navigation_mesh->get_edge_max_error();
+	cfg.minRegionArea = (int)(p_navigation_mesh->get_region_min_size() * p_navigation_mesh->get_region_min_size());
+	cfg.mergeRegionArea = (int)(p_navigation_mesh->get_region_merge_size() * p_navigation_mesh->get_region_merge_size());
+	cfg.maxVertsPerPoly = (int)p_navigation_mesh->get_vertices_per_polygon();
+	cfg.detailSampleDist = MAX(p_navigation_mesh->get_cell_size() * p_navigation_mesh->get_detail_sample_distance(), 0.1f);
+	cfg.detailSampleMaxError = p_navigation_mesh->get_cell_height() * p_navigation_mesh->get_detail_sample_max_error();
+
+	if (p_navigation_mesh->get_border_size() > 0.0 && !Math::is_zero_approx(Math::fmod(p_navigation_mesh->get_border_size(), p_navigation_mesh->get_cell_size()))) {
+		WARN_PRINT("Property border_size is ceiled to cell_size voxel units and loses precision.");
+	}
+	if (!Math::is_equal_approx((float)cfg.walkableHeight * cfg.ch, p_navigation_mesh->get_agent_height())) {
+		WARN_PRINT("Property agent_height is ceiled to cell_height voxel units and loses precision.");
+	}
+	if (!Math::is_equal_approx((float)cfg.walkableClimb * cfg.ch, p_navigation_mesh->get_agent_max_climb())) {
+		WARN_PRINT("Property agent_max_climb is floored to cell_height voxel units and loses precision.");
+	}
+	if (!Math::is_equal_approx((float)cfg.walkableRadius * cfg.cs, p_navigation_mesh->get_agent_radius())) {
+		WARN_PRINT("Property agent_radius is ceiled to cell_size voxel units and loses precision.");
+	}
+	if (!Math::is_equal_approx((float)cfg.maxEdgeLen * cfg.cs, p_navigation_mesh->get_edge_max_length())) {
+		WARN_PRINT("Property edge_max_length is rounded to cell_size voxel units and loses precision.");
+	}
+	if (!Math::is_equal_approx((float)cfg.minRegionArea, p_navigation_mesh->get_region_min_size() * p_navigation_mesh->get_region_min_size())) {
+		WARN_PRINT("Property region_min_size is converted to int and loses precision.");
+	}
+	if (!Math::is_equal_approx((float)cfg.mergeRegionArea, p_navigation_mesh->get_region_merge_size() * p_navigation_mesh->get_region_merge_size())) {
+		WARN_PRINT("Property region_merge_size is converted to int and loses precision.");
+	}
+	if (!Math::is_equal_approx((float)cfg.maxVertsPerPoly, p_navigation_mesh->get_vertices_per_polygon())) {
+		WARN_PRINT("Property vertices_per_polygon is converted to int and loses precision.");
+	}
+	if (p_navigation_mesh->get_cell_size() * p_navigation_mesh->get_detail_sample_distance() < 0.1f) {
+		WARN_PRINT("Property detail_sample_distance is clamped to 0.1 world units as the resulting value from multiplying with cell_size is too low.");
+	}
+
+	cfg.bmin[0] = bmin[0];
+	cfg.bmin[1] = bmin[1];
+	cfg.bmin[2] = bmin[2];
+	cfg.bmax[0] = bmax[0];
+	cfg.bmax[1] = bmax[1];
+	cfg.bmax[2] = bmax[2];
+
+	AABB baking_aabb = p_navigation_mesh->get_filter_baking_aabb();
+	if (baking_aabb.has_volume()) {
+		Vector3 baking_aabb_offset = p_navigation_mesh->get_filter_baking_aabb_offset();
+		cfg.bmin[0] = baking_aabb.position[0] + baking_aabb_offset.x;
+		cfg.bmin[1] = baking_aabb.position[1] + baking_aabb_offset.y;
+		cfg.bmin[2] = baking_aabb.position[2] + baking_aabb_offset.z;
+		cfg.bmax[0] = cfg.bmin[0] + baking_aabb.size[0];
+		cfg.bmax[1] = cfg.bmin[1] + baking_aabb.size[1];
+		cfg.bmax[2] = cfg.bmin[2] + baking_aabb.size[2];
+	}
+
+	p_generator_task->bake_state = NavMeshBakeState::BAKE_STATE_CALC_GRID_SIZE; // step #2
+	rcCalcGridSize(cfg.bmin, cfg.bmax, cfg.cs, &cfg.width, &cfg.height);
+
+	// ~30000000 seems to be around sweetspot where Editor baking breaks
+	if ((cfg.width * cfg.height) > 30000000 && GLOBAL_GET("navigation/baking/use_crash_prevention_checks")) {
+		ERR_FAIL_MSG("Baking interrupted."
+					 "\nNavigationMesh baking process would likely crash the engine."
+					 "\nSource geometry is suspiciously big for the current Cell Size and Cell Height in the NavMesh Resource bake settings."
+					 "\nIf baking does not crash the engine or fail, the resulting NavigationMesh will create serious pathfinding performance issues."
+					 "\nIt is advised to increase Cell Size and/or Cell Height in the NavMesh Resource bake settings or reduce the size / scale of the source geometry."
+					 "\nIf you would like to try baking anyway, disable the 'navigation/baking/use_crash_prevention_checks' project setting.");
+		return;
+	}
+
+	p_generator_task->bake_state = NavMeshBakeState::BAKE_STATE_CREATE_HEIGHTFIELD; // step #3
+	hf = rcAllocHeightfield();
+
+	ERR_FAIL_NULL(hf);
+	ERR_FAIL_COND(!rcCreateHeightfield(&ctx, *hf, cfg.width, cfg.height, cfg.bmin, cfg.bmax, cfg.cs, cfg.ch));
+
+	p_generator_task->bake_state = NavMeshBakeState::BAKE_STATE_MARK_WALKABLE_TRIANGLES; // step #4
+	{
+		Vector<unsigned char> tri_areas;
+		tri_areas.resize(ntris);
+
+		ERR_FAIL_COND(tri_areas.is_empty());
+
+		memset(tri_areas.ptrw(), 0, ntris * sizeof(unsigned char));
+		rcMarkWalkableTriangles(&ctx, cfg.walkableSlopeAngle, verts, nverts, tris, ntris, tri_areas.ptrw());
+
+		ERR_FAIL_COND(!rcRasterizeTriangles(&ctx, verts, nverts, tris, tri_areas.ptr(), ntris, *hf, cfg.walkableClimb));
+	}
+
+	if (p_navigation_mesh->get_filter_low_hanging_obstacles()) {
+		rcFilterLowHangingWalkableObstacles(&ctx, cfg.walkableClimb, *hf);
+	}
+	if (p_navigation_mesh->get_filter_ledge_spans()) {
+		rcFilterLedgeSpans(&ctx, cfg.walkableHeight, cfg.walkableClimb, *hf);
+	}
+	if (p_navigation_mesh->get_filter_walkable_low_height_spans()) {
+		rcFilterWalkableLowHeightSpans(&ctx, cfg.walkableHeight, *hf);
+	}
+
+	p_generator_task->bake_state = NavMeshBakeState::BAKE_STATE_CONSTRUCT_COMPACT_HEIGHTFIELD; // step #5
+
+	chf = rcAllocCompactHeightfield();
+
+	ERR_FAIL_NULL(chf);
+	ERR_FAIL_COND(!rcBuildCompactHeightfield(&ctx, cfg.walkableHeight, cfg.walkableClimb, *hf, *chf));
+
+	rcFreeHeightField(hf);
+	hf = nullptr;
+
+	// Add obstacles to the source geometry. Those will be affected by e.g. agent_radius.
+	if (!projected_obstructions.is_empty()) {
+		for (const NavigationMeshSourceGeometryData3D::ProjectedObstruction &projected_obstruction : projected_obstructions) {
+			if (projected_obstruction.carve) {
+				continue;
+			}
+			if (projected_obstruction.vertices.is_empty() || projected_obstruction.vertices.size() % 3 != 0) {
+				continue;
+			}
+
+			const float *projected_obstruction_verts = projected_obstruction.vertices.ptr();
+			const int projected_obstruction_nverts = projected_obstruction.vertices.size() / 3;
+
+			rcMarkConvexPolyArea(&ctx, projected_obstruction_verts, projected_obstruction_nverts, projected_obstruction.elevation, projected_obstruction.elevation + projected_obstruction.height, RC_NULL_AREA, *chf);
+		}
+	}
+
+	p_generator_task->bake_state = NavMeshBakeState::BAKE_STATE_ERODE_WALKABLE_AREA; // step #6
+
+	ERR_FAIL_COND(!rcErodeWalkableArea(&ctx, cfg.walkableRadius, *chf));
+
+	// Carve obstacles to the eroded geometry. Those will NOT be affected by e.g. agent_radius because that step is already done.
+	if (!projected_obstructions.is_empty()) {
+		for (const NavigationMeshSourceGeometryData3D::ProjectedObstruction &projected_obstruction : projected_obstructions) {
+			if (!projected_obstruction.carve) {
+				continue;
+			}
+			if (projected_obstruction.vertices.is_empty() || projected_obstruction.vertices.size() % 3 != 0) {
+				continue;
+			}
+
+			const float *projected_obstruction_verts = projected_obstruction.vertices.ptr();
+			const int projected_obstruction_nverts = projected_obstruction.vertices.size() / 3;
+
+			rcMarkConvexPolyArea(&ctx, projected_obstruction_verts, projected_obstruction_nverts, projected_obstruction.elevation, projected_obstruction.elevation + projected_obstruction.height, RC_NULL_AREA, *chf);
+		}
+	}
+
+	p_generator_task->bake_state = NavMeshBakeState::BAKE_STATE_SAMPLE_PARTITIONING; // step #7
+
+	if (p_navigation_mesh->get_sample_partition_type() == NavigationMesh::SAMPLE_PARTITION_WATERSHED) {
+		ERR_FAIL_COND(!rcBuildDistanceField(&ctx, *chf));
+		ERR_FAIL_COND(!rcBuildRegions(&ctx, *chf, cfg.borderSize, cfg.minRegionArea, cfg.mergeRegionArea));
+	} else if (p_navigation_mesh->get_sample_partition_type() == NavigationMesh::SAMPLE_PARTITION_MONOTONE) {
+		ERR_FAIL_COND(!rcBuildRegionsMonotone(&ctx, *chf, cfg.borderSize, cfg.minRegionArea, cfg.mergeRegionArea));
+	} else {
+		ERR_FAIL_COND(!rcBuildLayerRegions(&ctx, *chf, cfg.borderSize, cfg.minRegionArea));
+	}
+
+	p_generator_task->bake_state = NavMeshBakeState::BAKE_STATE_CREATING_CONTOURS; // step #8
+
+	cset = rcAllocContourSet();
+
+	ERR_FAIL_NULL(cset);
+	ERR_FAIL_COND(!rcBuildContours(&ctx, *chf, cfg.maxSimplificationError, cfg.maxEdgeLen, *cset));
+
+	p_generator_task->bake_state = NavMeshBakeState::BAKE_STATE_CREATING_POLYMESH; // step #9
+
+	poly_mesh = rcAllocPolyMesh();
+	ERR_FAIL_NULL(poly_mesh);
+	ERR_FAIL_COND(!rcBuildPolyMesh(&ctx, *cset, cfg.maxVertsPerPoly, *poly_mesh));
+
+	detail_mesh = rcAllocPolyMeshDetail();
+	ERR_FAIL_NULL(detail_mesh);
+	ERR_FAIL_COND(!rcBuildPolyMeshDetail(&ctx, *poly_mesh, *chf, cfg.detailSampleDist, cfg.detailSampleMaxError, *detail_mesh));
+
+	rcFreeCompactHeightfield(chf);
+	chf = nullptr;
+	rcFreeContourSet(cset);
+	cset = nullptr;
+
+	p_generator_task->bake_state = NavMeshBakeState::BAKE_STATE_CONVERTING_NATIVE_NAVMESH; // step #10
+
+	Vector<Vector3> nav_vertices;
+	Vector<Vector<int>> nav_polygons;
+
+	HashMap<Vector3, int> recast_vertex_to_native_index;
+	LocalVector<int> recast_index_to_native_index;
+	recast_index_to_native_index.resize(detail_mesh->nverts);
+
+	for (int i = 0; i < detail_mesh->nverts; i++) {
+		const float *v = &detail_mesh->verts[i * 3];
+		const Vector3 vertex = Vector3(v[0], v[1], v[2]);
+		int *existing_index_ptr = recast_vertex_to_native_index.getptr(vertex);
+		if (!existing_index_ptr) {
+			int new_index = recast_vertex_to_native_index.size();
+			recast_index_to_native_index[i] = new_index;
+			recast_vertex_to_native_index[vertex] = new_index;
+			nav_vertices.push_back(vertex);
+		} else {
+			recast_index_to_native_index[i] = *existing_index_ptr;
+		}
+	}
+
+	for (int i = 0; i < detail_mesh->nmeshes; i++) {
+		const unsigned int *detail_mesh_m = &detail_mesh->meshes[i * 4];
+		const unsigned int detail_mesh_bverts = detail_mesh_m[0];
+		const unsigned int detail_mesh_m_btris = detail_mesh_m[2];
+		const unsigned int detail_mesh_ntris = detail_mesh_m[3];
+		const unsigned char *detail_mesh_tris = &detail_mesh->tris[detail_mesh_m_btris * 4];
+		for (unsigned int j = 0; j < detail_mesh_ntris; j++) {
+			Vector<int> nav_indices;
+			nav_indices.resize(3);
+			// Polygon order in recast is opposite than godot's
+			int index1 = ((int)(detail_mesh_bverts + detail_mesh_tris[j * 4 + 0]));
+			int index2 = ((int)(detail_mesh_bverts + detail_mesh_tris[j * 4 + 2]));
+			int index3 = ((int)(detail_mesh_bverts + detail_mesh_tris[j * 4 + 1]));
+
+			nav_indices.write[0] = recast_index_to_native_index[index1];
+			nav_indices.write[1] = recast_index_to_native_index[index2];
+			nav_indices.write[2] = recast_index_to_native_index[index3];
+
+			nav_polygons.push_back(nav_indices);
+		}
+	}
+
+	p_navigation_mesh->set_data(nav_vertices, nav_polygons);
+
+	p_generator_task->bake_state = NavMeshBakeState::BAKE_STATE_BAKE_CLEANUP; // step #11
+
+	rcFreePolyMesh(poly_mesh);
+	poly_mesh = nullptr;
+	rcFreePolyMeshDetail(detail_mesh);
+	detail_mesh = nullptr;
+
+	p_generator_task->bake_state = NavMeshBakeState::BAKE_STATE_BAKE_FINISHED; // step #12
+}
+
+bool NavMeshGenerator3D::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;
+}

modules/navigation_3d/3d/nav_mesh_generator_3d.h

@@ -0,0 +1,121 @@
+/**************************************************************************/
+/*  nav_mesh_generator_3d.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/object/class_db.h"
+#include "core/object/worker_thread_pool.h"
+#include "core/templates/rid_owner.h"
+#include "servers/navigation_server_3d.h"
+
+class Node;
+class NavigationMesh;
+class NavigationMeshSourceGeometryData3D;
+
+class NavMeshGenerator3D : public Object {
+	static NavMeshGenerator3D *singleton;
+
+	static Mutex baking_navmesh_mutex;
+	static Mutex generator_task_mutex;
+
+	static RWLock generator_parsers_rwlock;
+	static LocalVector<NavMeshGeometryParser3D *> generator_parsers;
+
+	static bool use_threads;
+	static bool baking_use_multiple_threads;
+	static bool baking_use_high_priority_threads;
+
+public:
+	enum NavMeshBakeState {
+		BAKE_STATE_NONE,
+		BAKE_STATE_CONFIGURATION,
+		BAKE_STATE_CALC_GRID_SIZE,
+		BAKE_STATE_CREATE_HEIGHTFIELD,
+		BAKE_STATE_MARK_WALKABLE_TRIANGLES,
+		BAKE_STATE_CONSTRUCT_COMPACT_HEIGHTFIELD,
+		BAKE_STATE_ERODE_WALKABLE_AREA,
+		BAKE_STATE_SAMPLE_PARTITIONING,
+		BAKE_STATE_CREATING_CONTOURS,
+		BAKE_STATE_CREATING_POLYMESH,
+		BAKE_STATE_CONVERTING_NATIVE_NAVMESH,
+		BAKE_STATE_BAKE_CLEANUP,
+		BAKE_STATE_BAKE_FINISHED,
+		BAKE_STATE_MAX,
+	};
+
+private:
+	struct NavMeshGeneratorTask3D {
+		enum TaskStatus {
+			BAKING_STARTED,
+			BAKING_FINISHED,
+			BAKING_FAILED,
+			CALLBACK_DISPATCHED,
+			CALLBACK_FAILED,
+		};
+
+		Ref<NavigationMesh> navigation_mesh;
+		Ref<NavigationMeshSourceGeometryData3D> source_geometry_data;
+		Callable callback;
+		WorkerThreadPool::TaskID thread_task_id = WorkerThreadPool::INVALID_TASK_ID;
+		NavMeshGeneratorTask3D::TaskStatus status = NavMeshGeneratorTask3D::TaskStatus::BAKING_STARTED;
+
+		NavMeshBakeState bake_state = NavMeshBakeState::BAKE_STATE_NONE;
+	};
+
+	static HashMap<WorkerThreadPool::TaskID, NavMeshGeneratorTask3D *> generator_tasks;
+
+	static void generator_thread_bake(void *p_arg);
+
+	static HashMap<Ref<NavigationMesh>, NavMeshGeneratorTask3D *> baking_navmeshes;
+
+	static void generator_parse_geometry_node(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_node, bool p_recurse_children);
+	static void generator_parse_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_root_node);
+	static void generator_bake_from_source_geometry_data(NavMeshGeneratorTask3D *p_generator_task);
+
+	static bool generator_emit_callback(const Callable &p_callback);
+
+public:
+	static NavMeshGenerator3D *get_singleton();
+
+	static void sync();
+	static void cleanup();
+	static void finish();
+
+	static void set_generator_parsers(LocalVector<NavMeshGeometryParser3D *> p_parsers);
+
+	static void parse_source_geometry_data(Ref<NavigationMesh> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_root_node, const Callable &p_callback = Callable());
+	static void bake_from_source_geometry_data(Ref<NavigationMesh> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, const Callable &p_callback = Callable());
+	static void bake_from_source_geometry_data_async(Ref<NavigationMesh> p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, const Callable &p_callback = Callable());
+	static bool is_baking(Ref<NavigationMesh> p_navigation_mesh);
+	static String get_baking_state_msg(Ref<NavigationMesh> p_navigation_mesh);
+
+	NavMeshGenerator3D();
+	~NavMeshGenerator3D();
+};

modules/navigation_3d/3d/nav_mesh_queries_3d.h

@@ -0,0 +1,162 @@
+/**************************************************************************/
+/*  nav_mesh_queries_3d.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_3d.h"
+
+#include "core/templates/a_hash_map.h"
+
+#include "servers/navigation/navigation_globals.h"
+#include "servers/navigation/navigation_path_query_parameters_3d.h"
+#include "servers/navigation/navigation_path_query_result_3d.h"
+#include "servers/navigation/navigation_utilities.h"
+
+using namespace NavigationUtilities;
+
+class NavMap3D;
+struct NavMapIteration3D;
+
+class NavMeshQueries3D {
+public:
+	struct PathQuerySlot {
+		LocalVector<Nav3D::NavigationPoly> path_corridor;
+		Heap<Nav3D::NavigationPoly *, Nav3D::NavPolyTravelCostGreaterThan, Nav3D::NavPolyHeapIndexer> traversable_polys;
+		bool in_use = false;
+		uint32_t slot_index = 0;
+		AHashMap<const Nav3D::Polygon *, uint32_t> poly_to_id;
+	};
+
+	struct NavMeshPathQueryTask3D {
+		enum TaskStatus {
+			QUERY_STARTED,
+			QUERY_FINISHED,
+			QUERY_FAILED,
+			CALLBACK_DISPATCHED,
+			CALLBACK_FAILED,
+		};
+
+		// Parameters.
+		Vector3 start_position;
+		Vector3 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 = NavigationDefaults3D::path_search_max_polygons;
+		float path_search_max_distance = 0.0;
+
+		// Path building.
+		Vector3 begin_position;
+		Vector3 end_position;
+		const Nav3D::Polygon *begin_polygon = nullptr;
+		const Nav3D::Polygon *end_polygon = nullptr;
+		uint32_t least_cost_id = 0;
+
+		// Map.
+		Vector3 map_up;
+		NavMap3D *map = nullptr;
+		PathQuerySlot *path_query_slot = nullptr;
+
+		// Path points.
+		LocalVector<Vector3> 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<NavigationPathQueryParameters3D> query_parameters;
+		Ref<NavigationPathQueryResult3D> query_result;
+		Callable callback;
+		NavMeshPathQueryTask3D::TaskStatus status = NavMeshPathQueryTask3D::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 Vector3 polygons_get_random_point(const LocalVector<Nav3D::Polygon> &p_polygons, uint32_t p_navigation_layers, bool p_uniformly);
+
+	static Vector3 polygons_get_closest_point_to_segment(const LocalVector<Nav3D::Polygon> &p_polygons, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision);
+	static Vector3 polygons_get_closest_point(const LocalVector<Nav3D::Polygon> &p_polygons, const Vector3 &p_point);
+	static Vector3 polygons_get_closest_point_normal(const LocalVector<Nav3D::Polygon> &p_polygons, const Vector3 &p_point);
+	static Nav3D::ClosestPointQueryResult polygons_get_closest_point_info(const LocalVector<Nav3D::Polygon> &p_polygons, const Vector3 &p_point);
+	static RID polygons_get_closest_point_owner(const LocalVector<Nav3D::Polygon> &p_polygons, const Vector3 &p_point);
+
+	static Vector3 map_iteration_get_closest_point_to_segment(const NavMapIteration3D &p_map_iteration, const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision);
+	static Vector3 map_iteration_get_closest_point(const NavMapIteration3D &p_map_iteration, const Vector3 &p_point);
+	static Vector3 map_iteration_get_closest_point_normal(const NavMapIteration3D &p_map_iteration, const Vector3 &p_point);
+	static RID map_iteration_get_closest_point_owner(const NavMapIteration3D &p_map_iteration, const Vector3 &p_point);
+	static Nav3D::ClosestPointQueryResult map_iteration_get_closest_point_info(const NavMapIteration3D &p_map_iteration, const Vector3 &p_point);
+	static Vector3 map_iteration_get_random_point(const NavMapIteration3D &p_map_iteration, uint32_t p_navigation_layers, bool p_uniformly);
+
+	static void map_query_path(NavMap3D *map, const Ref<NavigationPathQueryParameters3D> &p_query_parameters, Ref<NavigationPathQueryResult3D> p_query_result, const Callable &p_callback);
+
+	static void query_task_map_iteration_get_path(NavMeshPathQueryTask3D &p_query_task, const NavMapIteration3D &p_map_iteration);
+	static void _query_task_push_back_point_with_metadata(NavMeshPathQueryTask3D &p_query_task, const Vector3 &p_point, const Nav3D::Polygon *p_point_polygon);
+	static void _query_task_find_start_end_positions(NavMeshPathQueryTask3D &p_query_task, const NavMapIteration3D &p_map_iteration);
+	static void _query_task_build_path_corridor(NavMeshPathQueryTask3D &p_query_task, const NavMapIteration3D &p_map_iteration);
+	static void _query_task_post_process_corridorfunnel(NavMeshPathQueryTask3D &p_query_task);
+	static void _query_task_post_process_edgecentered(NavMeshPathQueryTask3D &p_query_task);
+	static void _query_task_post_process_nopostprocessing(NavMeshPathQueryTask3D &p_query_task);
+	static void _query_task_clip_path(NavMeshPathQueryTask3D &p_query_task, const Nav3D::NavigationPoly *from_poly, const Vector3 &p_to_point, const Nav3D::NavigationPoly *p_to_poly);
+	static void _query_task_simplified_path_points(NavMeshPathQueryTask3D &p_query_task);
+	static bool _query_task_is_connection_owner_usable(const NavMeshPathQueryTask3D &p_query_task, const NavBaseIteration3D *p_owner);
+	static void _query_task_process_path_result_limits(NavMeshPathQueryTask3D &p_query_task);
+
+	static void _query_task_search_polygon_connections(NavMeshPathQueryTask3D &p_query_task, const Nav3D::Connection &p_connection, uint32_t p_least_cost_id, const Nav3D::NavigationPoly &p_least_cost_poly, real_t p_poly_enter_cost, const Vector3 &p_end_point);
+
+	static void simplify_path_segment(int p_start_inx, int p_end_inx, const LocalVector<Vector3> &p_points, real_t p_epsilon, LocalVector<uint32_t> &r_simplified_path_indices);
+	static LocalVector<uint32_t> get_simplified_path_indices(const LocalVector<Vector3> &p_path, real_t p_epsilon);
+
+	static float _calculate_path_length(const LocalVector<Vector3> &p_path, uint32_t p_start_index, uint32_t p_end_index);
+};

modules/navigation_3d/3d/nav_region_builder_3d.cpp

@@ -0,0 +1,257 @@
+/**************************************************************************/
+/*  nav_region_builder_3d.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_3d.h"
+
+#include "../nav_map_3d.h"
+#include "../nav_region_3d.h"
+#include "nav_region_iteration_3d.h"
+
+using namespace Nav3D;
+
+void NavRegionBuilder3D::build_iteration(NavRegionIterationBuild3D &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 NavRegionBuilder3D::_build_step_process_navmesh_data(NavRegionIterationBuild3D &r_build) {
+	Vector<Vector3> _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<NavRegionIteration3D> region_iteration = r_build.region_iteration;
+
+	const Transform3D &region_transform = region_iteration->transform;
+	LocalVector<Nav3D::Polygon> &navmesh_polygons = region_iteration->navmesh_polygons;
+
+	const int vertex_count = _navmesh_vertices.size();
+
+	const Vector3 *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;
+	AABB _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 Face3 face = Face3(
+						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 += face.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 Vector3 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();
+}
+
+Nav3D::PointKey NavRegionBuilder3D::get_point_key(const Vector3 &p_pos, const Vector3 &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));
+	const int z = static_cast<int>(Math::floor(p_pos.z / p_cell_size.z));
+
+	PointKey p;
+	p.key = 0;
+	p.x = x;
+	p.y = y;
+	p.z = z;
+	return p;
+}
+
+Nav3D::EdgeKey NavRegionBuilder3D::get_edge_key(const Vector3 &p_vertex1, const Vector3 &p_vertex2, const Vector3 &p_cell_size) {
+	EdgeKey ek(get_point_key(p_vertex1, p_cell_size), get_point_key(p_vertex2, p_cell_size));
+	return ek;
+}
+
+void NavRegionBuilder3D::_build_step_find_edge_connection_pairs(NavRegionIterationBuild3D &r_build) {
+	PerformanceData &performance_data = r_build.performance_data;
+
+	const Vector3 &map_cell_size = r_build.map_cell_size;
+	Ref<NavRegionIteration3D> region_iteration = r_build.region_iteration;
+	LocalVector<Nav3D::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 NavRegionBuilder3D::_build_step_merge_edge_connection_pairs(NavRegionIterationBuild3D &r_build) {
+	PerformanceData &performance_data = r_build.performance_data;
+
+	Ref<NavRegionIteration3D> 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 NavRegionBuilder3D::_build_update_iteration(NavRegionIterationBuild3D &r_build) {
+	ERR_FAIL_NULL(r_build.region);
+	// Stub. End of the build.
+}

modules/navigation_3d/3d/nav_region_builder_3d.h

@@ -0,0 +1,48 @@
+/**************************************************************************/
+/*  nav_region_builder_3d.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_3d.h"
+
+struct NavRegionIterationBuild3D;
+
+class NavRegionBuilder3D {
+	static void _build_step_process_navmesh_data(NavRegionIterationBuild3D &r_build);
+	static void _build_step_find_edge_connection_pairs(NavRegionIterationBuild3D &r_build);
+	static void _build_step_merge_edge_connection_pairs(NavRegionIterationBuild3D &r_build);
+	static void _build_update_iteration(NavRegionIterationBuild3D &r_build);
+
+public:
+	static Nav3D::PointKey get_point_key(const Vector3 &p_pos, const Vector3 &p_cell_size);
+	static Nav3D::EdgeKey get_edge_key(const Vector3 &p_vertex1, const Vector3 &p_vertex2, const Vector3 &p_cell_size);
+
+	static void build_iteration(NavRegionIterationBuild3D &r_build);
+};

modules/navigation_3d/3d/nav_region_iteration_3d.h

@@ -0,0 +1,92 @@
+/**************************************************************************/
+/*  nav_region_iteration_3d.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_3d.h"
+#include "nav_base_iteration_3d.h"
+#include "scene/resources/navigation_mesh.h"
+
+#include "core/math/aabb.h"
+
+class NavRegion3D;
+class NavRegionIteration3D;
+
+struct NavRegionIterationBuild3D {
+	Nav3D::PerformanceData performance_data;
+
+	NavRegion3D *region = nullptr;
+
+	Vector3 map_cell_size;
+	Transform3D region_transform;
+
+	struct NavMeshData {
+		Vector<Vector3> vertices;
+		Vector<Vector<int>> polygons;
+
+		void clear() {
+			vertices.clear();
+			polygons.clear();
+		}
+	} navmesh_data;
+
+	Ref<NavRegionIteration3D> region_iteration;
+
+	HashMap<Nav3D::EdgeKey, Nav3D::EdgeConnectionPair, Nav3D::EdgeKey> iter_connection_pairs_map;
+
+	void reset() {
+		performance_data.reset();
+
+		navmesh_data.clear();
+		region_iteration = Ref<NavRegionIteration3D>();
+		iter_connection_pairs_map.clear();
+	}
+};
+
+class NavRegionIteration3D : public NavBaseIteration3D {
+	GDCLASS(NavRegionIteration3D, NavBaseIteration3D);
+
+public:
+	Transform3D transform;
+	real_t surface_area = 0.0;
+	AABB bounds;
+	LocalVector<Nav3D::ConnectableEdge> external_edges;
+
+	const Transform3D &get_transform() const { return transform; }
+	real_t get_surface_area() const { return surface_area; }
+	AABB get_bounds() const { return bounds; }
+	const LocalVector<Nav3D::ConnectableEdge> &get_external_edges() const { return external_edges; }
+
+	virtual ~NavRegionIteration3D() override {
+		external_edges.clear();
+		navmesh_polygons.clear();
+		internal_connections.clear();
+	}
+};

modules/navigation_3d/3d/navigation_mesh_generator.cpp

@@ -0,0 +1,74 @@
+/**************************************************************************/
+/*  navigation_mesh_generator.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_mesh_generator.h"
+
+#include "scene/resources/3d/navigation_mesh_source_geometry_data_3d.h"
+#include "servers/navigation_server_3d.h"
+
+NavigationMeshGenerator *NavigationMeshGenerator::singleton = nullptr;
+
+NavigationMeshGenerator *NavigationMeshGenerator::get_singleton() {
+	return singleton;
+}
+
+NavigationMeshGenerator::NavigationMeshGenerator() {
+	singleton = this;
+}
+
+NavigationMeshGenerator::~NavigationMeshGenerator() {
+}
+
+void NavigationMeshGenerator::bake(const Ref<NavigationMesh> &p_navigation_mesh, Node *p_root_node) {
+	WARN_PRINT_ONCE("NavigationMeshGenerator::bake() is deprecated due to core threading changes. To upgrade existing code, first create a NavigationMeshSourceGeometryData3D resource. Use this resource with method parse_source_geometry_data() to parse the SceneTree for nodes that should contribute to the navigation mesh baking. The SceneTree parsing needs to happen on the main thread. After the parsing is finished use the resource with method bake_from_source_geometry_data() to bake a navigation mesh..");
+}
+
+void NavigationMeshGenerator::clear(Ref<NavigationMesh> p_navigation_mesh) {
+	if (p_navigation_mesh.is_valid()) {
+		p_navigation_mesh->clear_polygons();
+		p_navigation_mesh->set_vertices(Vector<Vector3>());
+	}
+}
+
+void NavigationMeshGenerator::parse_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_root_node, const Callable &p_callback) {
+	NavigationServer3D::get_singleton()->parse_source_geometry_data(p_navigation_mesh, p_source_geometry_data, p_root_node, p_callback);
+}
+
+void NavigationMeshGenerator::bake_from_source_geometry_data(Ref<NavigationMesh> p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data, const Callable &p_callback) {
+	NavigationServer3D::get_singleton()->bake_from_source_geometry_data(p_navigation_mesh, p_source_geometry_data, p_callback);
+}
+
+void NavigationMeshGenerator::_bind_methods() {
+	ClassDB::bind_method(D_METHOD("bake", "navigation_mesh", "root_node"), &NavigationMeshGenerator::bake);
+	ClassDB::bind_method(D_METHOD("clear", "navigation_mesh"), &NavigationMeshGenerator::clear);
+
+	ClassDB::bind_method(D_METHOD("parse_source_geometry_data", "navigation_mesh", "source_geometry_data", "root_node", "callback"), &NavigationMeshGenerator::parse_source_geometry_data, DEFVAL(Callable()));
+	ClassDB::bind_method(D_METHOD("bake_from_source_geometry_data", "navigation_mesh", "source_geometry_data", "callback"), &NavigationMeshGenerator::bake_from_source_geometry_data, DEFVAL(Callable()));
+}

modules/navigation_3d/3d/navigation_mesh_generator.h

@@ -0,0 +1,57 @@
+/**************************************************************************/
+/*  navigation_mesh_generator.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 "scene/3d/navigation/navigation_region_3d.h"
+#include "scene/resources/navigation_mesh.h"
+
+class NavigationMeshSourceGeometryData3D;
+
+class NavigationMeshGenerator : public Object {
+	GDCLASS(NavigationMeshGenerator, Object);
+
+	static NavigationMeshGenerator *singleton;
+
+protected:
+	static void _bind_methods();
+
+public:
+	static NavigationMeshGenerator *get_singleton();
+
+	NavigationMeshGenerator();
+	~NavigationMeshGenerator();
+
+	void bake(const Ref<NavigationMesh> &p_navigation_mesh, Node *p_root_node);
+	void clear(Ref<NavigationMesh> p_navigation_mesh);
+
+	void parse_source_geometry_data(const Ref<NavigationMesh> &p_navigation_mesh, Ref<NavigationMeshSourceGeometryData3D> p_source_geometry_data, Node *p_root_node, const Callable &p_callback = Callable());
+	void bake_from_source_geometry_data(Ref<NavigationMesh> p_navigation_mesh, const Ref<NavigationMeshSourceGeometryData3D> &p_source_geometry_data, const Callable &p_callback = Callable());
+};

modules/navigation_3d/SCsub

@@ -0,0 +1,87 @@
+#!/usr/bin/env python
+from misc.utility.scons_hints import *
+
+Import("env")
+Import("env_modules")
+
+env_navigation_3d = env_modules.Clone()
+
+# Thirdparty source files
+
+thirdparty_obj = []
+navigation_2d_enabled = "navigation_2d" in env.module_list
+
+# Recast Thirdparty source files
+if env["builtin_recastnavigation"]:
+    thirdparty_dir = "#thirdparty/recastnavigation/Recast/"
+    thirdparty_sources = [
+        "Source/Recast.cpp",
+        "Source/RecastAlloc.cpp",
+        "Source/RecastArea.cpp",
+        "Source/RecastAssert.cpp",
+        "Source/RecastContour.cpp",
+        "Source/RecastFilter.cpp",
+        "Source/RecastLayers.cpp",
+        "Source/RecastMesh.cpp",
+        "Source/RecastMeshDetail.cpp",
+        "Source/RecastRasterization.cpp",
+        "Source/RecastRegion.cpp",
+    ]
+    thirdparty_sources = [thirdparty_dir + file for file in thirdparty_sources]
+
+    env_navigation_3d.Prepend(CPPEXTPATH=[thirdparty_dir + "Include"])
+
+    env_thirdparty = env_navigation_3d.Clone()
+    env_thirdparty.disable_warnings()
+    env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources)
+
+# 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_3d.Prepend(CPPEXTPATH=[thirdparty_dir])
+
+    # Don't build rvo_2d if 2D navigation is enabled.
+    if not navigation_2d_enabled:
+        env_thirdparty = env_navigation_3d.Clone()
+        env_thirdparty.disable_warnings()
+        env_thirdparty.add_source_files(thirdparty_obj, thirdparty_sources)
+
+# RVO 3D Thirdparty source files
+if env["builtin_rvo2_3d"]:
+    thirdparty_dir = "#thirdparty/rvo2/rvo2_3d/"
+    thirdparty_sources = [
+        "Agent3d.cpp",
+        "KdTree3d.cpp",
+        "RVOSimulator3d.cpp",
+    ]
+    thirdparty_sources = [thirdparty_dir + file for file in thirdparty_sources]
+
+    env_navigation_3d.Prepend(CPPEXTPATH=[thirdparty_dir])
+
+    env_thirdparty = env_navigation_3d.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_3d.add_source_files(module_obj, "*.cpp")
+env_navigation_3d.add_source_files(module_obj, "3d/*.cpp")
+if env.editor_build:
+    env_navigation_3d.add_source_files(module_obj, "editor/*.cpp")
+env.modules_sources += module_obj
+
+# Needed to force rebuilding the module files when the thirdparty library is updated.
+env.Depends(module_obj, thirdparty_obj)

modules/navigation_3d/config.py

@@ -0,0 +1,10 @@
+def can_build(env, platform):
+    if env["disable_navigation_3d"]:
+        return False
+
+    env.module_add_dependencies("navigation", ["csg", "gridmap"], True)
+    return True
+
+
+def configure(env):
+    pass

modules/navigation_3d/editor/navigation_link_3d_editor_plugin.cpp

@@ -0,0 +1,46 @@
+/**************************************************************************/
+/*  navigation_link_3d_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_3d_editor_plugin.h"
+
+#include "editor/scene/3d/node_3d_editor_plugin.h"
+#include "scene/3d/navigation/navigation_link_3d.h"
+
+void NavigationLink3DEditorPlugin::edit(Object *p_object) {
+}
+
+bool NavigationLink3DEditorPlugin::handles(Object *p_object) const {
+	return Object::cast_to<NavigationLink3D>(p_object) != nullptr;
+}
+
+NavigationLink3DEditorPlugin::NavigationLink3DEditorPlugin() {
+	gizmo_plugin.instantiate();
+	Node3DEditor::get_singleton()->add_gizmo_plugin(gizmo_plugin);
+}

modules/navigation_3d/editor/navigation_link_3d_editor_plugin.h

@@ -0,0 +1,49 @@
+/**************************************************************************/
+/*  navigation_link_3d_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 "navigation_link_3d_gizmo_plugin.h"
+
+class NavigationLink3DEditorPlugin : public EditorPlugin {
+	GDCLASS(NavigationLink3DEditorPlugin, EditorPlugin);
+
+	Ref<NavigationLink3DGizmoPlugin> gizmo_plugin;
+
+public:
+	virtual String get_plugin_name() const override { return "NavigationLink3D"; }
+	bool has_main_screen() const override { return false; }
+	virtual void edit(Object *p_object) override;
+	virtual bool handles(Object *p_object) const override;
+
+	NavigationLink3DEditorPlugin();
+};

modules/navigation_3d/editor/navigation_link_3d_gizmo_plugin.cpp

@@ -0,0 +1,272 @@
+/**************************************************************************/
+/*  navigation_link_3d_gizmo_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_3d_gizmo_plugin.h"
+
+#include "editor/editor_undo_redo_manager.h"
+#include "editor/scene/3d/node_3d_editor_plugin.h"
+#include "scene/3d/navigation/navigation_link_3d.h"
+#include "servers/navigation_server_3d.h"
+
+NavigationLink3DGizmoPlugin::NavigationLink3DGizmoPlugin() {
+	create_material("navigation_link_material", NavigationServer3D::get_singleton()->get_debug_navigation_link_connection_color());
+	create_material("navigation_link_material_disabled", NavigationServer3D::get_singleton()->get_debug_navigation_link_connection_disabled_color());
+	create_handle_material("handles");
+}
+
+bool NavigationLink3DGizmoPlugin::has_gizmo(Node3D *p_spatial) {
+	return Object::cast_to<NavigationLink3D>(p_spatial) != nullptr;
+}
+
+String NavigationLink3DGizmoPlugin::get_gizmo_name() const {
+	return "NavigationLink3D";
+}
+
+int NavigationLink3DGizmoPlugin::get_priority() const {
+	return -1;
+}
+
+void NavigationLink3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) {
+	NavigationLink3D *link = Object::cast_to<NavigationLink3D>(p_gizmo->get_node_3d());
+
+	RID nav_map = link->get_world_3d()->get_navigation_map();
+	real_t search_radius = NavigationServer3D::get_singleton()->map_get_link_connection_radius(nav_map);
+	Vector3 up_vector = NavigationServer3D::get_singleton()->map_get_up(nav_map);
+	Vector3::Axis up_axis = up_vector.max_axis_index();
+
+	Vector3 start_position = link->get_start_position();
+	Vector3 end_position = link->get_end_position();
+
+	Ref<Material> link_material = get_material("navigation_link_material", p_gizmo);
+	Ref<Material> link_material_disabled = get_material("navigation_link_material_disabled", p_gizmo);
+	Ref<Material> handles_material = get_material("handles");
+
+	p_gizmo->clear();
+
+	// Number of points in an octant. So there will be 8 * points_in_octant points in total.
+	// Correspond to the smoothness of the circle.
+	const uint32_t points_in_octant = 8;
+	real_t inc = (Math::PI / (4 * points_in_octant));
+
+	Vector<Vector3> lines;
+	// points_in_octant * 8 * 2 per circle * 2 circles. 2 for the start-end. 4 for the arrow, and another 4 if bidirectionnal.
+	lines.resize(points_in_octant * 8 * 2 * 2 + 2 + 4 + (link->is_bidirectional() ? 4 : 0));
+	uint32_t index = 0;
+	Vector3 *lines_ptrw = lines.ptrw();
+	// Draw line between the points.
+	lines_ptrw[index++] = start_position;
+	lines_ptrw[index++] = end_position;
+	real_t search_radius_squared = search_radius * search_radius;
+
+	// Draw circles at start and end positions in one go.
+	real_t r = 0;
+	Vector2 a = Vector2(search_radius, 0);
+	for (uint32_t i = 0; i < points_in_octant; i++) {
+		r += inc;
+		real_t x = Math::cos(r) * search_radius;
+		real_t y = Math::sqrt(search_radius_squared - (x * x));
+
+		// Draw axis-aligned circle.
+		switch (up_axis) {
+			case Vector3::AXIS_X:
+#define PUSH_OCTANT(_position, a, b)                          \
+	lines_ptrw[index++] = _position + Vector3(0, a.x, a.y);   \
+	lines_ptrw[index++] = _position + Vector3(0, x, y);       \
+	lines_ptrw[index++] = _position + Vector3(0, -a.x, a.y);  \
+	lines_ptrw[index++] = _position + Vector3(0, x, y);       \
+	lines_ptrw[index++] = _position + Vector3(0, a.x, -a.y);  \
+	lines_ptrw[index++] = _position + Vector3(0, x, -y);      \
+	lines_ptrw[index++] = _position + Vector3(0, -a.x, -a.y); \
+	lines_ptrw[index++] = _position + Vector3(0, x, y);       \
+	lines_ptrw[index++] = _position + Vector3(0, a.y, a.x);   \
+	lines_ptrw[index++] = _position + Vector3(0, y, x);       \
+	lines_ptrw[index++] = _position + Vector3(0, -a.y, a.x);  \
+	lines_ptrw[index++] = _position + Vector3(0, -y, x);      \
+	lines_ptrw[index++] = _position + Vector3(0, a.y, -a.x);  \
+	lines_ptrw[index++] = _position + Vector3(0, y, -x);      \
+	lines_ptrw[index++] = _position + Vector3(0, -a.y, -a.x); \
+	lines_ptrw[index++] = _position + Vector3(0, -y, -x);
+
+				PUSH_OCTANT(start_position, a, b)
+				PUSH_OCTANT(end_position, a, b)
+#undef PUSH_OCTANT
+				break;
+			case Vector3::AXIS_Y:
+#define PUSH_OCTANT(_position, a, b)                          \
+	lines_ptrw[index++] = _position + Vector3(a.x, 0, a.y);   \
+	lines_ptrw[index++] = _position + Vector3(x, 0, y);       \
+	lines_ptrw[index++] = _position + Vector3(-a.x, 0, a.y);  \
+	lines_ptrw[index++] = _position + Vector3(-x, 0, y);      \
+	lines_ptrw[index++] = _position + Vector3(a.x, 0, -a.y);  \
+	lines_ptrw[index++] = _position + Vector3(x, 0, -y);      \
+	lines_ptrw[index++] = _position + Vector3(-a.x, 0, -a.y); \
+	lines_ptrw[index++] = _position + Vector3(-x, 0, -y);     \
+	lines_ptrw[index++] = _position + Vector3(a.y, 0, a.x);   \
+	lines_ptrw[index++] = _position + Vector3(y, 0, x);       \
+	lines_ptrw[index++] = _position + Vector3(-a.y, 0, a.x);  \
+	lines_ptrw[index++] = _position + Vector3(-y, 0, x);      \
+	lines_ptrw[index++] = _position + Vector3(a.y, 0, -a.x);  \
+	lines_ptrw[index++] = _position + Vector3(y, 0, -x);      \
+	lines_ptrw[index++] = _position + Vector3(-a.y, 0, -a.x); \
+	lines_ptrw[index++] = _position + Vector3(-y, 0, -x);
+
+				PUSH_OCTANT(start_position, a, b)
+				PUSH_OCTANT(end_position, a, b)
+#undef PUSH_OCTANT
+				break;
+			case Vector3::AXIS_Z:
+#define PUSH_OCTANT(_position, a, b)                          \
+	lines_ptrw[index++] = _position + Vector3(a.x, a.y, 0);   \
+	lines_ptrw[index++] = _position + Vector3(x, y, 0);       \
+	lines_ptrw[index++] = _position + Vector3(-a.x, a.y, 0);  \
+	lines_ptrw[index++] = _position + Vector3(-x, y, 0);      \
+	lines_ptrw[index++] = _position + Vector3(a.x, -a.y, 0);  \
+	lines_ptrw[index++] = _position + Vector3(x, -y, 0);      \
+	lines_ptrw[index++] = _position + Vector3(-a.x, -a.y, 0); \
+	lines_ptrw[index++] = _position + Vector3(-x, -y, 0);     \
+	lines_ptrw[index++] = _position + Vector3(a.y, a.x, 0);   \
+	lines_ptrw[index++] = _position + Vector3(y, x, 0);       \
+	lines_ptrw[index++] = _position + Vector3(-a.y, a.x, 0);  \
+	lines_ptrw[index++] = _position + Vector3(-y, x, 0);      \
+	lines_ptrw[index++] = _position + Vector3(a.y, -a.x, 0);  \
+	lines_ptrw[index++] = _position + Vector3(y, -x, 0);      \
+	lines_ptrw[index++] = _position + Vector3(-a.y, -a.x, 0); \
+	lines_ptrw[index++] = _position + Vector3(-y, -x, 0);
+
+				PUSH_OCTANT(start_position, a, b)
+				PUSH_OCTANT(end_position, a, b)
+#undef PUSH_OCTANT
+				break;
+		}
+		a.x = x;
+		a.y = y;
+	}
+
+	const Vector3 link_segment = end_position - start_position;
+	const Vector3 up = Vector3(0.0, 1.0, 0.0);
+	const float arror_len = 0.5;
+
+	{
+		Vector3 anchor = start_position + (link_segment * 0.75);
+		Vector3 direction = start_position.direction_to(end_position);
+		Vector3 arrow_dir = direction.cross(up);
+		lines_ptrw[index++] = anchor;
+		lines_ptrw[index++] = anchor + (arrow_dir - direction) * arror_len;
+
+		arrow_dir = -direction.cross(up);
+		lines_ptrw[index++] = anchor;
+		lines_ptrw[index++] = anchor + (arrow_dir - direction) * arror_len;
+	}
+
+	if (link->is_bidirectional()) {
+		Vector3 anchor = start_position + (link_segment * 0.25);
+		Vector3 direction = end_position.direction_to(start_position);
+		Vector3 arrow_dir = direction.cross(up);
+		lines_ptrw[index++] = anchor;
+		lines_ptrw[index++] = anchor + (arrow_dir - direction) * arror_len;
+
+		arrow_dir = -direction.cross(up);
+		lines_ptrw[index++] = anchor;
+		lines_ptrw[index++] = anchor + (arrow_dir - direction) * arror_len;
+	}
+
+	p_gizmo->add_lines(lines, link->is_enabled() ? link_material : link_material_disabled);
+	p_gizmo->add_collision_segments(lines);
+
+	p_gizmo->add_handles(Vector({ start_position, end_position }), handles_material);
+}
+
+String NavigationLink3DGizmoPlugin::get_handle_name(const EditorNode3DGizmo *p_gizmo, int p_id, bool p_secondary) const {
+	return p_id == 0 ? TTR("Start Position") : TTR("End Position");
+}
+
+Variant NavigationLink3DGizmoPlugin::get_handle_value(const EditorNode3DGizmo *p_gizmo, int p_id, bool p_secondary) const {
+	NavigationLink3D *link = Object::cast_to<NavigationLink3D>(p_gizmo->get_node_3d());
+	return p_id == 0 ? link->get_start_position() : link->get_end_position();
+}
+
+void NavigationLink3DGizmoPlugin::set_handle(const EditorNode3DGizmo *p_gizmo, int p_id, bool p_secondary, Camera3D *p_camera, const Point2 &p_point) {
+	NavigationLink3D *link = Object::cast_to<NavigationLink3D>(p_gizmo->get_node_3d());
+
+	Transform3D gt = link->get_global_transform();
+	Transform3D gi = gt.affine_inverse();
+
+	Transform3D ct = p_camera->get_global_transform();
+	Vector3 cam_dir = ct.basis.get_column(Vector3::AXIS_Z);
+
+	Vector3 ray_from = p_camera->project_ray_origin(p_point);
+	Vector3 ray_dir = p_camera->project_ray_normal(p_point);
+
+	Vector3 position = p_id == 0 ? link->get_start_position() : link->get_end_position();
+	Plane move_plane = Plane(cam_dir, gt.xform(position));
+
+	Vector3 intersection;
+	if (!move_plane.intersects_ray(ray_from, ray_dir, &intersection)) {
+		return;
+	}
+
+	if (Node3DEditor::get_singleton()->is_snap_enabled()) {
+		double snap = Node3DEditor::get_singleton()->get_translate_snap();
+		intersection.snapf(snap);
+	}
+
+	position = gi.xform(intersection);
+	if (p_id == 0) {
+		link->set_start_position(position);
+	} else if (p_id == 1) {
+		link->set_end_position(position);
+	}
+}
+
+void NavigationLink3DGizmoPlugin::commit_handle(const EditorNode3DGizmo *p_gizmo, int p_id, bool p_secondary, const Variant &p_restore, bool p_cancel) {
+	NavigationLink3D *link = Object::cast_to<NavigationLink3D>(p_gizmo->get_node_3d());
+
+	if (p_cancel) {
+		if (p_id == 0) {
+			link->set_start_position(p_restore);
+		} else {
+			link->set_end_position(p_restore);
+		}
+		return;
+	}
+
+	EditorUndoRedoManager *ur = EditorUndoRedoManager::get_singleton();
+	if (p_id == 0) {
+		ur->create_action(TTR("Change Start Position"));
+		ur->add_do_method(link, "set_start_position", link->get_start_position());
+		ur->add_undo_method(link, "set_start_position", p_restore);
+	} else {
+		ur->create_action(TTR("Change End Position"));
+		ur->add_do_method(link, "set_end_position", link->get_end_position());
+		ur->add_undo_method(link, "set_end_position", p_restore);
+	}
+
+	ur->commit_action();
+}

modules/navigation_3d/editor/navigation_link_3d_gizmo_plugin.h

@@ -0,0 +1,50 @@
+/**************************************************************************/
+/*  navigation_link_3d_gizmo_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/3d/node_3d_editor_gizmos.h"
+
+class NavigationLink3DGizmoPlugin : public EditorNode3DGizmoPlugin {
+	GDCLASS(NavigationLink3DGizmoPlugin, EditorNode3DGizmoPlugin);
+
+public:
+	bool has_gizmo(Node3D *p_spatial) override;
+	String get_gizmo_name() const override;
+	int get_priority() const override;
+	void redraw(EditorNode3DGizmo *p_gizmo) override;
+
+	String get_handle_name(const EditorNode3DGizmo *p_gizmo, int p_id, bool p_secondary) const override;
+	Variant get_handle_value(const EditorNode3DGizmo *p_gizmo, int p_id, bool p_secondary) const override;
+	void set_handle(const EditorNode3DGizmo *p_gizmo, int p_id, bool p_secondary, Camera3D *p_camera, const Point2 &p_point) override;
+	void commit_handle(const EditorNode3DGizmo *p_gizmo, int p_id, bool p_secondary, const Variant &p_restore, bool p_cancel = false) override;
+
+	NavigationLink3DGizmoPlugin();
+};

modules/navigation_3d/editor/navigation_obstacle_3d_editor_plugin.cpp

@@ -0,0 +1,901 @@
+/**************************************************************************/
+/*  navigation_obstacle_3d_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_3d_editor_plugin.h"
+
+#include "core/math/geometry_2d.h"
+#include "editor/editor_node.h"
+#include "editor/editor_string_names.h"
+#include "editor/editor_undo_redo_manager.h"
+#include "editor/scene/3d/node_3d_editor_plugin.h"
+#include "editor/settings/editor_settings.h"
+#include "scene/3d/navigation/navigation_obstacle_3d.h"
+#include "scene/gui/button.h"
+#include "scene/gui/dialogs.h"
+#include "servers/navigation_server_3d.h"
+
+bool NavigationObstacle3DGizmoPlugin::has_gizmo(Node3D *p_spatial) {
+	return Object::cast_to<NavigationObstacle3D>(p_spatial) != nullptr;
+}
+
+String NavigationObstacle3DGizmoPlugin::get_gizmo_name() const {
+	return "NavigationObstacle3D";
+}
+
+void NavigationObstacle3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) {
+	p_gizmo->clear();
+
+	if (!p_gizmo->is_selected() && get_state() == HIDDEN) {
+		return;
+	}
+
+	NavigationObstacle3D *obstacle = Object::cast_to<NavigationObstacle3D>(p_gizmo->get_node_3d());
+
+	if (!obstacle) {
+		return;
+	}
+
+	const Vector<Vector3> &vertices = obstacle->get_vertices();
+	if (vertices.is_empty()) {
+		return;
+	}
+
+	float height = obstacle->get_height();
+
+	const Basis safe_basis = Basis(Vector3(0.0, 1.0, 0.0), obstacle->get_global_rotation().y, obstacle->get_global_basis().get_scale().abs().maxf(0.001));
+	const Basis gbi = obstacle->get_global_basis().inverse();
+	const Basis safe_global_basis = gbi * safe_basis;
+	const int vertex_count = vertices.size();
+
+	Vector<Vector3> lines_mesh_vertices;
+	lines_mesh_vertices.resize(vertex_count * 8);
+	Vector3 *lines_mesh_vertices_ptrw = lines_mesh_vertices.ptrw();
+
+	int vertex_index = 0;
+
+	for (int i = 0; i < vertex_count; i++) {
+		Vector3 point = vertices[i];
+		Vector3 next_point = vertices[(i + 1) % vertex_count];
+
+		Vector3 direction = safe_basis.xform(next_point.direction_to(point));
+		Vector3 arrow_dir = direction.cross(Vector3(0.0, 1.0, 0.0));
+		Vector3 edge_middle = point + ((next_point - point) * 0.5);
+
+		// Ensure vector stays perpendicular even when scaled non-uniformly.
+		lines_mesh_vertices_ptrw[vertex_index++] = safe_global_basis.xform(edge_middle);
+		lines_mesh_vertices_ptrw[vertex_index++] = safe_global_basis.xform(edge_middle) + gbi.xform(arrow_dir) * 0.5;
+
+		lines_mesh_vertices_ptrw[vertex_index++] = safe_global_basis.xform(point);
+		lines_mesh_vertices_ptrw[vertex_index++] = safe_global_basis.xform(next_point);
+
+		lines_mesh_vertices_ptrw[vertex_index++] = safe_global_basis.xform(Vector3(point.x, height, point.z));
+		lines_mesh_vertices_ptrw[vertex_index++] = safe_global_basis.xform(Vector3(next_point.x, height, next_point.z));
+
+		lines_mesh_vertices_ptrw[vertex_index++] = safe_global_basis.xform(point);
+		lines_mesh_vertices_ptrw[vertex_index++] = safe_global_basis.xform(Vector3(point.x, height, point.z));
+	}
+
+	NavigationServer3D *ns3d = NavigationServer3D::get_singleton();
+
+	if (obstacle->are_vertices_valid()) {
+		p_gizmo->add_lines(lines_mesh_vertices, ns3d->get_debug_navigation_avoidance_static_obstacle_pushout_edge_material());
+	} else {
+		p_gizmo->add_lines(lines_mesh_vertices, ns3d->get_debug_navigation_avoidance_static_obstacle_pushin_edge_material());
+	}
+	p_gizmo->add_collision_segments(lines_mesh_vertices);
+
+	if (p_gizmo->is_selected()) {
+		NavigationObstacle3DEditorPlugin::singleton->redraw();
+	}
+}
+
+bool NavigationObstacle3DGizmoPlugin::can_be_hidden() const {
+	return true;
+}
+
+int NavigationObstacle3DGizmoPlugin::get_priority() const {
+	return -1;
+}
+
+int NavigationObstacle3DGizmoPlugin::subgizmos_intersect_ray(const EditorNode3DGizmo *p_gizmo, Camera3D *p_camera, const Vector2 &p_point) const {
+	if (NavigationObstacle3DEditorPlugin::singleton->get_mode() != 1) { // MODE_EDIT
+		return -1;
+	}
+
+	NavigationObstacle3D *obstacle_node = Object::cast_to<NavigationObstacle3D>(p_gizmo->get_node_3d());
+	ERR_FAIL_NULL_V(obstacle_node, -1);
+
+	const Vector3 safe_scale = obstacle_node->get_global_basis().get_scale().abs().maxf(0.001);
+	const Transform3D gt = Transform3D(Basis().scaled(safe_scale).rotated(Vector3(0.0, 1.0, 0.0), obstacle_node->get_global_rotation().y), obstacle_node->get_global_position());
+	const Vector<Vector3> &vertices = obstacle_node->get_vertices();
+
+	for (int idx = 0; idx < vertices.size(); ++idx) {
+		Vector3 pos = gt.xform(vertices[idx]);
+		if (p_camera->unproject_position(pos).distance_to(p_point) < 20) {
+			return idx;
+		}
+	}
+
+	return -1;
+}
+
+Vector<int> NavigationObstacle3DGizmoPlugin::subgizmos_intersect_frustum(const EditorNode3DGizmo *p_gizmo, const Camera3D *p_camera, const Vector<Plane> &p_frustum) const {
+	Vector<int> contained_points;
+	if (NavigationObstacle3DEditorPlugin::singleton->get_mode() != 1) { // MODE_EDIT
+		return contained_points;
+	}
+
+	NavigationObstacle3D *obstacle_node = Object::cast_to<NavigationObstacle3D>(p_gizmo->get_node_3d());
+	ERR_FAIL_NULL_V(obstacle_node, contained_points);
+
+	const Vector3 safe_scale = obstacle_node->get_global_basis().get_scale().abs().maxf(0.001);
+	const Transform3D gt = Transform3D(Basis().scaled(safe_scale).rotated(Vector3(0.0, 1.0, 0.0), obstacle_node->get_global_rotation().y), obstacle_node->get_global_position());
+	const Vector<Vector3> &vertices = obstacle_node->get_vertices();
+
+	for (int idx = 0; idx < vertices.size(); ++idx) {
+		Vector3 pos = gt.xform(vertices[idx]);
+		bool is_contained_in_frustum = true;
+		for (int i = 0; i < p_frustum.size(); ++i) {
+			if (p_frustum[i].distance_to(pos) > 0) {
+				is_contained_in_frustum = false;
+				break;
+			}
+		}
+
+		if (is_contained_in_frustum) {
+			contained_points.push_back(idx);
+		}
+	}
+
+	return contained_points;
+}
+
+Transform3D NavigationObstacle3DGizmoPlugin::get_subgizmo_transform(const EditorNode3DGizmo *p_gizmo, int p_id) const {
+	NavigationObstacle3D *obstacle_node = Object::cast_to<NavigationObstacle3D>(p_gizmo->get_node_3d());
+	ERR_FAIL_NULL_V(obstacle_node, Transform3D());
+
+	const Vector<Vector3> &vertices = obstacle_node->get_vertices();
+	ERR_FAIL_INDEX_V(p_id, vertices.size(), Transform3D());
+
+	const Basis safe_basis_inverse = Basis(Vector3(0.0, 1.0, 0.0), obstacle_node->get_global_rotation().y, obstacle_node->get_global_basis().get_scale().abs().maxf(0.001)).inverse();
+	Transform3D subgizmo_transform = Transform3D(Basis(), safe_basis_inverse.xform(vertices[p_id]));
+	return subgizmo_transform;
+}
+
+void NavigationObstacle3DGizmoPlugin::set_subgizmo_transform(const EditorNode3DGizmo *p_gizmo, int p_id, Transform3D p_transform) {
+	NavigationObstacle3D *obstacle_node = Object::cast_to<NavigationObstacle3D>(p_gizmo->get_node_3d());
+	ERR_FAIL_NULL(obstacle_node);
+
+	const Basis safe_basis = Basis(Vector3(0.0, 1.0, 0.0), obstacle_node->get_global_rotation().y, obstacle_node->get_global_basis().get_scale().abs().maxf(0.001));
+	Vector3 new_vertex_pos = p_transform.origin;
+
+	Vector<Vector3> vertices = obstacle_node->get_vertices();
+	ERR_FAIL_INDEX(p_id, vertices.size());
+
+	Vector3 vertex = safe_basis.xform(new_vertex_pos);
+	vertex.y = 0.0;
+	vertices.write[p_id] = vertex;
+
+	obstacle_node->set_vertices(vertices);
+}
+
+void NavigationObstacle3DGizmoPlugin::commit_subgizmos(const EditorNode3DGizmo *p_gizmo, const Vector<int> &p_ids, const Vector<Transform3D> &p_restore, bool p_cancel) {
+	NavigationObstacle3D *obstacle_node = Object::cast_to<NavigationObstacle3D>(p_gizmo->get_node_3d());
+	ERR_FAIL_NULL(obstacle_node);
+
+	const Basis safe_basis = Basis(Vector3(0.0, 1.0, 0.0), obstacle_node->get_global_rotation().y, obstacle_node->get_global_basis().get_scale().abs().maxf(0.001));
+
+	Vector<Vector3> vertices = obstacle_node->get_vertices();
+	Vector<Vector3> restore_vertices = vertices;
+
+	for (int i = 0; i < p_ids.size(); ++i) {
+		const int idx = p_ids[i];
+		Vector3 vertex = safe_basis.xform(p_restore[i].origin);
+		vertex.y = 0.0;
+		restore_vertices.write[idx] = vertex;
+	}
+
+	if (p_cancel) {
+		obstacle_node->set_vertices(restore_vertices);
+		return;
+	}
+
+	EditorUndoRedoManager *undo_redo = EditorUndoRedoManager::get_singleton();
+	undo_redo->create_action(TTR("Set Obstacle Vertices"));
+	undo_redo->add_do_method(obstacle_node, "set_vertices", vertices);
+	undo_redo->add_undo_method(obstacle_node, "set_vertices", restore_vertices);
+	undo_redo->commit_action();
+}
+
+NavigationObstacle3DGizmoPlugin::NavigationObstacle3DGizmoPlugin() {
+	current_state = VISIBLE;
+}
+
+void NavigationObstacle3DEditorPlugin::_notification(int p_what) {
+	switch (p_what) {
+		case NOTIFICATION_ENTER_TREE: {
+			_update_theme();
+		} break;
+
+		case NOTIFICATION_READY: {
+			_update_theme();
+			button_edit->set_pressed(true);
+			get_tree()->connect("node_removed", callable_mp(this, &NavigationObstacle3DEditorPlugin::_node_removed));
+			EditorNode::get_singleton()->get_gui_base()->connect(SceneStringName(theme_changed), callable_mp(this, &NavigationObstacle3DEditorPlugin::_update_theme));
+		} break;
+
+		case NOTIFICATION_EXIT_TREE: {
+			get_tree()->disconnect("node_removed", callable_mp(this, &NavigationObstacle3DEditorPlugin::_node_removed));
+			EditorNode::get_singleton()->get_gui_base()->disconnect(SceneStringName(theme_changed), callable_mp(this, &NavigationObstacle3DEditorPlugin::_update_theme));
+		} break;
+	}
+}
+
+void NavigationObstacle3DEditorPlugin::edit(Object *p_object) {
+	obstacle_node = Object::cast_to<NavigationObstacle3D>(p_object);
+
+	RenderingServer *rs = RenderingServer::get_singleton();
+
+	if (obstacle_node) {
+		if (obstacle_node->get_vertices().is_empty()) {
+			set_mode(MODE_CREATE);
+		} else {
+			set_mode(MODE_EDIT);
+		}
+		wip_vertices.clear();
+		wip_active = false;
+		edited_point = -1;
+
+		rs->instance_set_scenario(point_lines_instance_rid, obstacle_node->get_world_3d()->get_scenario());
+		rs->instance_set_scenario(point_handles_instance_rid, obstacle_node->get_world_3d()->get_scenario());
+
+		redraw();
+
+	} else {
+		obstacle_node = nullptr;
+
+		rs->mesh_clear(point_lines_mesh_rid);
+		rs->mesh_clear(point_handle_mesh_rid);
+		rs->instance_set_scenario(point_lines_instance_rid, RID());
+		rs->instance_set_scenario(point_handles_instance_rid, RID());
+	}
+}
+
+bool NavigationObstacle3DEditorPlugin::handles(Object *p_object) const {
+	return Object::cast_to<NavigationObstacle3D>(p_object);
+}
+
+void NavigationObstacle3DEditorPlugin::make_visible(bool p_visible) {
+	if (p_visible) {
+		obstacle_editor->show();
+	} else {
+		obstacle_editor->hide();
+		edit(nullptr);
+	}
+}
+
+void NavigationObstacle3DEditorPlugin::action_flip_vertices() {
+	if (!obstacle_node) {
+		return;
+	}
+
+	Vector<Vector3> flipped_vertices = obstacle_node->get_vertices();
+	flipped_vertices.reverse();
+
+	EditorUndoRedoManager *undo_redo = EditorUndoRedoManager::get_singleton();
+	undo_redo->create_action(TTR("Edit Obstacle (Flip Winding)"));
+	undo_redo->add_do_method(obstacle_node, "set_vertices", flipped_vertices);
+	undo_redo->add_undo_method(obstacle_node, "set_vertices", obstacle_node->get_vertices());
+	undo_redo->commit_action();
+
+	obstacle_node->update_gizmos();
+}
+
+void NavigationObstacle3DEditorPlugin::action_clear_vertices() {
+	if (!obstacle_node) {
+		return;
+	}
+
+	EditorUndoRedoManager *undo_redo = EditorUndoRedoManager::get_singleton();
+	undo_redo->create_action(TTR("Edit Obstacle (Clear Vertices)"));
+	undo_redo->add_do_method(obstacle_node, "set_vertices", Vector<Vector3>());
+	undo_redo->add_undo_method(obstacle_node, "set_vertices", obstacle_node->get_vertices());
+	undo_redo->commit_action();
+
+	obstacle_node->update_gizmos();
+	edit(obstacle_node);
+}
+
+void NavigationObstacle3DEditorPlugin::_update_theme() {
+	button_create->set_tooltip_text(TTR("Add Vertex"));
+	button_edit->set_tooltip_text(TTR("Edit Vertex"));
+	button_delete->set_tooltip_text(TTR("Delete Vertex"));
+	button_flip->set_tooltip_text(TTR("Flip Winding"));
+	button_clear->set_tooltip_text(TTR("Clear Vertices"));
+	button_create->set_button_icon(button_create->get_editor_theme_icon(SNAME("CurveCreate")));
+	button_edit->set_button_icon(button_edit->get_editor_theme_icon(SNAME("CurveEdit")));
+	button_delete->set_button_icon(button_delete->get_editor_theme_icon(SNAME("CurveDelete")));
+	button_flip->set_button_icon(button_flip->get_editor_theme_icon(SNAME("FlipWinding")));
+	button_clear->set_button_icon(button_clear->get_editor_theme_icon(SNAME("Clear")));
+}
+
+void NavigationObstacle3DEditorPlugin::_node_removed(Node *p_node) {
+	if (obstacle_node == p_node) {
+		obstacle_node = nullptr;
+
+		RenderingServer *rs = RenderingServer::get_singleton();
+		rs->mesh_clear(point_lines_mesh_rid);
+		rs->mesh_clear(point_handle_mesh_rid);
+
+		obstacle_editor->hide();
+	}
+}
+
+void NavigationObstacle3DEditorPlugin::set_mode(int p_option) {
+	if (p_option == NavigationObstacle3DEditorPlugin::ACTION_FLIP) {
+		button_flip->set_pressed(false);
+		action_flip_vertices();
+		return;
+	}
+
+	if (p_option == NavigationObstacle3DEditorPlugin::ACTION_CLEAR) {
+		button_clear->set_pressed(false);
+		button_clear_dialog->reset_size();
+		button_clear_dialog->popup_centered();
+		return;
+	}
+
+	mode = p_option;
+
+	button_create->set_pressed(p_option == NavigationObstacle3DEditorPlugin::MODE_CREATE);
+	button_edit->set_pressed(p_option == NavigationObstacle3DEditorPlugin::MODE_EDIT);
+	button_delete->set_pressed(p_option == NavigationObstacle3DEditorPlugin::MODE_DELETE);
+	button_flip->set_pressed(false);
+	button_clear->set_pressed(false);
+}
+
+void NavigationObstacle3DEditorPlugin::_wip_cancel() {
+	wip_vertices.clear();
+	wip_active = false;
+
+	edited_point = -1;
+
+	redraw();
+}
+
+void NavigationObstacle3DEditorPlugin::_wip_close() {
+	ERR_FAIL_NULL_MSG(obstacle_node, "Edited NavigationObstacle3D is not valid.");
+
+	Vector<Vector2> wip_2d_vertices;
+	wip_2d_vertices.resize(wip_vertices.size());
+	for (int i = 0; i < wip_vertices.size(); i++) {
+		const Vector3 &vert = wip_vertices[i];
+		wip_2d_vertices.write[i] = Vector2(vert.x, vert.z);
+	}
+	Vector<int> triangulated_polygon_2d_indices = Geometry2D::triangulate_polygon(wip_2d_vertices);
+
+	if (!triangulated_polygon_2d_indices.is_empty()) {
+		EditorUndoRedoManager *undo_redo = EditorUndoRedoManager::get_singleton();
+		undo_redo->create_action(TTR("Set Obstacle Vertices"));
+		undo_redo->add_do_method(obstacle_node, "set_vertices", wip_vertices);
+		undo_redo->add_undo_method(obstacle_node, "set_vertices", obstacle_node->get_vertices());
+		undo_redo->commit_action();
+
+		wip_vertices.clear();
+		wip_active = false;
+		//mode = MODE_EDIT;
+		NavigationObstacle3DEditorPlugin::singleton->set_mode(NavigationObstacle3DEditorPlugin::MODE_EDIT);
+		button_edit->set_pressed(true);
+		button_create->set_pressed(false);
+		edited_point = -1;
+	}
+}
+
+EditorPlugin::AfterGUIInput NavigationObstacle3DEditorPlugin::forward_3d_gui_input(Camera3D *p_camera, const Ref<InputEvent> &p_event) {
+	if (!obstacle_node) {
+		return EditorPlugin::AFTER_GUI_INPUT_PASS;
+	}
+
+	if (!obstacle_node->is_visible_in_tree()) {
+		return EditorPlugin::AFTER_GUI_INPUT_PASS;
+	}
+
+	Ref<InputEventMouse> mouse_event = p_event;
+
+	if (mouse_event.is_null()) {
+		return EditorPlugin::AFTER_GUI_INPUT_PASS;
+	}
+
+	Ref<InputEventMouseButton> mb = p_event;
+
+	if (mb.is_valid()) {
+		Vector2 mouse_position = mb->get_position();
+		Vector3 ray_from = p_camera->project_ray_origin(mouse_position);
+		Vector3 ray_dir = p_camera->project_ray_normal(mouse_position);
+
+		const Vector3 safe_scale = obstacle_node->get_global_basis().get_scale().abs().maxf(0.001);
+		const Transform3D gt = Transform3D(Basis().scaled(safe_scale).rotated(Vector3(0.0, 1.0, 0.0), obstacle_node->get_global_rotation().y), obstacle_node->get_global_position());
+		Transform3D gi = gt.affine_inverse();
+		Plane projection_plane(Vector3(0.0, 1.0, 0.0), gt.origin);
+
+		Vector3 spoint;
+
+		if (!projection_plane.intersects_ray(ray_from, ray_dir, &spoint)) {
+			return EditorPlugin::AFTER_GUI_INPUT_PASS;
+		}
+
+		spoint = gi.xform(spoint);
+
+		Vector3 cpoint = Vector3(spoint.x, 0.0, spoint.z);
+		Vector<Vector3> obstacle_vertices = obstacle_node->get_vertices();
+
+		real_t grab_threshold = EDITOR_GET("editors/polygon_editor/point_grab_radius");
+
+		switch (mode) {
+			case MODE_CREATE: {
+				if (mb->get_button_index() == MouseButton::LEFT && mb->is_pressed()) {
+					if (obstacle_vertices.size() >= 3) {
+						int closest_idx = -1;
+						Vector2 closest_edge_point;
+						real_t closest_dist = 1e10;
+						for (int i = 0; i < obstacle_vertices.size(); i++) {
+							const Vector2 a = p_camera->unproject_position(gt.xform(obstacle_vertices[i]));
+							const Vector2 b = p_camera->unproject_position(gt.xform(obstacle_vertices[(i + 1) % obstacle_vertices.size()]));
+
+							Vector2 cp = Geometry2D::get_closest_point_to_segment(mouse_position, a, b);
+							if (cp.distance_squared_to(a) < grab_threshold || cp.distance_squared_to(b) < grab_threshold) {
+								continue; // Skip edge as clicked point is too close to existing vertex.
+							}
+
+							real_t d = cp.distance_to(mouse_position);
+							if (d < closest_dist && d < grab_threshold) {
+								closest_dist = d;
+								closest_edge_point = cp;
+								closest_idx = i;
+							}
+						}
+						if (closest_idx >= 0) {
+							edited_point = -1;
+							Vector3 _ray_from = p_camera->project_ray_origin(closest_edge_point);
+							Vector3 _ray_dir = p_camera->project_ray_normal(closest_edge_point);
+							Vector3 edge_intersection_point;
+							if (projection_plane.intersects_ray(_ray_from, _ray_dir, &edge_intersection_point)) {
+								edge_intersection_point = gi.xform(edge_intersection_point);
+
+								EditorUndoRedoManager *undo_redo = EditorUndoRedoManager::get_singleton();
+								undo_redo->create_action(TTR("Edit Obstacle (Add Vertex)"));
+								undo_redo->add_undo_method(obstacle_node, "set_vertices", obstacle_vertices);
+								obstacle_vertices.insert(closest_idx + 1, edge_intersection_point);
+								undo_redo->add_do_method(obstacle_node, "set_vertices", obstacle_vertices);
+								undo_redo->commit_action();
+								redraw();
+								return EditorPlugin::AFTER_GUI_INPUT_STOP;
+							}
+						}
+					}
+					if (!wip_active) {
+						wip_vertices.clear();
+						wip_vertices.push_back(cpoint);
+						wip_active = true;
+						edited_point_pos = cpoint;
+						snap_ignore = false;
+						redraw();
+						edited_point = 1;
+						return EditorPlugin::AFTER_GUI_INPUT_STOP;
+					} else {
+						if (wip_vertices.size() > 1 && p_camera->unproject_position(gt.xform(wip_vertices[0])).distance_to(mouse_position) < grab_threshold) {
+							_wip_close();
+
+							return EditorPlugin::AFTER_GUI_INPUT_STOP;
+						} else {
+							wip_vertices.push_back(cpoint);
+							edited_point = wip_vertices.size();
+							snap_ignore = false;
+							redraw();
+							return EditorPlugin::AFTER_GUI_INPUT_STOP;
+						}
+					}
+				} else if (mb->get_button_index() == MouseButton::RIGHT && mb->is_pressed() && wip_active) {
+					_wip_close();
+				}
+
+			} break;
+
+			case MODE_EDIT: {
+				if (mb->get_button_index() == MouseButton::LEFT) {
+					if (mb->is_pressed()) {
+						if (mb->is_ctrl_pressed()) {
+							if (obstacle_vertices.size() < 3) {
+								EditorUndoRedoManager *undo_redo = EditorUndoRedoManager::get_singleton();
+								undo_redo->create_action(TTR("Edit Obstacle (Add Vertex)"));
+								undo_redo->add_undo_method(obstacle_node, "set_vertices", obstacle_node->get_vertices());
+								obstacle_vertices.push_back(cpoint);
+								undo_redo->commit_action();
+								return EditorPlugin::AFTER_GUI_INPUT_STOP;
+							}
+
+							//search edges
+							int closest_idx = -1;
+							Vector2 closest_pos;
+							real_t closest_dist = 1e10;
+							for (int i = 0; i < obstacle_vertices.size(); i++) {
+								const Vector2 a = p_camera->unproject_position(gt.xform(obstacle_vertices[i]));
+								const Vector2 b = p_camera->unproject_position(gt.xform(obstacle_vertices[(i + 1) % obstacle_vertices.size()]));
+
+								Vector2 cp = Geometry2D::get_closest_point_to_segment(mouse_position, a, b);
+								if (cp.distance_squared_to(a) < CMP_EPSILON2 || cp.distance_squared_to(b) < CMP_EPSILON2) {
+									continue; //not valid to reuse point
+								}
+
+								real_t d = cp.distance_to(mouse_position);
+								if (d < closest_dist && d < grab_threshold) {
+									closest_dist = d;
+									closest_pos = cp;
+									closest_idx = i;
+								}
+							}
+
+							if (closest_idx >= 0) {
+								pre_move_edit = obstacle_vertices;
+								obstacle_vertices.insert(closest_idx + 1, cpoint);
+								edited_point = closest_idx + 1;
+								edited_point_pos = cpoint;
+								obstacle_node->set_vertices(obstacle_vertices);
+								redraw();
+								snap_ignore = true;
+
+								return EditorPlugin::AFTER_GUI_INPUT_STOP;
+							}
+						} else {
+							int closest_idx = -1;
+							Vector2 closest_pos;
+							real_t closest_dist = 1e10;
+							for (int i = 0; i < obstacle_vertices.size(); i++) {
+								Vector2 cp = p_camera->unproject_position(gt.xform(obstacle_vertices[i]));
+
+								real_t d = cp.distance_to(mouse_position);
+								if (d < closest_dist && d < grab_threshold) {
+									closest_dist = d;
+									closest_pos = cp;
+									closest_idx = i;
+								}
+							}
+
+							if (closest_idx >= 0) {
+								pre_move_edit = obstacle_vertices;
+								edited_point = closest_idx;
+								edited_point_pos = obstacle_vertices[closest_idx];
+								redraw();
+								snap_ignore = false;
+								return EditorPlugin::AFTER_GUI_INPUT_STOP;
+							}
+						}
+					} else {
+						snap_ignore = false;
+
+						if (edited_point != -1) {
+							ERR_FAIL_INDEX_V(edited_point, obstacle_vertices.size(), EditorPlugin::AFTER_GUI_INPUT_PASS);
+							obstacle_vertices.write[edited_point] = edited_point_pos;
+
+							EditorUndoRedoManager *undo_redo = EditorUndoRedoManager::get_singleton();
+							undo_redo->create_action(TTR("Edit Obstacle (Move Vertex)"));
+							undo_redo->add_undo_method(obstacle_node, "set_vertices", obstacle_node->get_vertices());
+							undo_redo->add_do_method(obstacle_node, "set_vertices", obstacle_vertices);
+							undo_redo->commit_action();
+
+							edited_point = -1;
+							return EditorPlugin::AFTER_GUI_INPUT_STOP;
+						}
+					}
+				}
+
+			} break;
+
+			case MODE_DELETE: {
+				if (mb->get_button_index() == MouseButton::LEFT && mb->is_pressed()) {
+					int closest_idx = -1;
+					real_t closest_dist = 1e10;
+					for (int i = 0; i < obstacle_vertices.size(); i++) {
+						Vector2 point = p_camera->unproject_position(gt.xform(obstacle_vertices[i]));
+						real_t d = point.distance_to(mouse_position);
+						if (d < closest_dist && d < grab_threshold) {
+							closest_dist = d;
+							closest_idx = i;
+						}
+					}
+
+					if (closest_idx >= 0) {
+						edited_point = -1;
+						EditorUndoRedoManager *undo_redo = EditorUndoRedoManager::get_singleton();
+						undo_redo->create_action(TTR("Edit Obstacle (Remove Vertex)"));
+						undo_redo->add_undo_method(obstacle_node, "set_vertices", obstacle_vertices);
+						obstacle_vertices.remove_at(closest_idx);
+						undo_redo->add_do_method(obstacle_node, "set_vertices", obstacle_vertices);
+						undo_redo->commit_action();
+						redraw();
+						return EditorPlugin::AFTER_GUI_INPUT_STOP;
+					}
+				}
+
+			} break;
+		}
+	}
+
+	Ref<InputEventMouseMotion> mm = p_event;
+
+	if (mm.is_valid()) {
+		if (edited_point != -1 && (wip_active || mm->get_button_mask().has_flag(MouseButtonMask::LEFT))) {
+			Vector2 mouse_position = mm->get_position();
+
+			Vector3 ray_from = p_camera->project_ray_origin(mouse_position);
+			Vector3 ray_dir = p_camera->project_ray_normal(mouse_position);
+
+			const Vector3 safe_scale = obstacle_node->get_global_basis().get_scale().abs().maxf(0.001);
+			const Transform3D gt = Transform3D(Basis().scaled(safe_scale).rotated(Vector3(0.0, 1.0, 0.0), obstacle_node->get_global_rotation().y), obstacle_node->get_global_position());
+			Transform3D gi = gt.affine_inverse();
+			Plane projection_plane(Vector3(0.0, 1.0, 0.0), gt.origin);
+
+			Vector3 intersection_point;
+
+			if (!projection_plane.intersects_ray(ray_from, ray_dir, &intersection_point)) {
+				return EditorPlugin::AFTER_GUI_INPUT_PASS;
+			}
+
+			intersection_point = gi.xform(intersection_point);
+
+			Vector2 cpoint(intersection_point.x, intersection_point.z);
+
+			if (snap_ignore && !Input::get_singleton()->is_key_pressed(Key::CTRL)) {
+				snap_ignore = false;
+			}
+
+			if (!snap_ignore && Node3DEditor::get_singleton()->is_snap_enabled()) {
+				cpoint = cpoint.snappedf(Node3DEditor::get_singleton()->get_translate_snap());
+			}
+			edited_point_pos = Vector3(cpoint.x, 0.0, cpoint.y);
+
+			redraw();
+		}
+	}
+
+	Ref<InputEventKey> k = p_event;
+
+	if (k.is_valid() && k->is_pressed()) {
+		if (wip_active && k->get_keycode() == Key::ENTER) {
+			_wip_close();
+		} else if (wip_active && k->get_keycode() == Key::ESCAPE) {
+			_wip_cancel();
+		}
+	}
+
+	return EditorPlugin::AFTER_GUI_INPUT_PASS;
+}
+
+void NavigationObstacle3DEditorPlugin::redraw() {
+	if (!obstacle_node) {
+		return;
+	}
+	RenderingServer *rs = RenderingServer::get_singleton();
+
+	rs->mesh_clear(point_lines_mesh_rid);
+	rs->mesh_clear(point_handle_mesh_rid);
+
+	if (!obstacle_node->is_visible_in_tree()) {
+		return;
+	}
+
+	Vector<Vector3> edited_vertices;
+
+	if (wip_active) {
+		edited_vertices = wip_vertices;
+	} else {
+		edited_vertices = obstacle_node->get_vertices();
+	}
+
+	if (edited_vertices.is_empty()) {
+		return;
+	}
+
+	Array point_lines_mesh_array;
+	point_lines_mesh_array.resize(Mesh::ARRAY_MAX);
+
+	Vector<Vector3> point_lines_mesh_vertices;
+	point_lines_mesh_vertices.resize(edited_vertices.size() * 2);
+	Vector3 *point_lines_mesh_vertices_ptr = point_lines_mesh_vertices.ptrw();
+
+	int vertex_index = 0;
+
+	for (int i = 0; i < edited_vertices.size(); i++) {
+		Vector3 point, next_point;
+		if (i == edited_point) {
+			point = edited_point_pos;
+		} else {
+			point = edited_vertices[i];
+		}
+
+		if ((wip_active && i == edited_vertices.size() - 1) || (((i + 1) % edited_vertices.size()) == edited_point)) {
+			next_point = edited_point_pos;
+		} else {
+			next_point = edited_vertices[(i + 1) % edited_vertices.size()];
+		}
+
+		point_lines_mesh_vertices_ptr[vertex_index++] = point;
+		point_lines_mesh_vertices_ptr[vertex_index++] = next_point;
+	}
+
+	point_lines_mesh_array[Mesh::ARRAY_VERTEX] = point_lines_mesh_vertices;
+
+	rs->mesh_add_surface_from_arrays(point_lines_mesh_rid, RS::PRIMITIVE_LINES, point_lines_mesh_array);
+	rs->instance_set_surface_override_material(point_lines_instance_rid, 0, line_material->get_rid());
+	const Vector3 safe_scale = obstacle_node->get_global_basis().get_scale().abs().maxf(0.001);
+	const Transform3D gt = Transform3D(Basis().scaled(safe_scale).rotated(Vector3(0.0, 1.0, 0.0), obstacle_node->get_global_rotation().y), obstacle_node->get_global_position());
+	rs->instance_set_transform(point_lines_instance_rid, gt);
+
+	Array point_handle_mesh_array;
+	point_handle_mesh_array.resize(Mesh::ARRAY_MAX);
+	Vector<Vector3> point_handle_mesh_vertices;
+
+	point_handle_mesh_vertices.resize(edited_vertices.size());
+	Vector3 *point_handle_mesh_vertices_ptr = point_handle_mesh_vertices.ptrw();
+
+	for (int i = 0; i < edited_vertices.size(); i++) {
+		Vector3 point_handle_3d;
+
+		if (i == edited_point) {
+			point_handle_3d = edited_point_pos;
+		} else {
+			point_handle_3d = edited_vertices[i];
+		}
+
+		point_handle_mesh_vertices_ptr[i] = point_handle_3d;
+	}
+
+	point_handle_mesh_array[Mesh::ARRAY_VERTEX] = point_handle_mesh_vertices;
+
+	rs->mesh_add_surface_from_arrays(point_handle_mesh_rid, RS::PRIMITIVE_POINTS, point_handle_mesh_array);
+	rs->instance_set_surface_override_material(point_handles_instance_rid, 0, handle_material->get_rid());
+	rs->instance_set_transform(point_handles_instance_rid, gt);
+}
+
+NavigationObstacle3DEditorPlugin *NavigationObstacle3DEditorPlugin::singleton = nullptr;
+
+NavigationObstacle3DEditorPlugin::NavigationObstacle3DEditorPlugin() {
+	singleton = this;
+
+	line_material = Ref<StandardMaterial3D>(memnew(StandardMaterial3D));
+	line_material->set_shading_mode(StandardMaterial3D::SHADING_MODE_UNSHADED);
+	line_material->set_flag(StandardMaterial3D::FLAG_ALBEDO_FROM_VERTEX_COLOR, true);
+	line_material->set_flag(StandardMaterial3D::FLAG_SRGB_VERTEX_COLOR, true);
+	line_material->set_flag(StandardMaterial3D::FLAG_DISABLE_FOG, true);
+	line_material->set_albedo(Color(1, 0.3, 0.1, 0.8));
+	line_material->set_flag(StandardMaterial3D::FLAG_DISABLE_DEPTH_TEST, true);
+
+	handle_material = Ref<StandardMaterial3D>(memnew(StandardMaterial3D));
+	handle_material->set_shading_mode(StandardMaterial3D::SHADING_MODE_UNSHADED);
+	handle_material->set_transparency(StandardMaterial3D::TRANSPARENCY_ALPHA);
+	handle_material->set_flag(StandardMaterial3D::FLAG_USE_POINT_SIZE, true);
+	handle_material->set_flag(StandardMaterial3D::FLAG_ALBEDO_FROM_VERTEX_COLOR, true);
+	handle_material->set_flag(StandardMaterial3D::FLAG_SRGB_VERTEX_COLOR, true);
+	handle_material->set_flag(StandardMaterial3D::FLAG_DISABLE_FOG, true);
+	Ref<Texture2D> handle = EditorNode::get_singleton()->get_editor_theme()->get_icon(SNAME("Editor3DHandle"), EditorStringName(EditorIcons));
+	handle_material->set_point_size(handle->get_width());
+	handle_material->set_texture(StandardMaterial3D::TEXTURE_ALBEDO, handle);
+	handle_material->set_flag(StandardMaterial3D::FLAG_DISABLE_DEPTH_TEST, true);
+
+	RenderingServer *rs = RenderingServer::get_singleton();
+
+	point_lines_mesh_rid = rs->mesh_create();
+	point_handle_mesh_rid = rs->mesh_create();
+
+	point_lines_instance_rid = rs->instance_create();
+	point_handles_instance_rid = rs->instance_create();
+
+	rs->instance_set_base(point_lines_instance_rid, point_lines_mesh_rid);
+	rs->instance_set_base(point_handles_instance_rid, point_handle_mesh_rid);
+
+	obstacle_editor = memnew(HBoxContainer);
+	obstacle_editor->hide();
+
+	Ref<ButtonGroup> bg;
+	bg.instantiate();
+
+	button_create = memnew(Button);
+	button_create->set_theme_type_variation(SceneStringName(FlatButton));
+	obstacle_editor->add_child(button_create);
+	button_create->set_tooltip_text(TTR("Add Vertex"));
+	button_create->connect(SceneStringName(pressed), callable_mp(this, &NavigationObstacle3DEditorPlugin::set_mode).bind(NavigationObstacle3DEditorPlugin::MODE_CREATE));
+	button_create->set_toggle_mode(true);
+	button_create->set_button_group(bg);
+
+	button_edit = memnew(Button);
+	button_edit->set_theme_type_variation(SceneStringName(FlatButton));
+	button_edit->set_accessibility_name(TTRC("Edit"));
+	obstacle_editor->add_child(button_edit);
+	button_edit->connect(SceneStringName(pressed), callable_mp(this, &NavigationObstacle3DEditorPlugin::set_mode).bind(NavigationObstacle3DEditorPlugin::MODE_EDIT));
+	button_edit->set_toggle_mode(true);
+	button_edit->set_button_group(bg);
+
+	button_delete = memnew(Button);
+	button_delete->set_theme_type_variation(SceneStringName(FlatButton));
+	button_delete->set_accessibility_name(TTRC("Delete"));
+	obstacle_editor->add_child(button_delete);
+	button_delete->connect(SceneStringName(pressed), callable_mp(this, &NavigationObstacle3DEditorPlugin::set_mode).bind(NavigationObstacle3DEditorPlugin::MODE_DELETE));
+	button_delete->set_toggle_mode(true);
+	button_delete->set_button_group(bg);
+
+	button_flip = memnew(Button);
+	button_flip->set_theme_type_variation(SceneStringName(FlatButton));
+	button_flip->set_accessibility_name(TTRC("Flip"));
+	obstacle_editor->add_child(button_flip);
+	button_flip->connect(SceneStringName(pressed), callable_mp(this, &NavigationObstacle3DEditorPlugin::set_mode).bind(NavigationObstacle3DEditorPlugin::ACTION_FLIP));
+	button_flip->set_toggle_mode(true);
+
+	button_clear = memnew(Button);
+	button_clear->set_theme_type_variation(SceneStringName(FlatButton));
+	button_clear->set_accessibility_name(TTRC("Clear"));
+	obstacle_editor->add_child(button_clear);
+	button_clear->connect(SceneStringName(pressed), callable_mp(this, &NavigationObstacle3DEditorPlugin::set_mode).bind(NavigationObstacle3DEditorPlugin::ACTION_CLEAR));
+	button_clear->set_toggle_mode(true);
+
+	button_clear_dialog = memnew(ConfirmationDialog);
+	button_clear_dialog->set_title(TTR("Please Confirm..."));
+	button_clear_dialog->set_text(TTR("Remove all vertices?"));
+	button_clear_dialog->connect(SceneStringName(confirmed), callable_mp(NavigationObstacle3DEditorPlugin::singleton, &NavigationObstacle3DEditorPlugin::action_clear_vertices));
+	obstacle_editor->add_child(button_clear_dialog);
+
+	Node3DEditor::get_singleton()->add_control_to_menu_panel(obstacle_editor);
+
+	Ref<NavigationObstacle3DGizmoPlugin> gizmo_plugin = memnew(NavigationObstacle3DGizmoPlugin());
+	obstacle_3d_gizmo_plugin = gizmo_plugin;
+	Node3DEditor::get_singleton()->add_gizmo_plugin(gizmo_plugin);
+}
+
+NavigationObstacle3DEditorPlugin::~NavigationObstacle3DEditorPlugin() {
+	RenderingServer *rs = RenderingServer::get_singleton();
+	ERR_FAIL_NULL(rs);
+
+	if (point_lines_instance_rid.is_valid()) {
+		rs->free(point_lines_instance_rid);
+		point_lines_instance_rid = RID();
+	}
+	if (point_lines_mesh_rid.is_valid()) {
+		rs->free(point_lines_mesh_rid);
+		point_lines_mesh_rid = RID();
+	}
+
+	if (point_handles_instance_rid.is_valid()) {
+		rs->free(point_handles_instance_rid);
+		point_handles_instance_rid = RID();
+	}
+	if (point_handle_mesh_rid.is_valid()) {
+		rs->free(point_handle_mesh_rid);
+		point_handle_mesh_rid = RID();
+	}
+}

modules/navigation_3d/editor/navigation_obstacle_3d_editor_plugin.h

@@ -0,0 +1,134 @@
+/**************************************************************************/
+/*  navigation_obstacle_3d_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/3d/node_3d_editor_gizmos.h"
+#include "scene/gui/box_container.h"
+
+class Button;
+class ConfirmationDialog;
+class NavigationObstacle3D;
+
+class NavigationObstacle3DGizmoPlugin : public EditorNode3DGizmoPlugin {
+	GDCLASS(NavigationObstacle3DGizmoPlugin, EditorNode3DGizmoPlugin);
+
+public:
+	virtual bool has_gizmo(Node3D *p_spatial) override;
+	virtual String get_gizmo_name() const override;
+
+	virtual void redraw(EditorNode3DGizmo *p_gizmo) override;
+
+	bool can_be_hidden() const override;
+	int get_priority() const override;
+
+	virtual int subgizmos_intersect_ray(const EditorNode3DGizmo *p_gizmo, Camera3D *p_camera, const Vector2 &p_point) const override;
+	virtual Vector<int> subgizmos_intersect_frustum(const EditorNode3DGizmo *p_gizmo, const Camera3D *p_camera, const Vector<Plane> &p_frustum) const override;
+	virtual Transform3D get_subgizmo_transform(const EditorNode3DGizmo *p_gizmo, int p_id) const override;
+	virtual void set_subgizmo_transform(const EditorNode3DGizmo *p_gizmo, int p_id, Transform3D p_transform) override;
+	virtual void commit_subgizmos(const EditorNode3DGizmo *p_gizmo, const Vector<int> &p_ids, const Vector<Transform3D> &p_restore, bool p_cancel = false) override;
+
+	NavigationObstacle3DGizmoPlugin();
+};
+
+class NavigationObstacle3DEditorPlugin : public EditorPlugin {
+	GDCLASS(NavigationObstacle3DEditorPlugin, EditorPlugin);
+
+	Ref<NavigationObstacle3DGizmoPlugin> obstacle_3d_gizmo_plugin;
+
+	NavigationObstacle3D *obstacle_node = nullptr;
+
+	Ref<StandardMaterial3D> line_material;
+	Ref<StandardMaterial3D> handle_material;
+
+	RID point_lines_mesh_rid;
+	RID point_lines_instance_rid;
+	RID point_handle_mesh_rid;
+	RID point_handles_instance_rid;
+
+public:
+	enum Mode {
+		MODE_CREATE = 0,
+		MODE_EDIT,
+		MODE_DELETE,
+		ACTION_FLIP,
+		ACTION_CLEAR,
+	};
+
+private:
+	int mode = MODE_EDIT;
+
+	int edited_point = 0;
+	Vector3 edited_point_pos;
+	Vector<Vector3> pre_move_edit;
+	Vector<Vector3> wip_vertices;
+	bool wip_active = false;
+	bool snap_ignore = false;
+
+	void _wip_close();
+	void _wip_cancel();
+	void _update_theme();
+
+	Button *button_create = nullptr;
+	Button *button_edit = nullptr;
+	Button *button_delete = nullptr;
+	Button *button_flip = nullptr;
+	Button *button_clear = nullptr;
+
+	ConfirmationDialog *button_clear_dialog = nullptr;
+
+protected:
+	void _notification(int p_what);
+	void _node_removed(Node *p_node);
+
+public:
+	HBoxContainer *obstacle_editor = nullptr;
+	static NavigationObstacle3DEditorPlugin *singleton;
+
+	void redraw();
+
+	void set_mode(int p_mode);
+	int get_mode() { return mode; }
+
+	void action_flip_vertices();
+	void action_clear_vertices();
+
+	virtual EditorPlugin::AfterGUIInput forward_3d_gui_input(Camera3D *p_camera, const Ref<InputEvent> &p_event) override;
+
+	virtual String get_plugin_name() const override { return "NavigationObstacle3DEditor"; }
+	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;
+
+	NavigationObstacle3DEditorPlugin();
+	~NavigationObstacle3DEditorPlugin();
+};

modules/navigation_3d/editor/navigation_region_3d_editor_plugin.cpp

@@ -0,0 +1,324 @@
+/**************************************************************************/
+/*  navigation_region_3d_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_3d_editor_plugin.h"
+
+#include "editor/editor_node.h"
+#include "editor/editor_string_names.h"
+#include "editor/inspector/multi_node_edit.h"
+#include "editor/scene/3d/node_3d_editor_plugin.h"
+#include "scene/3d/navigation/navigation_region_3d.h"
+#include "scene/gui/box_container.h"
+#include "scene/gui/button.h"
+#include "scene/gui/dialogs.h"
+#include "scene/gui/label.h"
+#include "servers/navigation_server_3d.h"
+
+void NavigationRegion3DEditor::_node_removed(Node *p_node) {
+	if (selected_regions.is_empty()) {
+		return;
+	}
+
+	NavigationRegion3D *region = Object::cast_to<NavigationRegion3D>(p_node);
+
+	if (region && selected_regions.has(region)) {
+		selected_regions.erase(region);
+		if (selected_regions.is_empty()) {
+			hide();
+		}
+	}
+}
+
+void NavigationRegion3DEditor::_notification(int p_what) {
+	switch (p_what) {
+		case NOTIFICATION_ENTER_TREE: {
+			button_bake->set_button_icon(get_theme_icon(SNAME("Bake"), EditorStringName(EditorIcons)));
+			button_reset->set_button_icon(get_theme_icon(SNAME("Reload"), EditorStringName(EditorIcons)));
+		} break;
+		case NOTIFICATION_PROCESS: {
+			if (currently_baking_region) {
+				const String bake_state_msg = NavigationServer3D::get_singleton()->get_baking_navigation_mesh_state_msg(currently_baking_region->get_navigation_mesh());
+				multibake_dialog->set_text(itos(processed_regions_to_bake_count) + " / " + itos(processed_regions_to_bake_count_max) + " - Baking navmesh from region '" + currently_baking_region->get_name() + "'.\n\nBake state: " + bake_state_msg + "\n\nDo NOT change nodes by any means while the baking is parsing the SceneTree.");
+			} else {
+				multibake_dialog->set_text("");
+			}
+		}
+	}
+}
+
+void NavigationRegion3DEditor::_bake_pressed() {
+	button_bake->set_pressed(false);
+
+	if (selected_regions.is_empty()) {
+		return;
+	}
+
+	if (bake_in_process) {
+		return;
+	}
+
+	HashSet<Ref<NavigationMesh>> unique_navmeshes;
+	regions_to_bake.clear();
+	regions_with_navmesh_to_bake.clear();
+
+	for (NavigationRegion3D *region : selected_regions) {
+		ERR_CONTINUE(region == nullptr);
+		Ref<NavigationMesh> navmesh = region->get_navigation_mesh();
+		if (navmesh.is_null()) {
+			err_dialog->set_text(TTR("A NavigationMesh resource must be set or created for this node to work."));
+			err_dialog->popup_centered();
+			return;
+		}
+
+		String path = navmesh->get_path();
+		if (!path.is_resource_file()) {
+			int srpos = path.find("::");
+			if (srpos != -1) {
+				String base = path.substr(0, srpos);
+				if (ResourceLoader::get_resource_type(base) == "PackedScene") {
+					if (!get_tree()->get_edited_scene_root() || get_tree()->get_edited_scene_root()->get_scene_file_path() != base) {
+						err_dialog->set_text(TTR("Cannot generate navigation mesh because it does not belong to the edited scene. Make it unique first."));
+						err_dialog->popup_centered();
+						return;
+					}
+				} else {
+					if (FileAccess::exists(base + ".import")) {
+						err_dialog->set_text(TTR("Cannot generate navigation mesh because it belongs to a resource which was imported."));
+						err_dialog->popup_centered();
+						return;
+					}
+				}
+			}
+		} else {
+			if (FileAccess::exists(path + ".import")) {
+				err_dialog->set_text(TTR("Cannot generate navigation mesh because the resource was imported from another type."));
+				err_dialog->popup_centered();
+				return;
+			}
+		}
+
+		regions_to_bake.push_back(region);
+
+		if (unique_navmeshes.has(navmesh)) {
+			// No point (re)baking the same resource in case of multi select.
+			// Trying to bake the same navmesh twice would trigger an error.
+			continue;
+		}
+		unique_navmeshes.insert(navmesh);
+		regions_with_navmesh_to_bake.push_back(region);
+	}
+
+	if (!regions_with_navmesh_to_bake.is_empty()) {
+		multibake_dialog->set_ok_button_text(TTR("Bake"));
+		multibake_dialog->get_ok_button()->set_disabled(false);
+		multibake_dialog->set_text("Attempting to bake " + itos(regions_with_navmesh_to_bake.size()) + " unique navmesh(es) from " + itos(regions_to_bake.size()) + " selected NavigationRegion3D node(s).\n\nThis can take some time and freeze the Editor temporarily.\n\nDo NOT change nodes by any means while the baking is parsing the SceneTree.");
+		multibake_dialog->popup_centered();
+		if (regions_with_navmesh_to_bake.size() == 1) {
+			// If we only have a single region start bake immediately.
+			_on_navmesh_multibake_confirmed();
+		}
+	}
+}
+
+void NavigationRegion3DEditor::_on_navmesh_multibake_confirmed() {
+	multibake_dialog->get_ok_button()->set_disabled(true);
+
+	bake_in_process = true;
+	region_baking_canceled = false;
+	processed_regions_to_bake_count = 0;
+	processed_regions_to_bake_count_max = regions_with_navmesh_to_bake.size();
+
+	set_process(true);
+	_process_regions_to_bake();
+}
+
+void NavigationRegion3DEditor::_process_regions_to_bake() {
+	if (region_baking_canceled) {
+		region_baking_canceled = false;
+		regions_with_navmesh_to_bake.clear();
+	}
+
+	if (regions_with_navmesh_to_bake.is_empty()) {
+		regions_to_bake.clear();
+		multibake_dialog->set_visible(false);
+		set_process(false);
+		currently_baking_region = nullptr;
+		bake_in_process = false;
+		return;
+	}
+
+	NavigationRegion3D *region_to_bake = regions_with_navmesh_to_bake[0];
+	regions_with_navmesh_to_bake.remove_at_unordered(0);
+	processed_regions_to_bake_count += 1;
+	if (region_to_bake && region_to_bake->get_navigation_mesh().is_valid()) {
+		currently_baking_region = region_to_bake;
+		region_to_bake->connect(SNAME("bake_finished"), callable_mp(this, &NavigationRegion3DEditor::_process_regions_to_bake), CONNECT_ONE_SHOT);
+		region_to_bake->bake_navigation_mesh(true);
+		return;
+	} else {
+		_process_regions_to_bake();
+	}
+}
+
+void NavigationRegion3DEditor::_on_navmesh_multibake_canceled() {
+	if (bake_in_process) {
+		region_baking_canceled = true;
+		return;
+	}
+
+	multibake_dialog->set_visible(false);
+	regions_to_bake.clear();
+	regions_with_navmesh_to_bake.clear();
+	processed_regions_to_bake_count = 0;
+	processed_regions_to_bake_count_max = 0;
+	region_baking_canceled = false;
+	currently_baking_region = nullptr;
+	bake_in_process = false;
+}
+
+void NavigationRegion3DEditor::_clear_pressed() {
+	button_bake->set_pressed(false);
+	bake_info->set_text("");
+
+	if (!selected_regions.is_empty()) {
+		for (NavigationRegion3D *region : selected_regions) {
+			if (region->get_navigation_mesh().is_valid()) {
+				region->get_navigation_mesh()->clear();
+				region->update_gizmos();
+			}
+		}
+	}
+}
+
+void NavigationRegion3DEditor::edit(LocalVector<NavigationRegion3D *> p_regions) {
+	if (p_regions.is_empty()) {
+		return;
+	}
+
+	selected_regions = p_regions;
+}
+
+NavigationRegion3DEditor::NavigationRegion3DEditor() {
+	bake_hbox = memnew(HBoxContainer);
+
+	button_bake = memnew(Button);
+	button_bake->set_theme_type_variation(SceneStringName(FlatButton));
+	bake_hbox->add_child(button_bake);
+	button_bake->set_toggle_mode(true);
+	button_bake->set_text(TTR("Bake NavigationMesh"));
+	button_bake->set_tooltip_text(TTR("Bakes the NavigationMesh by first parsing the scene for source geometry and then creating the navigation mesh vertices and polygons."));
+	button_bake->connect(SceneStringName(pressed), callable_mp(this, &NavigationRegion3DEditor::_bake_pressed));
+
+	button_reset = memnew(Button);
+	button_reset->set_theme_type_variation(SceneStringName(FlatButton));
+	bake_hbox->add_child(button_reset);
+	button_reset->set_text(TTR("Clear NavigationMesh"));
+	button_reset->set_tooltip_text(TTR("Clears the internal NavigationMesh vertices and polygons."));
+	button_reset->connect(SceneStringName(pressed), callable_mp(this, &NavigationRegion3DEditor::_clear_pressed));
+
+	bake_info = memnew(Label);
+	bake_info->set_focus_mode(FOCUS_ACCESSIBILITY);
+	bake_hbox->add_child(bake_info);
+
+	err_dialog = memnew(AcceptDialog);
+	add_child(err_dialog);
+
+	multibake_dialog = memnew(ConfirmationDialog);
+	add_child(multibake_dialog);
+	multibake_dialog->connect(SceneStringName(confirmed), callable_mp(this, &NavigationRegion3DEditor::_on_navmesh_multibake_confirmed));
+	multibake_dialog->connect(SNAME("canceled"), callable_mp(this, &NavigationRegion3DEditor::_on_navmesh_multibake_canceled));
+	multibake_dialog->set_hide_on_ok(false);
+	multibake_dialog->set_title(TTR("Baking NavigationMesh ..."));
+}
+
+void NavigationRegion3DEditorPlugin::edit(Object *p_object) {
+	LocalVector<NavigationRegion3D *> regions;
+
+	{
+		NavigationRegion3D *region = Object::cast_to<NavigationRegion3D>(p_object);
+		if (region) {
+			regions.push_back(region);
+			navigation_region_editor->edit(regions);
+			return;
+		}
+	}
+
+	Ref<MultiNodeEdit> mne = Ref<MultiNodeEdit>(p_object);
+	Node *edited_scene = EditorNode::get_singleton()->get_edited_scene();
+	if (mne.is_valid() && edited_scene) {
+		for (int i = 0; i < mne->get_node_count(); i++) {
+			NavigationRegion3D *region = Object::cast_to<NavigationRegion3D>(edited_scene->get_node(mne->get_node(i)));
+			if (region) {
+				regions.push_back(region);
+			}
+		}
+	}
+
+	navigation_region_editor->edit(regions);
+}
+
+bool NavigationRegion3DEditorPlugin::handles(Object *p_object) const {
+	if (Object::cast_to<NavigationRegion3D>(p_object)) {
+		return true;
+	}
+
+	Ref<MultiNodeEdit> mne = Ref<MultiNodeEdit>(p_object);
+	Node *edited_scene = EditorNode::get_singleton()->get_edited_scene();
+	if (mne.is_valid() && edited_scene) {
+		for (int i = 0; i < mne->get_node_count(); i++) {
+			if (Object::cast_to<NavigationRegion3D>(edited_scene->get_node(mne->get_node(i)))) {
+				return true;
+			}
+		}
+	}
+	return false;
+}
+
+void NavigationRegion3DEditorPlugin::make_visible(bool p_visible) {
+	if (p_visible) {
+		navigation_region_editor->show();
+		navigation_region_editor->bake_hbox->show();
+	} else {
+		navigation_region_editor->hide();
+		navigation_region_editor->bake_hbox->hide();
+		navigation_region_editor->edit(LocalVector<NavigationRegion3D *>());
+	}
+}
+
+NavigationRegion3DEditorPlugin::NavigationRegion3DEditorPlugin() {
+	navigation_region_editor = memnew(NavigationRegion3DEditor);
+	EditorNode::get_singleton()->get_gui_base()->add_child(navigation_region_editor);
+	add_control_to_container(CONTAINER_SPATIAL_EDITOR_MENU, navigation_region_editor->bake_hbox);
+	navigation_region_editor->hide();
+	navigation_region_editor->bake_hbox->hide();
+
+	gizmo_plugin.instantiate();
+	Node3DEditor::get_singleton()->add_gizmo_plugin(gizmo_plugin);
+}

modules/navigation_3d/editor/navigation_region_3d_editor_plugin.h

@@ -0,0 +1,100 @@
+/**************************************************************************/
+/*  navigation_region_3d_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 "navigation_region_3d_gizmo_plugin.h"
+
+class AcceptDialog;
+class Button;
+class ConfirmationDialog;
+class HBoxContainer;
+class Label;
+class NavigationRegion3D;
+
+class NavigationRegion3DEditor : public Control {
+	friend class NavigationRegion3DEditorPlugin;
+
+	GDCLASS(NavigationRegion3DEditor, Control);
+
+	AcceptDialog *err_dialog = nullptr;
+	ConfirmationDialog *multibake_dialog = nullptr;
+
+	HBoxContainer *bake_hbox = nullptr;
+	Button *button_bake = nullptr;
+	Button *button_reset = nullptr;
+	Label *bake_info = nullptr;
+
+	LocalVector<NavigationRegion3D *> selected_regions;
+
+	LocalVector<NavigationRegion3D *> regions_to_bake;
+	LocalVector<NavigationRegion3D *> regions_with_navmesh_to_bake;
+
+	int processed_regions_to_bake_count = 0;
+	int processed_regions_to_bake_count_max = 0;
+	bool region_baking_canceled = false;
+	NavigationRegion3D *currently_baking_region = nullptr;
+
+	bool bake_in_process = false;
+
+	void _bake_pressed();
+	void _clear_pressed();
+
+	void _on_navmesh_multibake_confirmed();
+	void _on_navmesh_multibake_canceled();
+	void _process_regions_to_bake();
+
+protected:
+	void _node_removed(Node *p_node);
+	void _notification(int p_what);
+
+public:
+	void edit(LocalVector<NavigationRegion3D *> p_regions);
+	NavigationRegion3DEditor();
+};
+
+class NavigationRegion3DEditorPlugin : public EditorPlugin {
+	GDCLASS(NavigationRegion3DEditorPlugin, EditorPlugin);
+
+	NavigationRegion3DEditor *navigation_region_editor = nullptr;
+
+	Ref<NavigationRegion3DGizmoPlugin> gizmo_plugin;
+
+public:
+	virtual String get_plugin_name() const override { return "NavigationRegion3D"; }
+	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;
+
+	NavigationRegion3DEditorPlugin();
+};

modules/navigation_3d/editor/navigation_region_3d_gizmo_plugin.cpp

@@ -0,0 +1,208 @@
+/**************************************************************************/
+/*  navigation_region_3d_gizmo_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_3d_gizmo_plugin.h"
+
+#include "core/math/random_pcg.h"
+#include "scene/3d/navigation/navigation_region_3d.h"
+#include "servers/navigation_server_3d.h"
+
+NavigationRegion3DGizmoPlugin::NavigationRegion3DGizmoPlugin() {
+	create_material("face_material", NavigationServer3D::get_singleton()->get_debug_navigation_geometry_face_color(), false, false, true);
+	create_material("face_material_disabled", NavigationServer3D::get_singleton()->get_debug_navigation_geometry_face_disabled_color(), false, false, true);
+	create_material("edge_material", NavigationServer3D::get_singleton()->get_debug_navigation_geometry_edge_color());
+	create_material("edge_material_disabled", NavigationServer3D::get_singleton()->get_debug_navigation_geometry_edge_disabled_color());
+
+	Color baking_aabb_material_color = Color(0.8, 0.5, 0.7);
+	baking_aabb_material_color.a = 0.1;
+	create_material("baking_aabb_material", baking_aabb_material_color);
+}
+
+bool NavigationRegion3DGizmoPlugin::has_gizmo(Node3D *p_spatial) {
+	return Object::cast_to<NavigationRegion3D>(p_spatial) != nullptr;
+}
+
+String NavigationRegion3DGizmoPlugin::get_gizmo_name() const {
+	return "NavigationRegion3D";
+}
+
+int NavigationRegion3DGizmoPlugin::get_priority() const {
+	return -1;
+}
+
+void NavigationRegion3DGizmoPlugin::redraw(EditorNode3DGizmo *p_gizmo) {
+	NavigationRegion3D *navigationregion = Object::cast_to<NavigationRegion3D>(p_gizmo->get_node_3d());
+
+	p_gizmo->clear();
+	Ref<NavigationMesh> navigationmesh = navigationregion->get_navigation_mesh();
+	if (navigationmesh.is_null()) {
+		return;
+	}
+
+	AABB baking_aabb = navigationmesh->get_filter_baking_aabb();
+	if (baking_aabb.has_volume()) {
+		Vector3 baking_aabb_offset = navigationmesh->get_filter_baking_aabb_offset();
+
+		if (p_gizmo->is_selected()) {
+			Ref<Material> material = get_material("baking_aabb_material", p_gizmo);
+			p_gizmo->add_solid_box(material, baking_aabb.get_size(), baking_aabb.get_center() + baking_aabb_offset);
+		}
+	}
+
+	Vector<Vector3> vertices = navigationmesh->get_vertices();
+	const Vector3 *vr = vertices.ptr();
+	List<Face3> faces;
+	for (int i = 0; i < navigationmesh->get_polygon_count(); i++) {
+		Vector<int> p = navigationmesh->get_polygon(i);
+
+		for (int j = 2; j < p.size(); j++) {
+			Face3 f;
+			f.vertex[0] = vr[p[0]];
+			f.vertex[1] = vr[p[j - 1]];
+			f.vertex[2] = vr[p[j]];
+
+			faces.push_back(f);
+		}
+	}
+
+	if (faces.is_empty()) {
+		return;
+	}
+
+	HashMap<_EdgeKey, bool, _EdgeKey> edge_map;
+	Vector<Vector3> tmeshfaces;
+	tmeshfaces.resize(faces.size() * 3);
+
+	{
+		Vector3 *tw = tmeshfaces.ptrw();
+		int tidx = 0;
+
+		for (const Face3 &f : faces) {
+			for (int j = 0; j < 3; j++) {
+				tw[tidx++] = f.vertex[j];
+				_EdgeKey ek;
+				ek.from = f.vertex[j].snappedf(CMP_EPSILON);
+				ek.to = f.vertex[(j + 1) % 3].snappedf(CMP_EPSILON);
+				if (ek.from < ek.to) {
+					SWAP(ek.from, ek.to);
+				}
+
+				HashMap<_EdgeKey, bool, _EdgeKey>::Iterator F = edge_map.find(ek);
+
+				if (F) {
+					F->value = false;
+
+				} else {
+					edge_map[ek] = true;
+				}
+			}
+		}
+	}
+	Vector<Vector3> lines;
+
+	for (const KeyValue<_EdgeKey, bool> &E : edge_map) {
+		if (E.value) {
+			lines.push_back(E.key.from);
+			lines.push_back(E.key.to);
+		}
+	}
+
+	Ref<TriangleMesh> tmesh = memnew(TriangleMesh);
+	tmesh->create(tmeshfaces);
+
+	p_gizmo->add_collision_triangles(tmesh);
+	p_gizmo->add_collision_segments(lines);
+
+	Ref<ArrayMesh> debug_mesh = Ref<ArrayMesh>(memnew(ArrayMesh));
+	int polygon_count = navigationmesh->get_polygon_count();
+
+	// build geometry face surface
+	Vector<Vector3> face_vertex_array;
+	face_vertex_array.resize(polygon_count * 3);
+
+	for (int i = 0; i < polygon_count; i++) {
+		Vector<int> polygon = navigationmesh->get_polygon(i);
+
+		face_vertex_array.push_back(vertices[polygon[0]]);
+		face_vertex_array.push_back(vertices[polygon[1]]);
+		face_vertex_array.push_back(vertices[polygon[2]]);
+	}
+
+	Array face_mesh_array;
+	face_mesh_array.resize(Mesh::ARRAY_MAX);
+	face_mesh_array[Mesh::ARRAY_VERTEX] = face_vertex_array;
+
+	// if enabled add vertex colors to colorize each face individually
+	RandomPCG rand;
+	bool enabled_geometry_face_random_color = NavigationServer3D::get_singleton()->get_debug_navigation_enable_geometry_face_random_color();
+	if (enabled_geometry_face_random_color) {
+		Color debug_navigation_geometry_face_color = NavigationServer3D::get_singleton()->get_debug_navigation_geometry_face_color();
+		Color polygon_color = debug_navigation_geometry_face_color;
+
+		Vector<Color> face_color_array;
+		face_color_array.resize(polygon_count * 3);
+
+		for (int i = 0; i < polygon_count; i++) {
+			// Generate the polygon color, slightly randomly modified from the settings one.
+			polygon_color.set_hsv(debug_navigation_geometry_face_color.get_h() + rand.random(-1.0, 1.0) * 0.1, debug_navigation_geometry_face_color.get_s(), debug_navigation_geometry_face_color.get_v() + rand.random(-1.0, 1.0) * 0.2);
+			polygon_color.a = debug_navigation_geometry_face_color.a;
+
+			Vector<int> polygon = navigationmesh->get_polygon(i);
+
+			face_color_array.push_back(polygon_color);
+			face_color_array.push_back(polygon_color);
+			face_color_array.push_back(polygon_color);
+		}
+		face_mesh_array[Mesh::ARRAY_COLOR] = face_color_array;
+	}
+
+	debug_mesh->add_surface_from_arrays(Mesh::PRIMITIVE_TRIANGLES, face_mesh_array);
+	p_gizmo->add_mesh(debug_mesh, navigationregion->is_enabled() ? get_material("face_material", p_gizmo) : get_material("face_material_disabled", p_gizmo));
+
+	// if enabled build geometry edge line surface
+	bool enabled_edge_lines = NavigationServer3D::get_singleton()->get_debug_navigation_enable_edge_lines();
+	if (enabled_edge_lines) {
+		Vector<Vector3> line_vertex_array;
+		line_vertex_array.resize(polygon_count * 6);
+
+		for (int i = 0; i < polygon_count; i++) {
+			Vector<int> polygon = navigationmesh->get_polygon(i);
+
+			line_vertex_array.push_back(vertices[polygon[0]]);
+			line_vertex_array.push_back(vertices[polygon[1]]);
+			line_vertex_array.push_back(vertices[polygon[1]]);
+			line_vertex_array.push_back(vertices[polygon[2]]);
+			line_vertex_array.push_back(vertices[polygon[2]]);
+			line_vertex_array.push_back(vertices[polygon[0]]);
+		}
+
+		p_gizmo->add_lines(line_vertex_array, navigationregion->is_enabled() ? get_material("edge_material", p_gizmo) : get_material("edge_material_disabled", p_gizmo));
+	}
+}

modules/navigation_3d/editor/navigation_region_3d_gizmo_plugin.h

@@ -0,0 +1,58 @@
+/**************************************************************************/
+/*  navigation_region_3d_gizmo_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/3d/node_3d_editor_gizmos.h"
+
+class NavigationRegion3DGizmoPlugin : public EditorNode3DGizmoPlugin {
+	GDCLASS(NavigationRegion3DGizmoPlugin, EditorNode3DGizmoPlugin);
+
+	struct _EdgeKey {
+		Vector3 from;
+		Vector3 to;
+
+		static uint32_t hash(const _EdgeKey &p_key) {
+			return HashMapHasherDefault::hash(p_key.from) ^ HashMapHasherDefault::hash(p_key.to);
+		}
+
+		bool operator==(const _EdgeKey &p_with) const {
+			return HashMapComparatorDefault<Vector3>::compare(from, p_with.from) && HashMapComparatorDefault<Vector3>::compare(to, p_with.to);
+		}
+	};
+
+public:
+	bool has_gizmo(Node3D *p_spatial) override;
+	String get_gizmo_name() const override;
+	int get_priority() const override;
+	void redraw(EditorNode3DGizmo *p_gizmo) override;
+
+	NavigationRegion3DGizmoPlugin();
+};

modules/navigation_3d/nav_agent_3d.cpp

@@ -0,0 +1,425 @@
+/**************************************************************************/
+/*  nav_agent_3d.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_3d.h"
+
+#include "nav_map_3d.h"
+
+void NavAgent3D::set_avoidance_enabled(bool p_enabled) {
+	avoidance_enabled = p_enabled;
+	_update_rvo_agent_properties();
+}
+
+void NavAgent3D::set_use_3d_avoidance(bool p_enabled) {
+	use_3d_avoidance = p_enabled;
+	_update_rvo_agent_properties();
+}
+
+void NavAgent3D::_update_rvo_agent_properties() {
+	if (use_3d_avoidance) {
+		rvo_agent_3d.neighborDist_ = neighbor_distance;
+		rvo_agent_3d.maxNeighbors_ = max_neighbors;
+		rvo_agent_3d.timeHorizon_ = time_horizon_agents;
+		rvo_agent_3d.timeHorizonObst_ = time_horizon_obstacles;
+		rvo_agent_3d.radius_ = radius;
+		rvo_agent_3d.maxSpeed_ = max_speed;
+		rvo_agent_3d.position_ = RVO3D::Vector3(position.x, position.y, position.z);
+		// Replacing the internal velocity directly causes major jitter / bugs due to unpredictable velocity jumps, left line here for testing.
+		//rvo_agent_3d.velocity_ = RVO3D::Vector3(velocity.x, velocity.y ,velocity.z);
+		rvo_agent_3d.prefVelocity_ = RVO3D::Vector3(velocity.x, velocity.y, velocity.z);
+		rvo_agent_3d.height_ = height;
+		rvo_agent_3d.avoidance_layers_ = avoidance_layers;
+		rvo_agent_3d.avoidance_mask_ = avoidance_mask;
+		rvo_agent_3d.avoidance_priority_ = avoidance_priority;
+	} else {
+		rvo_agent_2d.neighborDist_ = neighbor_distance;
+		rvo_agent_2d.maxNeighbors_ = max_neighbors;
+		rvo_agent_2d.timeHorizon_ = time_horizon_agents;
+		rvo_agent_2d.timeHorizonObst_ = time_horizon_obstacles;
+		rvo_agent_2d.radius_ = radius;
+		rvo_agent_2d.maxSpeed_ = max_speed;
+		rvo_agent_2d.position_ = RVO2D::Vector2(position.x, position.z);
+		rvo_agent_2d.elevation_ = position.y;
+		// Replacing the internal velocity directly causes major jitter / bugs due to unpredictable velocity jumps, left line here for testing.
+		//rvo_agent_2d.velocity_ = RVO2D::Vector2(velocity.x, velocity.z);
+		rvo_agent_2d.prefVelocity_ = RVO2D::Vector2(velocity.x, velocity.z);
+		rvo_agent_2d.height_ = height;
+		rvo_agent_2d.avoidance_layers_ = avoidance_layers;
+		rvo_agent_2d.avoidance_mask_ = avoidance_mask;
+		rvo_agent_2d.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 NavAgent3D::set_map(NavMap3D *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 NavAgent3D::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 NavAgent3D::set_avoidance_callback(Callable p_callback) {
+	avoidance_callback = p_callback;
+}
+
+bool NavAgent3D::has_avoidance_callback() const {
+	return avoidance_callback.is_valid();
+}
+
+void NavAgent3D::dispatch_avoidance_callback() {
+	if (!avoidance_callback.is_valid()) {
+		return;
+	}
+
+	Vector3 new_velocity;
+
+	if (use_3d_avoidance) {
+		new_velocity = Vector3(rvo_agent_3d.velocity_.x(), rvo_agent_3d.velocity_.y(), rvo_agent_3d.velocity_.z());
+	} else {
+		new_velocity = Vector3(rvo_agent_2d.velocity_.x(), 0.0, rvo_agent_2d.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 NavAgent3D::set_neighbor_distance(real_t p_neighbor_distance) {
+	neighbor_distance = p_neighbor_distance;
+	if (use_3d_avoidance) {
+		rvo_agent_3d.neighborDist_ = neighbor_distance;
+	} else {
+		rvo_agent_2d.neighborDist_ = neighbor_distance;
+	}
+	agent_dirty = true;
+
+	request_sync();
+}
+
+void NavAgent3D::set_max_neighbors(int p_max_neighbors) {
+	max_neighbors = p_max_neighbors;
+	if (use_3d_avoidance) {
+		rvo_agent_3d.maxNeighbors_ = max_neighbors;
+	} else {
+		rvo_agent_2d.maxNeighbors_ = max_neighbors;
+	}
+	agent_dirty = true;
+
+	request_sync();
+}
+
+void NavAgent3D::set_time_horizon_agents(real_t p_time_horizon) {
+	time_horizon_agents = p_time_horizon;
+	if (use_3d_avoidance) {
+		rvo_agent_3d.timeHorizon_ = time_horizon_agents;
+	} else {
+		rvo_agent_2d.timeHorizon_ = time_horizon_agents;
+	}
+	agent_dirty = true;
+
+	request_sync();
+}
+
+void NavAgent3D::set_time_horizon_obstacles(real_t p_time_horizon) {
+	time_horizon_obstacles = p_time_horizon;
+	if (use_3d_avoidance) {
+		rvo_agent_3d.timeHorizonObst_ = time_horizon_obstacles;
+	} else {
+		rvo_agent_2d.timeHorizonObst_ = time_horizon_obstacles;
+	}
+	agent_dirty = true;
+
+	request_sync();
+}
+
+void NavAgent3D::set_radius(real_t p_radius) {
+	radius = p_radius;
+	if (use_3d_avoidance) {
+		rvo_agent_3d.radius_ = radius;
+	} else {
+		rvo_agent_2d.radius_ = radius;
+	}
+	agent_dirty = true;
+
+	request_sync();
+}
+
+void NavAgent3D::set_height(real_t p_height) {
+	height = p_height;
+	if (use_3d_avoidance) {
+		rvo_agent_3d.height_ = height;
+	} else {
+		rvo_agent_2d.height_ = height;
+	}
+	agent_dirty = true;
+
+	request_sync();
+}
+
+void NavAgent3D::set_max_speed(real_t p_max_speed) {
+	max_speed = p_max_speed;
+	if (avoidance_enabled) {
+		if (use_3d_avoidance) {
+			rvo_agent_3d.maxSpeed_ = max_speed;
+		} else {
+			rvo_agent_2d.maxSpeed_ = max_speed;
+		}
+	}
+	agent_dirty = true;
+
+	request_sync();
+}
+
+void NavAgent3D::set_position(const Vector3 p_position) {
+	position = p_position;
+	if (avoidance_enabled) {
+		if (use_3d_avoidance) {
+			rvo_agent_3d.position_ = RVO3D::Vector3(p_position.x, p_position.y, p_position.z);
+		} else {
+			rvo_agent_2d.elevation_ = p_position.y;
+			rvo_agent_2d.position_ = RVO2D::Vector2(p_position.x, p_position.z);
+		}
+	}
+	agent_dirty = true;
+
+	request_sync();
+}
+
+void NavAgent3D::set_target_position(const Vector3 p_target_position) {
+	target_position = p_target_position;
+}
+
+void NavAgent3D::set_velocity(const Vector3 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) {
+		if (use_3d_avoidance) {
+			rvo_agent_3d.prefVelocity_ = RVO3D::Vector3(velocity.x, velocity.y, velocity.z);
+		} else {
+			rvo_agent_2d.prefVelocity_ = RVO2D::Vector2(velocity.x, velocity.z);
+		}
+	}
+	agent_dirty = true;
+
+	request_sync();
+}
+
+void NavAgent3D::set_velocity_forced(const Vector3 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) {
+		if (use_3d_avoidance) {
+			rvo_agent_3d.velocity_ = RVO3D::Vector3(p_velocity.x, p_velocity.y, p_velocity.z);
+		} else {
+			rvo_agent_2d.velocity_ = RVO2D::Vector2(p_velocity.x, p_velocity.z);
+		}
+	}
+	agent_dirty = true;
+
+	request_sync();
+}
+
+void NavAgent3D::update() {
+	// Updates this agent with the calculated results from the rvo simulation
+	if (avoidance_enabled) {
+		if (use_3d_avoidance) {
+			velocity = Vector3(rvo_agent_3d.velocity_.x(), rvo_agent_3d.velocity_.y(), rvo_agent_3d.velocity_.z());
+		} else {
+			velocity = Vector3(rvo_agent_2d.velocity_.x(), 0.0, rvo_agent_2d.velocity_.y());
+		}
+	}
+}
+
+void NavAgent3D::set_avoidance_mask(uint32_t p_mask) {
+	avoidance_mask = p_mask;
+	if (use_3d_avoidance) {
+		rvo_agent_3d.avoidance_mask_ = avoidance_mask;
+	} else {
+		rvo_agent_2d.avoidance_mask_ = avoidance_mask;
+	}
+	agent_dirty = true;
+
+	request_sync();
+}
+
+void NavAgent3D::set_avoidance_layers(uint32_t p_layers) {
+	avoidance_layers = p_layers;
+	if (use_3d_avoidance) {
+		rvo_agent_3d.avoidance_layers_ = avoidance_layers;
+	} else {
+		rvo_agent_2d.avoidance_layers_ = avoidance_layers;
+	}
+	agent_dirty = true;
+
+	request_sync();
+}
+
+void NavAgent3D::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;
+	if (use_3d_avoidance) {
+		rvo_agent_3d.avoidance_priority_ = avoidance_priority;
+	} else {
+		rvo_agent_2d.avoidance_priority_ = avoidance_priority;
+	}
+	agent_dirty = true;
+
+	request_sync();
+}
+
+bool NavAgent3D::is_dirty() const {
+	return agent_dirty;
+}
+
+void NavAgent3D::sync() {
+	agent_dirty = false;
+}
+
+const Dictionary NavAgent3D::get_avoidance_data() const {
+	// Returns debug data from RVO simulation internals of this agent.
+	Dictionary _avoidance_data;
+	if (use_3d_avoidance) {
+		_avoidance_data["max_neighbors"] = int(rvo_agent_3d.maxNeighbors_);
+		_avoidance_data["max_speed"] = float(rvo_agent_3d.maxSpeed_);
+		_avoidance_data["neighbor_distance"] = float(rvo_agent_3d.neighborDist_);
+		_avoidance_data["new_velocity"] = Vector3(rvo_agent_3d.newVelocity_.x(), rvo_agent_3d.newVelocity_.y(), rvo_agent_3d.newVelocity_.z());
+		_avoidance_data["velocity"] = Vector3(rvo_agent_3d.velocity_.x(), rvo_agent_3d.velocity_.y(), rvo_agent_3d.velocity_.z());
+		_avoidance_data["position"] = Vector3(rvo_agent_3d.position_.x(), rvo_agent_3d.position_.y(), rvo_agent_3d.position_.z());
+		_avoidance_data["preferred_velocity"] = Vector3(rvo_agent_3d.prefVelocity_.x(), rvo_agent_3d.prefVelocity_.y(), rvo_agent_3d.prefVelocity_.z());
+		_avoidance_data["radius"] = float(rvo_agent_3d.radius_);
+		_avoidance_data["time_horizon_agents"] = float(rvo_agent_3d.timeHorizon_);
+		_avoidance_data["time_horizon_obstacles"] = 0.0;
+		_avoidance_data["height"] = float(rvo_agent_3d.height_);
+		_avoidance_data["avoidance_layers"] = int(rvo_agent_3d.avoidance_layers_);
+		_avoidance_data["avoidance_mask"] = int(rvo_agent_3d.avoidance_mask_);
+		_avoidance_data["avoidance_priority"] = float(rvo_agent_3d.avoidance_priority_);
+	} else {
+		_avoidance_data["max_neighbors"] = int(rvo_agent_2d.maxNeighbors_);
+		_avoidance_data["max_speed"] = float(rvo_agent_2d.maxSpeed_);
+		_avoidance_data["neighbor_distance"] = float(rvo_agent_2d.neighborDist_);
+		_avoidance_data["new_velocity"] = Vector3(rvo_agent_2d.newVelocity_.x(), 0.0, rvo_agent_2d.newVelocity_.y());
+		_avoidance_data["velocity"] = Vector3(rvo_agent_2d.velocity_.x(), 0.0, rvo_agent_2d.velocity_.y());
+		_avoidance_data["position"] = Vector3(rvo_agent_2d.position_.x(), 0.0, rvo_agent_2d.position_.y());
+		_avoidance_data["preferred_velocity"] = Vector3(rvo_agent_2d.prefVelocity_.x(), 0.0, rvo_agent_2d.prefVelocity_.y());
+		_avoidance_data["radius"] = float(rvo_agent_2d.radius_);
+		_avoidance_data["time_horizon_agents"] = float(rvo_agent_2d.timeHorizon_);
+		_avoidance_data["time_horizon_obstacles"] = float(rvo_agent_2d.timeHorizonObst_);
+		_avoidance_data["height"] = float(rvo_agent_2d.height_);
+		_avoidance_data["avoidance_layers"] = int(rvo_agent_2d.avoidance_layers_);
+		_avoidance_data["avoidance_mask"] = int(rvo_agent_2d.avoidance_mask_);
+		_avoidance_data["avoidance_priority"] = float(rvo_agent_2d.avoidance_priority_);
+	}
+	return _avoidance_data;
+}
+
+void NavAgent3D::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 NavAgent3D::get_paused() const {
+	return paused;
+}
+
+void NavAgent3D::request_sync() {
+	if (map && !sync_dirty_request_list_element.in_list()) {
+		map->add_agent_sync_dirty_request(&sync_dirty_request_list_element);
+	}
+}
+
+void NavAgent3D::cancel_sync_request() {
+	if (map && sync_dirty_request_list_element.in_list()) {
+		map->remove_agent_sync_dirty_request(&sync_dirty_request_list_element);
+	}
+}
+
+NavAgent3D::NavAgent3D() :
+		sync_dirty_request_list_element(this) {
+}
+
+NavAgent3D::~NavAgent3D() {
+	cancel_sync_request();
+}

modules/navigation_3d/nav_agent_3d.h

@@ -0,0 +1,160 @@
+/**************************************************************************/
+/*  nav_agent_3d.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_3d.h"
+
+#include "core/object/class_db.h"
+#include "core/templates/self_list.h"
+#include "servers/navigation/navigation_globals.h"
+
+#include <Agent2d.h>
+#include <Agent3d.h>
+
+class NavMap3D;
+
+class NavAgent3D : public NavRid3D {
+	Vector3 position;
+	Vector3 target_position;
+	Vector3 velocity;
+	Vector3 velocity_forced;
+	real_t height = NavigationDefaults3D::AVOIDANCE_AGENT_HEIGHT;
+	real_t radius = NavigationDefaults3D::AVOIDANCE_AGENT_RADIUS;
+	real_t max_speed = NavigationDefaults3D::AVOIDANCE_AGENT_MAX_SPEED;
+	real_t time_horizon_agents = NavigationDefaults3D::AVOIDANCE_AGENT_TIME_HORIZON_AGENTS;
+	real_t time_horizon_obstacles = NavigationDefaults3D::AVOIDANCE_AGENT_TIME_HORIZON_OBSTACLES;
+	int max_neighbors = NavigationDefaults3D::AVOIDANCE_AGENT_MAX_NEIGHBORS;
+	real_t neighbor_distance = NavigationDefaults3D::AVOIDANCE_AGENT_NEIGHBOR_DISTANCE;
+	Vector3 safe_velocity;
+	bool clamp_speed = true; // Experimental, clamps velocity to max_speed.
+
+	NavMap3D *map = nullptr;
+
+	RVO2D::Agent2D rvo_agent_2d;
+	RVO3D::Agent3D rvo_agent_3d;
+	bool use_3d_avoidance = false;
+	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<NavAgent3D> sync_dirty_request_list_element;
+
+public:
+	NavAgent3D();
+	~NavAgent3D();
+
+	void set_avoidance_enabled(bool p_enabled);
+	bool is_avoidance_enabled() { return avoidance_enabled; }
+
+	void set_use_3d_avoidance(bool p_enabled);
+	bool get_use_3d_avoidance() { return use_3d_avoidance; }
+
+	void set_map(NavMap3D *p_map);
+	NavMap3D *get_map() { return map; }
+
+	bool is_map_changed();
+
+	RVO2D::Agent2D *get_rvo_agent_2d() { return &rvo_agent_2d; }
+	RVO3D::Agent3D *get_rvo_agent_3d() { return &rvo_agent_3d; }
+
+	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_height(real_t p_height);
+	real_t get_height() const { return height; }
+
+	void set_max_speed(real_t p_max_speed);
+	real_t get_max_speed() const { return max_speed; }
+
+	void set_position(const Vector3 p_position);
+	const Vector3 &get_position() const { return position; }
+
+	void set_target_position(const Vector3 p_target_position);
+	const Vector3 &get_target_position() const { return target_position; }
+
+	void set_velocity(const Vector3 p_velocity);
+	const Vector3 &get_velocity() const { return velocity; }
+
+	void set_velocity_forced(const Vector3 p_velocity);
+	const Vector3 &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_3d/nav_base_3d.h

@@ -0,0 +1,67 @@
+/**************************************************************************/
+/*  nav_base_3d.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_3d.h"
+#include "nav_utils_3d.h"
+
+#include "servers/navigation/navigation_utilities.h"
+
+class NavMap3D;
+
+class NavBase3D : public NavRid3D {
+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 ~NavBase3D() {}
+};

modules/navigation_3d/nav_link_3d.cpp

@@ -0,0 +1,214 @@
+/**************************************************************************/
+/*  nav_link_3d.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_3d.h"
+
+#include "nav_map_3d.h"
+
+void NavLink3D::set_map(NavMap3D *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 NavLink3D::set_enabled(bool p_enabled) {
+	if (enabled == p_enabled) {
+		return;
+	}
+	enabled = p_enabled;
+	iteration_dirty = true;
+
+	request_sync();
+}
+
+void NavLink3D::set_bidirectional(bool p_bidirectional) {
+	if (bidirectional == p_bidirectional) {
+		return;
+	}
+	bidirectional = p_bidirectional;
+	iteration_dirty = true;
+
+	request_sync();
+}
+
+void NavLink3D::set_start_position(const Vector3 p_position) {
+	if (start_position == p_position) {
+		return;
+	}
+	start_position = p_position;
+	iteration_dirty = true;
+
+	request_sync();
+}
+
+void NavLink3D::set_end_position(const Vector3 p_position) {
+	if (end_position == p_position) {
+		return;
+	}
+	end_position = p_position;
+	iteration_dirty = true;
+
+	request_sync();
+}
+
+void NavLink3D::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 NavLink3D::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 NavLink3D::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 NavLink3D::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 NavLink3D::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 NavLink3D::_build_iteration() {
+	if (!iteration_dirty || iteration_building || iteration_ready) {
+		return;
+	}
+
+	iteration_dirty = false;
+	iteration_building = true;
+	iteration_ready = false;
+
+	Ref<NavLinkIteration3D> 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<NavLinkIteration3D>();
+	DEV_ASSERT(iteration.is_null());
+	iteration = new_iteration;
+	iteration_id = iteration_id % UINT32_MAX + 1;
+
+	iteration_building = false;
+	iteration_ready = true;
+}
+
+void NavLink3D::request_sync() {
+	if (map && !sync_dirty_request_list_element.in_list()) {
+		map->add_link_sync_dirty_request(&sync_dirty_request_list_element);
+	}
+}
+
+void NavLink3D::cancel_sync_request() {
+	if (map && sync_dirty_request_list_element.in_list()) {
+		map->remove_link_sync_dirty_request(&sync_dirty_request_list_element);
+	}
+}
+
+Ref<NavLinkIteration3D> NavLink3D::get_iteration() {
+	RWLockRead read_lock(iteration_rwlock);
+	return iteration;
+}
+
+NavLink3D::NavLink3D() :
+		sync_dirty_request_list_element(this) {
+	type = NavigationUtilities::PathSegmentType::PATH_SEGMENT_TYPE_LINK;
+	iteration.instantiate();
+}
+
+NavLink3D::~NavLink3D() {
+	cancel_sync_request();
+
+	iteration = Ref<NavLinkIteration3D>();
+}

modules/navigation_3d/nav_link_3d.h

@@ -0,0 +1,118 @@
+/**************************************************************************/
+/*  nav_link_3d.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 "3d/nav_base_iteration_3d.h"
+#include "nav_base_3d.h"
+#include "nav_utils_3d.h"
+
+class NavLinkIteration3D : public NavBaseIteration3D {
+	GDCLASS(NavLinkIteration3D, NavBaseIteration3D);
+
+public:
+	bool bidirectional = true;
+	Vector3 start_position;
+	Vector3 end_position;
+
+	Vector3 get_start_position() const { return start_position; }
+	Vector3 get_end_position() const { return end_position; }
+	bool is_bidirectional() const { return bidirectional; }
+
+	virtual ~NavLinkIteration3D() override {
+		navmesh_polygons.clear();
+		internal_connections.clear();
+	}
+};
+
+#include "core/templates/self_list.h"
+
+class NavLink3D : public NavBase3D {
+	NavMap3D *map = nullptr;
+	bool bidirectional = true;
+	Vector3 start_position;
+	Vector3 end_position;
+	bool enabled = true;
+
+	SelfList<NavLink3D> sync_dirty_request_list_element;
+
+	uint32_t iteration_id = 0;
+
+	mutable RWLock iteration_rwlock;
+	Ref<NavLinkIteration3D> iteration;
+
+	bool iteration_dirty = true;
+	bool iteration_building = false;
+	bool iteration_ready = false;
+
+	void _build_iteration();
+	void _sync_iteration();
+
+public:
+	NavLink3D();
+	~NavLink3D();
+
+	uint32_t get_iteration_id() const { return iteration_id; }
+
+	void set_map(NavMap3D *p_map);
+	NavMap3D *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(Vector3 p_position);
+	Vector3 get_start_position() const {
+		return start_position;
+	}
+
+	void set_end_position(Vector3 p_position);
+	Vector3 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<NavLinkIteration3D> get_iteration();
+};

modules/navigation_3d/nav_map_3d.cpp

@@ -0,0 +1,899 @@
+/**************************************************************************/
+/*  nav_map_3d.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_3d.h"
+
+#include "3d/nav_map_builder_3d.h"
+#include "3d/nav_mesh_queries_3d.h"
+#include "3d/nav_region_iteration_3d.h"
+#include "nav_agent_3d.h"
+#include "nav_link_3d.h"
+#include "nav_obstacle_3d.h"
+#include "nav_region_3d.h"
+
+#include "core/config/project_settings.h"
+#include "core/object/worker_thread_pool.h"
+#include "servers/navigation_server_3d.h"
+
+#include <Obstacle2d.h>
+
+using namespace Nav3D;
+
+#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();                                        \
+	NavMapIteration3D &map_iteration = iteration_slots[iteration_slot_index]; \
+	NavMapIterationRead3D iteration_read_lock(map_iteration);                 \
+	iteration_slot_rwlock.read_unlock();
+
+#define GET_MAP_ITERATION_CONST()                                                   \
+	iteration_slot_rwlock.read_lock();                                              \
+	const NavMapIteration3D &map_iteration = iteration_slots[iteration_slot_index]; \
+	NavMapIterationRead3D iteration_read_lock(map_iteration);                       \
+	iteration_slot_rwlock.read_unlock();
+
+void NavMap3D::set_up(Vector3 p_up) {
+	if (up == p_up) {
+		return;
+	}
+	up = p_up;
+	map_settings_dirty = true;
+}
+
+void NavMap3D::set_cell_size(real_t p_cell_size) {
+	if (cell_size == p_cell_size) {
+		return;
+	}
+	cell_size = MAX(p_cell_size, NavigationDefaults3D::NAV_MESH_CELL_SIZE_MIN);
+	_update_merge_rasterizer_cell_dimensions();
+	map_settings_dirty = true;
+}
+
+void NavMap3D::set_cell_height(real_t p_cell_height) {
+	if (cell_height == p_cell_height) {
+		return;
+	}
+	cell_height = MAX(p_cell_height, NavigationDefaults3D::NAV_MESH_CELL_SIZE_MIN);
+	_update_merge_rasterizer_cell_dimensions();
+	map_settings_dirty = true;
+}
+
+void NavMap3D::set_merge_rasterizer_cell_scale(float p_value) {
+	if (merge_rasterizer_cell_scale == p_value) {
+		return;
+	}
+	merge_rasterizer_cell_scale = MAX(p_value, NavigationDefaults3D::NAV_MESH_CELL_SIZE_MIN);
+	_update_merge_rasterizer_cell_dimensions();
+	map_settings_dirty = true;
+}
+
+void NavMap3D::set_use_edge_connections(bool p_enabled) {
+	if (use_edge_connections == p_enabled) {
+		return;
+	}
+	use_edge_connections = p_enabled;
+	iteration_dirty = true;
+}
+
+void NavMap3D::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 NavMap3D::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 Vector3 &NavMap3D::get_merge_rasterizer_cell_size() const {
+	return merge_rasterizer_cell_size;
+}
+
+PointKey NavMap3D::get_point_key(const Vector3 &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));
+	const int z = static_cast<int>(Math::floor(p_pos.z / merge_rasterizer_cell_size.z));
+
+	PointKey p;
+	p.key = 0;
+	p.x = x;
+	p.y = y;
+	p.z = z;
+	return p;
+}
+
+void NavMap3D::query_path(NavMeshQueries3D::NavMeshPathQueryTask3D &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 (NavMeshQueries3D::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 NavMap3D path query slot found! This should never happen :(.");
+	}
+
+	p_query_task.map_up = map_iteration.map_up;
+
+	NavMeshQueries3D::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();
+}
+
+Vector3 NavMap3D::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const {
+	if (iteration_id == 0) {
+		NAVMAP_ITERATION_ZERO_ERROR_MSG();
+		return Vector3();
+	}
+
+	GET_MAP_ITERATION_CONST();
+
+	return NavMeshQueries3D::map_iteration_get_closest_point_to_segment(map_iteration, p_from, p_to, p_use_collision);
+}
+
+Vector3 NavMap3D::get_closest_point(const Vector3 &p_point) const {
+	if (iteration_id == 0) {
+		NAVMAP_ITERATION_ZERO_ERROR_MSG();
+		return Vector3();
+	}
+
+	GET_MAP_ITERATION_CONST();
+
+	return NavMeshQueries3D::map_iteration_get_closest_point(map_iteration, p_point);
+}
+
+Vector3 NavMap3D::get_closest_point_normal(const Vector3 &p_point) const {
+	if (iteration_id == 0) {
+		NAVMAP_ITERATION_ZERO_ERROR_MSG();
+		return Vector3();
+	}
+
+	GET_MAP_ITERATION_CONST();
+
+	return NavMeshQueries3D::map_iteration_get_closest_point_normal(map_iteration, p_point);
+}
+
+RID NavMap3D::get_closest_point_owner(const Vector3 &p_point) const {
+	if (iteration_id == 0) {
+		NAVMAP_ITERATION_ZERO_ERROR_MSG();
+		return RID();
+	}
+
+	GET_MAP_ITERATION_CONST();
+
+	return NavMeshQueries3D::map_iteration_get_closest_point_owner(map_iteration, p_point);
+}
+
+ClosestPointQueryResult NavMap3D::get_closest_point_info(const Vector3 &p_point) const {
+	GET_MAP_ITERATION_CONST();
+
+	return NavMeshQueries3D::map_iteration_get_closest_point_info(map_iteration, p_point);
+}
+
+void NavMap3D::add_region(NavRegion3D *p_region) {
+	DEV_ASSERT(!regions.has(p_region));
+
+	regions.push_back(p_region);
+	iteration_dirty = true;
+}
+
+void NavMap3D::remove_region(NavRegion3D *p_region) {
+	if (regions.erase_unordered(p_region)) {
+		iteration_dirty = true;
+	}
+}
+
+void NavMap3D::add_link(NavLink3D *p_link) {
+	DEV_ASSERT(!links.has(p_link));
+
+	links.push_back(p_link);
+	iteration_dirty = true;
+}
+
+void NavMap3D::remove_link(NavLink3D *p_link) {
+	if (links.erase_unordered(p_link)) {
+		iteration_dirty = true;
+	}
+}
+
+bool NavMap3D::has_agent(NavAgent3D *agent) const {
+	return agents.has(agent);
+}
+
+void NavMap3D::add_agent(NavAgent3D *agent) {
+	if (!has_agent(agent)) {
+		agents.push_back(agent);
+		agents_dirty = true;
+	}
+}
+
+void NavMap3D::remove_agent(NavAgent3D *agent) {
+	remove_agent_as_controlled(agent);
+	if (agents.erase_unordered(agent)) {
+		agents_dirty = true;
+	}
+}
+
+bool NavMap3D::has_obstacle(NavObstacle3D *obstacle) const {
+	return obstacles.has(obstacle);
+}
+
+void NavMap3D::add_obstacle(NavObstacle3D *obstacle) {
+	if (obstacle->get_paused()) {
+		// No point in adding a paused obstacle, it will add itself when unpaused again.
+		return;
+	}
+
+	if (!has_obstacle(obstacle)) {
+		obstacles.push_back(obstacle);
+		obstacles_dirty = true;
+	}
+}
+
+void NavMap3D::remove_obstacle(NavObstacle3D *obstacle) {
+	if (obstacles.erase_unordered(obstacle)) {
+		obstacles_dirty = true;
+	}
+}
+
+void NavMap3D::set_agent_as_controlled(NavAgent3D *agent) {
+	remove_agent_as_controlled(agent);
+
+	if (agent->get_paused()) {
+		// No point in adding a paused agent, it will add itself when unpaused again.
+		return;
+	}
+
+	if (agent->get_use_3d_avoidance()) {
+		int64_t agent_3d_index = active_3d_avoidance_agents.find(agent);
+		if (agent_3d_index < 0) {
+			active_3d_avoidance_agents.push_back(agent);
+			agents_dirty = true;
+		}
+	} else {
+		int64_t agent_2d_index = active_2d_avoidance_agents.find(agent);
+		if (agent_2d_index < 0) {
+			active_2d_avoidance_agents.push_back(agent);
+			agents_dirty = true;
+		}
+	}
+}
+
+void NavMap3D::remove_agent_as_controlled(NavAgent3D *agent) {
+	if (active_3d_avoidance_agents.erase_unordered(agent)) {
+		agents_dirty = true;
+	}
+	if (active_2d_avoidance_agents.erase_unordered(agent)) {
+		agents_dirty = true;
+	}
+}
+
+Vector3 NavMap3D::get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const {
+	GET_MAP_ITERATION_CONST();
+
+	return NavMeshQueries3D::map_iteration_get_random_point(map_iteration, p_navigation_layers, p_uniformly);
+}
+
+void NavMap3D::_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();
+	NavMapIteration3D &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 (NavRegion3D *region : regions) {
+		const Ref<NavRegionIteration3D> 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 (NavLink3D *link : links) {
+		const Ref<NavLinkIteration3D> link_iteration = link->get_iteration();
+		next_map_iteration.link_iterations[link_id_count++] = link_iteration;
+	}
+
+	next_map_iteration.map_up = get_up();
+
+	iteration_build.map_iteration = &next_map_iteration;
+
+	if (use_async_iterations) {
+		iteration_build_thread_task_id = WorkerThreadPool::get_singleton()->add_native_task(&NavMap3D::_build_iteration_threaded, &iteration_build, true, SNAME("NavMapBuilder3D"));
+	} else {
+		NavMapBuilder3D::build_navmap_iteration(iteration_build);
+
+		iteration_building = false;
+		iteration_ready = true;
+	}
+}
+
+void NavMap3D::_build_iteration_threaded(void *p_arg) {
+	NavMapIterationBuild3D *_iteration_build = static_cast<NavMapIterationBuild3D *>(p_arg);
+
+	NavMapBuilder3D::build_navmap_iteration(*_iteration_build);
+}
+
+void NavMap3D::_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 NavMap3D::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();
+
+		NavigationServer3D::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 (NavRegion3D *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 NavMap3D::_sync_avoidance() {
+	_sync_dirty_avoidance_update_requests();
+
+	if (obstacles_dirty || agents_dirty) {
+		_update_rvo_simulation();
+	}
+
+	obstacles_dirty = false;
+	agents_dirty = false;
+}
+
+void NavMap3D::_update_rvo_obstacles_tree_2d() {
+	int obstacle_vertex_count = 0;
+	for (NavObstacle3D *obstacle : obstacles) {
+		obstacle_vertex_count += obstacle->get_vertices().size();
+	}
+
+	// Cleaning old obstacles.
+	for (size_t i = 0; i < rvo_simulation_2d.obstacles_.size(); ++i) {
+		delete rvo_simulation_2d.obstacles_[i];
+	}
+	rvo_simulation_2d.obstacles_.clear();
+
+	// Cannot use LocalVector here as RVO library expects std::vector to build KdTree
+	std::vector<RVO2D::Obstacle2D *> &raw_obstacles = rvo_simulation_2d.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 (NavObstacle3D *obstacle : obstacles) {
+		if (!obstacle->is_avoidance_enabled()) {
+			continue;
+		}
+		const Vector3 &_obstacle_position = obstacle->get_position();
+		const Vector<Vector3> &_obstacle_vertices = obstacle->get_vertices();
+
+		if (_obstacle_vertices.size() < 2) {
+			continue;
+		}
+
+		std::vector<RVO2D::Vector2> rvo_2d_vertices;
+		rvo_2d_vertices.reserve(_obstacle_vertices.size());
+
+		uint32_t _obstacle_avoidance_layers = obstacle->get_avoidance_layers();
+		real_t _obstacle_height = obstacle->get_height();
+
+		for (const Vector3 &_obstacle_vertex : _obstacle_vertices) {
+#ifdef TOOLS_ENABLED
+			if (_obstacle_vertex.y != 0) {
+				WARN_PRINT_ONCE("Y coordinates of static obstacle vertices are ignored. Please use obstacle position Y to change elevation of obstacle.");
+			}
+#endif
+			rvo_2d_vertices.push_back(RVO2D::Vector2(_obstacle_vertex.x + _obstacle_position.x, _obstacle_vertex.z + _obstacle_position.z));
+		}
+
+		const size_t obstacleNo = raw_obstacles.size();
+
+		for (size_t i = 0; i < rvo_2d_vertices.size(); i++) {
+			RVO2D::Obstacle2D *rvo_2d_obstacle = new RVO2D::Obstacle2D();
+			rvo_2d_obstacle->point_ = rvo_2d_vertices[i];
+			rvo_2d_obstacle->height_ = _obstacle_height;
+			rvo_2d_obstacle->elevation_ = _obstacle_position.y;
+
+			rvo_2d_obstacle->avoidance_layers_ = _obstacle_avoidance_layers;
+
+			if (i != 0) {
+				rvo_2d_obstacle->prevObstacle_ = raw_obstacles.back();
+				rvo_2d_obstacle->prevObstacle_->nextObstacle_ = rvo_2d_obstacle;
+			}
+
+			if (i == rvo_2d_vertices.size() - 1) {
+				rvo_2d_obstacle->nextObstacle_ = raw_obstacles[obstacleNo];
+				rvo_2d_obstacle->nextObstacle_->prevObstacle_ = rvo_2d_obstacle;
+			}
+
+			rvo_2d_obstacle->unitDir_ = normalize(rvo_2d_vertices[(i == rvo_2d_vertices.size() - 1 ? 0 : i + 1)] - rvo_2d_vertices[i]);
+
+			if (rvo_2d_vertices.size() == 2) {
+				rvo_2d_obstacle->isConvex_ = true;
+			} else {
+				rvo_2d_obstacle->isConvex_ = (leftOf(rvo_2d_vertices[(i == 0 ? rvo_2d_vertices.size() - 1 : i - 1)], rvo_2d_vertices[i], rvo_2d_vertices[(i == rvo_2d_vertices.size() - 1 ? 0 : i + 1)]) >= 0.0f);
+			}
+
+			rvo_2d_obstacle->id_ = raw_obstacles.size();
+
+			raw_obstacles.push_back(rvo_2d_obstacle);
+		}
+	}
+
+	rvo_simulation_2d.kdTree_->buildObstacleTree(raw_obstacles);
+}
+
+void NavMap3D::_update_rvo_agents_tree_2d() {
+	// Cannot use LocalVector here as RVO library expects std::vector to build KdTree.
+	std::vector<RVO2D::Agent2D *> raw_agents;
+	raw_agents.reserve(active_2d_avoidance_agents.size());
+	for (NavAgent3D *agent : active_2d_avoidance_agents) {
+		raw_agents.push_back(agent->get_rvo_agent_2d());
+	}
+	rvo_simulation_2d.kdTree_->buildAgentTree(raw_agents);
+}
+
+void NavMap3D::_update_rvo_agents_tree_3d() {
+	// Cannot use LocalVector here as RVO library expects std::vector to build KdTree.
+	std::vector<RVO3D::Agent3D *> raw_agents;
+	raw_agents.reserve(active_3d_avoidance_agents.size());
+	for (NavAgent3D *agent : active_3d_avoidance_agents) {
+		raw_agents.push_back(agent->get_rvo_agent_3d());
+	}
+	rvo_simulation_3d.kdTree_->buildAgentTree(raw_agents);
+}
+
+void NavMap3D::_update_rvo_simulation() {
+	if (obstacles_dirty) {
+		_update_rvo_obstacles_tree_2d();
+	}
+	if (agents_dirty) {
+		_update_rvo_agents_tree_2d();
+		_update_rvo_agents_tree_3d();
+	}
+}
+
+void NavMap3D::compute_single_avoidance_step_2d(uint32_t index, NavAgent3D **agent) {
+	(*(agent + index))->get_rvo_agent_2d()->computeNeighbors(&rvo_simulation_2d);
+	(*(agent + index))->get_rvo_agent_2d()->computeNewVelocity(&rvo_simulation_2d);
+	(*(agent + index))->get_rvo_agent_2d()->update(&rvo_simulation_2d);
+	(*(agent + index))->update();
+}
+
+void NavMap3D::compute_single_avoidance_step_3d(uint32_t index, NavAgent3D **agent) {
+	(*(agent + index))->get_rvo_agent_3d()->computeNeighbors(&rvo_simulation_3d);
+	(*(agent + index))->get_rvo_agent_3d()->computeNewVelocity(&rvo_simulation_3d);
+	(*(agent + index))->get_rvo_agent_3d()->update(&rvo_simulation_3d);
+	(*(agent + index))->update();
+}
+
+void NavMap3D::step(double p_delta_time) {
+	rvo_simulation_2d.setTimeStep(float(p_delta_time));
+	rvo_simulation_3d.setTimeStep(float(p_delta_time));
+
+	if (active_2d_avoidance_agents.size() > 0) {
+		if (use_threads && avoidance_use_multiple_threads) {
+			WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &NavMap3D::compute_single_avoidance_step_2d, active_2d_avoidance_agents.ptr(), active_2d_avoidance_agents.size(), -1, true, SNAME("RVOAvoidanceAgents2D"));
+			WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task);
+		} else {
+			for (NavAgent3D *agent : active_2d_avoidance_agents) {
+				agent->get_rvo_agent_2d()->computeNeighbors(&rvo_simulation_2d);
+				agent->get_rvo_agent_2d()->computeNewVelocity(&rvo_simulation_2d);
+				agent->get_rvo_agent_2d()->update(&rvo_simulation_2d);
+				agent->update();
+			}
+		}
+	}
+
+	if (active_3d_avoidance_agents.size() > 0) {
+		if (use_threads && avoidance_use_multiple_threads) {
+			WorkerThreadPool::GroupID group_task = WorkerThreadPool::get_singleton()->add_template_group_task(this, &NavMap3D::compute_single_avoidance_step_3d, active_3d_avoidance_agents.ptr(), active_3d_avoidance_agents.size(), -1, true, SNAME("RVOAvoidanceAgents3D"));
+			WorkerThreadPool::get_singleton()->wait_for_group_task_completion(group_task);
+		} else {
+			for (NavAgent3D *agent : active_3d_avoidance_agents) {
+				agent->get_rvo_agent_3d()->computeNeighbors(&rvo_simulation_3d);
+				agent->get_rvo_agent_3d()->computeNewVelocity(&rvo_simulation_3d);
+				agent->get_rvo_agent_3d()->update(&rvo_simulation_3d);
+				agent->update();
+			}
+		}
+	}
+}
+
+void NavMap3D::dispatch_callbacks() {
+	for (NavAgent3D *agent : active_2d_avoidance_agents) {
+		agent->dispatch_avoidance_callback();
+	}
+
+	for (NavAgent3D *agent : active_3d_avoidance_agents) {
+		agent->dispatch_avoidance_callback();
+	}
+}
+
+void NavMap3D::_update_merge_rasterizer_cell_dimensions() {
+	merge_rasterizer_cell_size.x = cell_size * merge_rasterizer_cell_scale;
+	merge_rasterizer_cell_size.y = cell_height * merge_rasterizer_cell_scale;
+	merge_rasterizer_cell_size.z = cell_size * merge_rasterizer_cell_scale;
+}
+
+int NavMap3D::get_region_connections_count(NavRegion3D *p_region) const {
+	ERR_FAIL_NULL_V(p_region, 0);
+
+	GET_MAP_ITERATION_CONST();
+
+	HashMap<NavRegion3D *, Ref<NavRegionIteration3D>>::ConstIterator found_id = map_iteration.region_ptr_to_region_iteration.find(p_region);
+	if (found_id) {
+		HashMap<const NavBaseIteration3D *, 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;
+}
+
+Vector3 NavMap3D::get_region_connection_pathway_start(NavRegion3D *p_region, int p_connection_id) const {
+	ERR_FAIL_NULL_V(p_region, Vector3());
+
+	GET_MAP_ITERATION_CONST();
+
+	HashMap<NavRegion3D *, Ref<NavRegionIteration3D>>::ConstIterator found_id = map_iteration.region_ptr_to_region_iteration.find(p_region);
+	if (found_id) {
+		HashMap<const NavBaseIteration3D *, 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()), Vector3());
+			return found_connections->value[p_connection_id].pathway_start;
+		}
+	}
+
+	return Vector3();
+}
+
+Vector3 NavMap3D::get_region_connection_pathway_end(NavRegion3D *p_region, int p_connection_id) const {
+	ERR_FAIL_NULL_V(p_region, Vector3());
+
+	GET_MAP_ITERATION_CONST();
+
+	HashMap<NavRegion3D *, Ref<NavRegionIteration3D>>::ConstIterator found_id = map_iteration.region_ptr_to_region_iteration.find(p_region);
+	if (found_id) {
+		HashMap<const NavBaseIteration3D *, 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()), Vector3());
+			return found_connections->value[p_connection_id].pathway_end;
+		}
+	}
+
+	return Vector3();
+}
+
+void NavMap3D::add_region_sync_dirty_request(SelfList<NavRegion3D> *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 NavMap3D::add_link_sync_dirty_request(SelfList<NavLink3D> *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 NavMap3D::add_agent_sync_dirty_request(SelfList<NavAgent3D> *p_sync_request) {
+	if (p_sync_request->in_list()) {
+		return;
+	}
+	sync_dirty_requests.agents.list.add(p_sync_request);
+}
+
+void NavMap3D::add_obstacle_sync_dirty_request(SelfList<NavObstacle3D> *p_sync_request) {
+	if (p_sync_request->in_list()) {
+		return;
+	}
+	sync_dirty_requests.obstacles.list.add(p_sync_request);
+}
+
+void NavMap3D::remove_region_sync_dirty_request(SelfList<NavRegion3D> *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 NavMap3D::remove_link_sync_dirty_request(SelfList<NavLink3D> *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 NavMap3D::remove_agent_sync_dirty_request(SelfList<NavAgent3D> *p_sync_request) {
+	if (!p_sync_request->in_list()) {
+		return;
+	}
+	sync_dirty_requests.agents.list.remove(p_sync_request);
+}
+
+void NavMap3D::remove_obstacle_sync_dirty_request(SelfList<NavObstacle3D> *p_sync_request) {
+	if (!p_sync_request->in_list()) {
+		return;
+	}
+	sync_dirty_requests.obstacles.list.remove(p_sync_request);
+}
+
+void NavMap3D::_sync_dirty_map_update_requests() {
+	// If entire map settings changed make all regions dirty.
+	if (map_settings_dirty) {
+		for (NavRegion3D *region : regions) {
+			region->scratch_polygons();
+		}
+		iteration_dirty = true;
+	}
+
+	// Sync NavRegions.
+	RWLockWrite write_lock_regions(sync_dirty_requests.regions.rwlock);
+	for (SelfList<NavRegion3D> *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<NavLink3D> *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 NavMap3D::_sync_dirty_avoidance_update_requests() {
+	// Sync NavAgents.
+	if (!agents_dirty) {
+		agents_dirty = sync_dirty_requests.agents.list.first();
+	}
+	for (SelfList<NavAgent3D> *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<NavObstacle3D> *element = sync_dirty_requests.obstacles.list.first(); element; element = element->next()) {
+		element->self()->sync();
+	}
+	sync_dirty_requests.obstacles.list.clear();
+}
+
+void NavMap3D::add_region_async_thread_join_request(SelfList<NavRegion3D> *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 NavMap3D::remove_region_async_thread_join_request(SelfList<NavRegion3D> *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 NavMap3D::_sync_async_tasks() {
+	// Sync NavRegions that run async thread tasks.
+	RWLockWrite write_lock_regions(async_dirty_requests.regions.rwlock);
+	for (SelfList<NavRegion3D> *element = async_dirty_requests.regions.list.first(); element; element = element->next()) {
+		element->self()->sync_async_tasks();
+	}
+}
+
+void NavMap3D::set_use_async_iterations(bool p_enabled) {
+	if (use_async_iterations == p_enabled) {
+		return;
+	}
+#ifdef THREADS_ENABLED
+	use_async_iterations = p_enabled;
+#endif
+}
+
+bool NavMap3D::get_use_async_iterations() const {
+	return use_async_iterations;
+}
+
+NavMap3D::NavMap3D() {
+	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 (NavMapIteration3D &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
+}
+
+NavMap3D::~NavMap3D() {
+	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 (NavMapIteration3D &iteration_slot : iteration_slots) {
+		iteration_slot.clear();
+	}
+}

modules/navigation_3d/nav_map_3d.h

@@ -0,0 +1,290 @@
+/**************************************************************************/
+/*  nav_map_3d.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 "3d/nav_map_iteration_3d.h"
+#include "3d/nav_mesh_queries_3d.h"
+#include "nav_rid_3d.h"
+#include "nav_utils_3d.h"
+
+#include "core/math/math_defs.h"
+#include "core/object/worker_thread_pool.h"
+#include "servers/navigation/navigation_globals.h"
+
+#include <KdTree2d.h>
+#include <KdTree3d.h>
+#include <RVOSimulator2d.h>
+#include <RVOSimulator3d.h>
+
+class NavLink3D;
+class NavRegion3D;
+class NavAgent3D;
+class NavObstacle3D;
+
+class NavMap3D : public NavRid3D {
+	/// Map Up
+	Vector3 up = Vector3(0, 1, 0);
+
+	/// To find the polygons edges the vertices are displaced in a grid where
+	/// each cell has the following cell_size and cell_height.
+	real_t cell_size = NavigationDefaults3D::NAV_MESH_CELL_SIZE;
+	real_t cell_height = NavigationDefaults3D::NAV_MESH_CELL_HEIGHT;
+
+	// For the inter-region merging to work, internal rasterization is performed.
+	Vector3 merge_rasterizer_cell_size = Vector3(cell_size, cell_height, 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 = NavigationDefaults3D::EDGE_CONNECTION_MARGIN;
+
+	/// This value is used to limit how far links search to find polygons to connect to.
+	real_t link_connection_radius = NavigationDefaults3D::LINK_CONNECTION_RADIUS;
+
+	bool map_settings_dirty = true;
+
+	/// Map regions
+	LocalVector<NavRegion3D *> regions;
+
+	/// Map links
+	LocalVector<NavLink3D *> links;
+
+	/// RVO avoidance worlds
+	RVO2D::RVOSimulator2D rvo_simulation_2d;
+	RVO3D::RVOSimulator3D rvo_simulation_3d;
+
+	/// avoidance controlled agents
+	LocalVector<NavAgent3D *> active_2d_avoidance_agents;
+	LocalVector<NavAgent3D *> active_3d_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<NavAgent3D *> agents;
+
+	/// All the avoidance obstacles (both static and dynamic)
+	LocalVector<NavObstacle3D *> 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
+	Nav3D::PerformanceData performance_data;
+
+	struct {
+		struct {
+			RWLock rwlock;
+			SelfList<NavRegion3D>::List list;
+		} regions;
+		struct {
+			RWLock rwlock;
+			SelfList<NavLink3D>::List list;
+		} links;
+		struct {
+			RWLock rwlock;
+			SelfList<NavAgent3D>::List list;
+		} agents;
+		struct {
+			RWLock rwlock;
+			SelfList<NavObstacle3D>::List list;
+		} obstacles;
+	} sync_dirty_requests;
+
+	struct {
+		struct {
+			RWLock rwlock;
+			SelfList<NavRegion3D>::List list;
+		} regions;
+	} async_dirty_requests;
+
+	int path_query_slots_max = 4;
+
+	bool use_async_iterations = true;
+
+	uint32_t iteration_slot_index = 0;
+	LocalVector<NavMapIteration3D> iteration_slots;
+	mutable RWLock iteration_slot_rwlock;
+
+	NavMapIterationBuild3D 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:
+	NavMap3D();
+	~NavMap3D();
+
+	uint32_t get_iteration_id() const { return iteration_id; }
+
+	void set_up(Vector3 p_up);
+	Vector3 get_up() const {
+		return up;
+	}
+
+	void set_cell_size(real_t p_cell_size);
+	real_t get_cell_size() const {
+		return cell_size;
+	}
+
+	void set_cell_height(real_t p_cell_height);
+	real_t get_cell_height() const { return cell_height; }
+
+	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;
+	}
+
+	Nav3D::PointKey get_point_key(const Vector3 &p_pos) const;
+	const Vector3 &get_merge_rasterizer_cell_size() const;
+
+	void query_path(NavMeshQueries3D::NavMeshPathQueryTask3D &p_query_task);
+
+	Vector3 get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, const bool p_use_collision) const;
+	Vector3 get_closest_point(const Vector3 &p_point) const;
+	Vector3 get_closest_point_normal(const Vector3 &p_point) const;
+	Nav3D::ClosestPointQueryResult get_closest_point_info(const Vector3 &p_point) const;
+	RID get_closest_point_owner(const Vector3 &p_point) const;
+
+	void add_region(NavRegion3D *p_region);
+	void remove_region(NavRegion3D *p_region);
+	const LocalVector<NavRegion3D *> &get_regions() const {
+		return regions;
+	}
+
+	void add_link(NavLink3D *p_link);
+	void remove_link(NavLink3D *p_link);
+	const LocalVector<NavLink3D *> &get_links() const {
+		return links;
+	}
+
+	bool has_agent(NavAgent3D *agent) const;
+	void add_agent(NavAgent3D *agent);
+	void remove_agent(NavAgent3D *agent);
+	const LocalVector<NavAgent3D *> &get_agents() const {
+		return agents;
+	}
+
+	void set_agent_as_controlled(NavAgent3D *agent);
+	void remove_agent_as_controlled(NavAgent3D *agent);
+
+	bool has_obstacle(NavObstacle3D *obstacle) const;
+	void add_obstacle(NavObstacle3D *obstacle);
+	void remove_obstacle(NavObstacle3D *obstacle);
+	const LocalVector<NavObstacle3D *> &get_obstacles() const {
+		return obstacles;
+	}
+
+	Vector3 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(NavRegion3D *p_region) const;
+	Vector3 get_region_connection_pathway_start(NavRegion3D *p_region, int p_connection_id) const;
+	Vector3 get_region_connection_pathway_end(NavRegion3D *p_region, int p_connection_id) const;
+
+	void add_region_async_thread_join_request(SelfList<NavRegion3D> *p_async_request);
+	void remove_region_async_thread_join_request(SelfList<NavRegion3D> *p_async_request);
+
+	void add_region_sync_dirty_request(SelfList<NavRegion3D> *p_sync_request);
+	void add_link_sync_dirty_request(SelfList<NavLink3D> *p_sync_request);
+	void add_agent_sync_dirty_request(SelfList<NavAgent3D> *p_sync_request);
+	void add_obstacle_sync_dirty_request(SelfList<NavObstacle3D> *p_sync_request);
+
+	void remove_region_sync_dirty_request(SelfList<NavRegion3D> *p_sync_request);
+	void remove_link_sync_dirty_request(SelfList<NavLink3D> *p_sync_request);
+	void remove_agent_sync_dirty_request(SelfList<NavAgent3D> *p_sync_request);
+	void remove_obstacle_sync_dirty_request(SelfList<NavObstacle3D> *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 index, NavAgent3D **agent);
+
+	void compute_single_avoidance_step_2d(uint32_t index, NavAgent3D **agent);
+	void compute_single_avoidance_step_3d(uint32_t index, NavAgent3D **agent);
+
+	void _sync_avoidance();
+	void _update_rvo_simulation();
+	void _update_rvo_obstacles_tree_2d();
+	void _update_rvo_agents_tree_2d();
+	void _update_rvo_agents_tree_3d();
+
+	void _update_merge_rasterizer_cell_dimensions();
+};

modules/navigation_3d/nav_obstacle_3d.cpp

@@ -0,0 +1,254 @@
+/**************************************************************************/
+/*  nav_obstacle_3d.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_3d.h"
+
+#include "nav_agent_3d.h"
+#include "nav_map_3d.h"
+
+void NavObstacle3D::set_agent(NavAgent3D *p_agent) {
+	if (agent == p_agent) {
+		return;
+	}
+
+	agent = p_agent;
+
+	internal_update_agent();
+
+	request_sync();
+}
+
+void NavObstacle3D::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 NavObstacle3D::set_use_3d_avoidance(bool p_enabled) {
+	if (use_3d_avoidance == p_enabled) {
+		return;
+	}
+
+	use_3d_avoidance = p_enabled;
+	obstacle_dirty = true;
+
+	if (agent) {
+		agent->set_use_3d_avoidance(use_3d_avoidance);
+	}
+
+	request_sync();
+}
+
+void NavObstacle3D::set_map(NavMap3D *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 NavObstacle3D::set_position(const Vector3 p_position) {
+	if (position == p_position) {
+		return;
+	}
+
+	position = p_position;
+	obstacle_dirty = true;
+
+	if (agent) {
+		agent->set_position(position);
+	}
+
+	request_sync();
+}
+
+void NavObstacle3D::set_radius(real_t p_radius) {
+	if (radius == p_radius) {
+		return;
+	}
+
+	radius = p_radius;
+
+	if (agent) {
+		agent->set_radius(radius);
+	}
+}
+
+void NavObstacle3D::set_height(const real_t p_height) {
+	if (height == p_height) {
+		return;
+	}
+
+	height = p_height;
+	obstacle_dirty = true;
+
+	if (agent) {
+		agent->set_height(height);
+	}
+
+	request_sync();
+}
+
+void NavObstacle3D::set_velocity(const Vector3 p_velocity) {
+	velocity = p_velocity;
+
+	if (agent) {
+		agent->set_velocity(velocity);
+	}
+}
+
+void NavObstacle3D::set_vertices(const Vector<Vector3> &p_vertices) {
+	if (vertices == p_vertices) {
+		return;
+	}
+
+	vertices = p_vertices;
+	obstacle_dirty = true;
+
+	request_sync();
+}
+
+bool NavObstacle3D::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 NavObstacle3D::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 NavObstacle3D::is_dirty() const {
+	return obstacle_dirty;
+}
+
+void NavObstacle3D::sync() {
+	obstacle_dirty = false;
+}
+
+void NavObstacle3D::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_height(height);
+		agent->set_position(position);
+		agent->set_avoidance_layers(avoidance_layers);
+		agent->set_avoidance_enabled(avoidance_enabled);
+		agent->set_use_3d_avoidance(use_3d_avoidance);
+	}
+}
+
+void NavObstacle3D::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 NavObstacle3D::get_paused() const {
+	return paused;
+}
+
+void NavObstacle3D::request_sync() {
+	if (map && !sync_dirty_request_list_element.in_list()) {
+		map->add_obstacle_sync_dirty_request(&sync_dirty_request_list_element);
+	}
+}
+
+void NavObstacle3D::cancel_sync_request() {
+	if (map && sync_dirty_request_list_element.in_list()) {
+		map->remove_obstacle_sync_dirty_request(&sync_dirty_request_list_element);
+	}
+}
+
+NavObstacle3D::NavObstacle3D() :
+		sync_dirty_request_list_element(this) {
+}
+
+NavObstacle3D::~NavObstacle3D() {
+	cancel_sync_request();
+}

modules/navigation_3d/nav_obstacle_3d.h

@@ -0,0 +1,108 @@
+/**************************************************************************/
+/*  nav_obstacle_3d.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_3d.h"
+
+#include "core/object/class_db.h"
+#include "core/templates/self_list.h"
+
+class NavAgent3D;
+class NavMap3D;
+
+class NavObstacle3D : public NavRid3D {
+	NavAgent3D *agent = nullptr;
+	NavMap3D *map = nullptr;
+	Vector3 velocity;
+	Vector3 position;
+	Vector<Vector3> vertices;
+
+	real_t radius = 0.0;
+	real_t height = 0.0;
+
+	bool avoidance_enabled = false;
+	bool use_3d_avoidance = false;
+	uint32_t avoidance_layers = 1;
+
+	bool obstacle_dirty = true;
+
+	uint32_t last_map_iteration_id = 0;
+	bool paused = false;
+
+	SelfList<NavObstacle3D> sync_dirty_request_list_element;
+
+public:
+	NavObstacle3D();
+	~NavObstacle3D();
+
+	void set_avoidance_enabled(bool p_enabled);
+	bool is_avoidance_enabled() { return avoidance_enabled; }
+
+	void set_use_3d_avoidance(bool p_enabled);
+	bool get_use_3d_avoidance() { return use_3d_avoidance; }
+
+	void set_map(NavMap3D *p_map);
+	NavMap3D *get_map() { return map; }
+
+	void set_agent(NavAgent3D *p_agent);
+	NavAgent3D *get_agent() { return agent; }
+
+	void set_position(const Vector3 p_position);
+	const Vector3 &get_position() const { return position; }
+
+	void set_radius(real_t p_radius);
+	real_t get_radius() const { return radius; }
+
+	void set_height(const real_t p_height);
+	real_t get_height() const { return height; }
+
+	void set_velocity(const Vector3 p_velocity);
+	const Vector3 &get_velocity() const { return velocity; }
+
+	void set_vertices(const Vector<Vector3> &p_vertices);
+	const Vector<Vector3> &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_3d/nav_region_3d.cpp

@@ -0,0 +1,376 @@
+/**************************************************************************/
+/*  nav_region_3d.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_3d.h"
+
+#include "nav_map_3d.h"
+
+#include "3d/nav_mesh_queries_3d.h"
+#include "3d/nav_region_builder_3d.h"
+#include "3d/nav_region_iteration_3d.h"
+#include "core/config/project_settings.h"
+
+using namespace Nav3D;
+
+void NavRegion3D::set_map(NavMap3D *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 NavRegion3D::set_enabled(bool p_enabled) {
+	if (enabled == p_enabled) {
+		return;
+	}
+	enabled = p_enabled;
+	iteration_dirty = true;
+
+	request_sync();
+}
+
+void NavRegion3D::set_use_edge_connections(bool p_enabled) {
+	if (use_edge_connections != p_enabled) {
+		use_edge_connections = p_enabled;
+		iteration_dirty = true;
+	}
+
+	request_sync();
+}
+
+void NavRegion3D::set_transform(Transform3D p_transform) {
+	if (transform == p_transform) {
+		return;
+	}
+	transform = p_transform;
+	iteration_dirty = true;
+
+	request_sync();
+
+#ifdef DEBUG_ENABLED
+	if (map && Math::rad_to_deg(map->get_up().angle_to(transform.basis.get_column(1))) >= 90.0f) {
+		ERR_PRINT_ONCE("Attempted to update a navigation region transform rotated 90 degrees or more away from the current navigation map UP orientation.");
+	}
+#endif // DEBUG_ENABLED
+}
+
+void NavRegion3D::set_navigation_mesh(Ref<NavigationMesh> 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())));
+	}
+
+	if (map && p_navigation_mesh.is_valid() && !Math::is_equal_approx(double(map->get_cell_height()), double(p_navigation_mesh->get_cell_height()))) {
+		ERR_PRINT_ONCE(vformat("Attempted to update a navigation region with a navigation mesh that uses a `cell_height` of %s while assigned to a navigation map set to a `cell_height` of %s. The cell height for navigation maps can be changed by using the NavigationServer map_set_cell_height() function. The cell height for default navigation maps can also be changed in the ProjectSettings.", double(p_navigation_mesh->get_cell_height()), double(map->get_cell_height())));
+	}
+#endif // DEBUG_ENABLED
+
+	navmesh = p_navigation_mesh;
+
+	iteration_dirty = true;
+
+	request_sync();
+}
+
+Vector3 NavRegion3D::get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, bool p_use_collision) const {
+	RWLockRead read_lock(region_rwlock);
+
+	return NavMeshQueries3D::polygons_get_closest_point_to_segment(
+			get_polygons(), p_from, p_to, p_use_collision);
+}
+
+ClosestPointQueryResult NavRegion3D::get_closest_point_info(const Vector3 &p_point) const {
+	RWLockRead read_lock(region_rwlock);
+
+	return NavMeshQueries3D::polygons_get_closest_point_info(get_polygons(), p_point);
+}
+
+Vector3 NavRegion3D::get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const {
+	RWLockRead read_lock(region_rwlock);
+
+	if (!get_enabled()) {
+		return Vector3();
+	}
+
+	return NavMeshQueries3D::polygons_get_random_point(get_polygons(), p_navigation_layers, p_uniformly);
+}
+
+void NavRegion3D::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 NavRegion3D::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 NavRegion3D::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 NavRegion3D::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 NavRegion3D::scratch_polygons() {
+	iteration_dirty = true;
+
+	request_sync();
+}
+
+real_t NavRegion3D::get_surface_area() const {
+	RWLockRead read_lock(iteration_rwlock);
+	return iteration->get_surface_area();
+}
+
+AABB NavRegion3D::get_bounds() const {
+	RWLockRead read_lock(iteration_rwlock);
+	return iteration->get_bounds();
+}
+
+LocalVector<Nav3D::Polygon> const &NavRegion3D::get_polygons() const {
+	RWLockRead read_lock(iteration_rwlock);
+	return iteration->get_navmesh_polygons();
+}
+
+bool NavRegion3D::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 NavRegion3D::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 NavRegion3D::_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<NavRegionIteration3D> 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(&NavRegion3D::_build_iteration_threaded, &iteration_build, true, SNAME("NavRegionBuilder3D"));
+		request_async_thread_join();
+	} else {
+		NavRegionBuilder3D::build_iteration(iteration_build);
+
+		iteration_building = false;
+		iteration_ready = true;
+	}
+}
+
+void NavRegion3D::_build_iteration_threaded(void *p_arg) {
+	NavRegionIterationBuild3D *_iteration_build = static_cast<NavRegionIterationBuild3D *>(p_arg);
+
+	NavRegionBuilder3D::build_iteration(*_iteration_build);
+}
+
+void NavRegion3D::_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<NavRegionIteration3D>();
+	DEV_ASSERT(iteration.is_null());
+	iteration = iteration_build.region_iteration;
+	iteration_build.region_iteration = Ref<NavRegionIteration3D>();
+	DEV_ASSERT(iteration_build.region_iteration.is_null());
+	iteration_id = iteration_id % UINT32_MAX + 1;
+
+	iteration_ready = false;
+
+	cancel_async_thread_join();
+}
+
+Ref<NavRegionIteration3D> NavRegion3D::get_iteration() {
+	RWLockRead read_lock(iteration_rwlock);
+	return iteration;
+}
+
+void NavRegion3D::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 NavRegion3D::cancel_async_thread_join() {
+	if (map && async_list_element.in_list()) {
+		map->remove_region_async_thread_join_request(&async_list_element);
+	}
+}
+
+void NavRegion3D::request_sync() {
+	if (map && !sync_dirty_request_list_element.in_list()) {
+		map->add_region_sync_dirty_request(&sync_dirty_request_list_element);
+	}
+}
+
+void NavRegion3D::cancel_sync_request() {
+	if (map && sync_dirty_request_list_element.in_list()) {
+		map->remove_region_sync_dirty_request(&sync_dirty_request_list_element);
+	}
+}
+
+void NavRegion3D::set_use_async_iterations(bool p_enabled) {
+	if (use_async_iterations == p_enabled) {
+		return;
+	}
+#ifdef THREADS_ENABLED
+	use_async_iterations = p_enabled;
+#endif
+}
+
+bool NavRegion3D::get_use_async_iterations() const {
+	return use_async_iterations;
+}
+
+NavRegion3D::NavRegion3D() :
+		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
+}
+
+NavRegion3D::~NavRegion3D() {
+	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<NavRegionIteration3D>();
+	iteration = Ref<NavRegionIteration3D>();
+}

modules/navigation_3d/nav_region_3d.h

@@ -0,0 +1,134 @@
+/**************************************************************************/
+/*  nav_region_3d.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_3d.h"
+#include "nav_utils_3d.h"
+
+#include "core/os/rw_lock.h"
+#include "scene/resources/navigation_mesh.h"
+
+#include "3d/nav_region_iteration_3d.h"
+
+class NavRegion3D : public NavBase3D {
+	RWLock region_rwlock;
+
+	NavMap3D *map = nullptr;
+	Transform3D transform;
+	bool enabled = true;
+
+	bool use_edge_connections = true;
+
+	AABB bounds;
+
+	Ref<NavigationMesh> navmesh;
+
+	Nav3D::PerformanceData performance_data;
+
+	uint32_t iteration_id = 0;
+
+	SelfList<NavRegion3D> sync_dirty_request_list_element;
+	mutable RWLock iteration_rwlock;
+	Ref<NavRegionIteration3D> iteration;
+
+	NavRegionIterationBuild3D iteration_build;
+	bool use_async_iterations = true;
+	SelfList<NavRegion3D> 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:
+	NavRegion3D();
+	~NavRegion3D();
+
+	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(NavMap3D *p_map);
+	NavMap3D *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(Transform3D transform);
+	const Transform3D &get_transform() const {
+		return transform;
+	}
+
+	void set_navigation_mesh(Ref<NavigationMesh> p_navigation_mesh);
+	Ref<NavigationMesh> get_navigation_mesh() const { return navmesh; }
+
+	LocalVector<Nav3D::Polygon> const &get_polygons() const;
+
+	Vector3 get_closest_point_to_segment(const Vector3 &p_from, const Vector3 &p_to, bool p_use_collision) const;
+	Nav3D::ClosestPointQueryResult get_closest_point_info(const Vector3 &p_point) const;
+	Vector3 get_random_point(uint32_t p_navigation_layers, bool p_uniformly) const;
+
+	real_t get_surface_area() const;
+	AABB 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<NavRegionIteration3D> 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_3d/nav_rid_3d.h

@@ -0,0 +1,41 @@
+/**************************************************************************/
+/*  nav_rid_3d.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 NavRid3D {
+	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_3d/nav_utils_3d.h

@@ -0,0 +1,209 @@
+/**************************************************************************/
+/*  nav_utils_3d.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 NavBaseIteration3D;
+
+namespace Nav3D {
+struct Polygon;
+
+union PointKey {
+	struct {
+		int64_t x : 21;
+		int64_t y : 22;
+		int64_t z : 21;
+	};
+
+	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;
+	Vector3 pathway_start;
+	Vector3 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.
+	Vector3 pathway_start;
+
+	/// Point on the edge where the gateway leading to the poly ends.
+	Vector3 pathway_end;
+};
+
+struct Polygon {
+	uint32_t id = UINT32_MAX;
+
+	/// Navigation region or link that contains this polygon.
+	const NavBaseIteration3D *owner = nullptr;
+
+	LocalVector<Vector3> 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;
+	Vector3 back_navigation_edge_pathway_start;
+	Vector3 back_navigation_edge_pathway_end;
+
+	/// The entry position of this poly.
+	Vector3 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 {
+	Vector3 point;
+	Vector3 normal;
+	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 Nav3D

modules/navigation_3d/register_types.cpp

@@ -0,0 +1,86 @@
+/**************************************************************************/
+/*  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 "3d/godot_navigation_server_3d.h"
+
+#ifndef DISABLE_DEPRECATED
+#include "3d/navigation_mesh_generator.h"
+#endif // DISABLE_DEPRECATED
+
+#ifdef TOOLS_ENABLED
+#include "editor/navigation_link_3d_editor_plugin.h"
+#include "editor/navigation_obstacle_3d_editor_plugin.h"
+#include "editor/navigation_region_3d_editor_plugin.h"
+#endif
+
+#include "core/config/engine.h"
+#include "servers/navigation_server_3d.h"
+
+#ifndef DISABLE_DEPRECATED
+NavigationMeshGenerator *_nav_mesh_generator = nullptr;
+#endif // DISABLE_DEPRECATED
+
+NavigationServer3D *new_navigation_server_3d() {
+	return memnew(GodotNavigationServer3D);
+}
+
+void initialize_navigation_3d_module(ModuleInitializationLevel p_level) {
+	if (p_level == MODULE_INITIALIZATION_LEVEL_SERVERS) {
+		NavigationServer3DManager::set_default_server(new_navigation_server_3d);
+
+#ifndef DISABLE_DEPRECATED
+		_nav_mesh_generator = memnew(NavigationMeshGenerator);
+		GDREGISTER_CLASS(NavigationMeshGenerator);
+		Engine::get_singleton()->add_singleton(Engine::Singleton("NavigationMeshGenerator", NavigationMeshGenerator::get_singleton()));
+#endif // DISABLE_DEPRECATED
+	}
+
+#ifdef TOOLS_ENABLED
+	if (p_level == MODULE_INITIALIZATION_LEVEL_EDITOR) {
+		EditorPlugins::add_by_type<NavigationLink3DEditorPlugin>();
+		EditorPlugins::add_by_type<NavigationRegion3DEditorPlugin>();
+		EditorPlugins::add_by_type<NavigationObstacle3DEditorPlugin>();
+	}
+#endif
+}
+
+void uninitialize_navigation_3d_module(ModuleInitializationLevel p_level) {
+	if (p_level != MODULE_INITIALIZATION_LEVEL_SERVERS) {
+		return;
+	}
+
+#ifndef DISABLE_DEPRECATED
+	if (_nav_mesh_generator) {
+		memdelete(_nav_mesh_generator);
+	}
+#endif // DISABLE_DEPRECATED
+}

modules/navigation_3d/register_types.h

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